[Playerstage-users] Two_robot_communicating

[<bor...@gm...>]

I am interest how to make program thet will connect two robot in stage.
Last days I have test some programs and unfortunatly they don't work.
This are the problems thet I have:

I need to send data, like velocity, position of second robot2 to robot1.

This is my code:

primjer.cfg:

driver
(
  name "stage"
  provides ["simulation:0"]
  plugin "libstageplugin"
  worldfile "clear.world"
)
# robot 0
driver(
 name "stage"
 provides ["6665:position2d:0" "6665:laser:0" "6665:graphics2d:0"]
 requires ["6665:opaque:0"]
 model "crveni"
)
# robot 3
driver(
 name "stage"
 provides ["6668:position2d:0" "6668:laser:0" "6668:graphics2d:0"]
 requires ["6668:opaque:0"]
 model "zeleni"
)
# robot 4
driver(
 name "stage"
 provides ["6669:position2d:0" "6669:laser:0" "6669:graphics2d:0"]
 model "cijan"
)
driver
(
  name "stage"
  provides ["6665:map:0" "6668:map:0"]
  model "prazna"
)

driver
(
name "relay"
provides ["6665:opaque:0"]
)

driver
(
name "relay"
provides ["6668:opaque:0"]
)



#driver
#(
#name "relay"
#provides ["opaque:0"]
#)

---------------------------------------------------------------------------------------------------

cc for the robot1:
#include "moj.h"
#include <time.h>
#define RAY 32
#include <libplayerc++/playerc++.h>
#include <iostream>
#include "math.h"
//#include<libgnomecanvas>
#include <libplayerc/playerc.h>
#include<sys/types.h>
#include<stdio.h>
#include<sys/mman.h>
#include<fcntl.h>
//#include <pomocni.cc>
//#include "pomocni.cc "
#include "args.h"   //parser
    moj *M;   //pointer na klasu da se mogu pozivati podaci iz drugih cpp
datoteka
//#include "varijable.h"

int main(int argc, char **argv)
{
     parse_args(argc,argv);
    using namespace PlayerCc;

    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    Graphics2dProxy gp(&robot, gIndex);
    OpaqueProxy op(&robot, gIndex);
    player_color_t col;//boja

    player_opaque_data_t opData;
     uint8_t opGetData[PLAYER_OPAQUE_MAX_SIZE];

    PlayerClient robot1(gHostname, 6668);
    Position2dProxy pp1(&robot1, gIndex);
    LaserProxy lp1(&robot1, gIndex);
     Graphics2dProxy gp1(&robot1, gIndex);
     OpaqueProxy op1(&robot1, gIndex);
    //OpaqueProxy op1(&robot1, 0);

      player_opaque_data_t op1Data;
    uint8_t  op1GetData[PLAYER_OPAQUE_MAX_SIZE ];


    //player_point_2d_t pts[10];//malo polje tocaka x,y
    std::cout << robot << std::endl;
    int ciklus=0;
    FILE    *logfile;
    M = new moj();
    if ( (logfile = fopen("mirko","w")) == NULL )
        printf("Error! mirko file couldn't be opened.");
    else fprintf(logfile,"The mirko is the log file!\n");//primjer log fajla

    fclose(logfile);
        pp.SetMotorEnable (true);


    bool z1=0;
    bool z2=0;
        bool z3=0;
    bool z4=0;
    bool z5=0;
    bool z6=0;
    bool z7=0;
    bool z8=0;
    bool z9=0;
    bool z10=1;
    double pocetna_pozicija;
    const float Th = 5.5;

for(;;ciklus++){
    /* this blocks until new data comes; 10Hz by default */
        robot.Read();
//Andreja, preskoci jedan ciklus jer po dva imaju jednaka ocitanja
        gp.Clear();//cisti ekran za crtanje
        //ucitavanje laserskih mjerenja
        int count1 = lp.GetCount();


moj *p= new moj;
    double newspeed = 0;
    double newturnrate = 0;


const double pi = 3.141592654;
// this blocks until new data comes; 10Hz by default




uint count = op.GetCount();
std::cout << "count=" << count << std::endl;
op.GetData(opGetData);
std::cout << "opGetData[0]=" << opGetData[0] <<
std::endl;
std::cout << "opGetData[1]=" <<opGetData[1] <<
std::endl;
std::cout << "opGetData[2]=" <<opGetData[2] <<
std::endl;
std::cout << "opGetData[3]=" <<opGetData[3] <<
std::endl;
std::cout << "pp1=" <<pp1.GetXSpeed() <<
std::endl;



        M-> setvel.v=0.2;//primjer
        M->setvel.w=0;
        pp.SetSpeed( M->setvel.v, M->setvel.w );


//printf("v1x: %.3f\n",v1x);
//printf("v1y: %.3f\n",v1y);


/*
       std::cout << "v1x: " << v1x
            << "v1y: " << v1y
                 << std::endl;
*/
      // write commands to robot
      //pp.SetSpeed(newspeed, newturnrate);


    /*
     * Locate the segment.
     */
/*
int oflag = O_RDONLY;
    mode_t mode;
    char *s;

    mode = 5678;



   if (( shm_open(*s,oflag, 5678)) == -1) {
        perror("shmat");
        exit(1);
    }

  */
/*
dvx = vx-v1x;

dvy = vy -v1y;
*/

for (int j=0; j<count1; j++) {


            M-> LB.point[j].x=lp.GetPoint(j).px;  //u metrima
            M->LB.point[j].y=lp.GetPoint(j).py;  //lokalne koordinate
            M->LB.scan[j].r=lp.GetRange(j);  //polarne koordinate laserskih
ocitanja
            M-> LB.scan[j].th=atan2(M->LB.point[j].y,M->LB.point[j].x);//svakih
0.008727 radijana je zraka lasera od -pi/2 do pi/2 lokalno
//printf("M->LB.x:%.3f\tM->Lb.y:%.3f\tM->LB.scan[j].r:%.3f\tM->LB.scan[j].th:%.3f\n",M->
LB.point[j].x,M->LB.point[j].y,M->LB.scan[j].r,M->LB.scan[j].th);

float globalnax=p->Lokalna_u_globalnu_x(M->LB.scan[j].r,M->LB.scan[j].th,
M->LB.point[j].x,M->LB.point[j].y);
float globalnay=p->Lokalna_u_globalnu_y(M-> LB.scan[j].r,M->LB.scan[j].th,
M->LB.point[j].x,M->LB.point[j].y);
float globalnar = globalnax*globalnax + globalnay*globalnay;
globalnar= sqrt(globalnar);
//printf("globalnax:%.3f\n globalnay:%.3f\n",globalnax,globalnay);
//printf("globalnar:%.3f\n", globalnar);
//printf("polarne.r.koordinate:%.3f\n", M->LB.scan[j].r);
//printf("polarne.th.koordinate:%.3f\n",M->LB.scan[j].th);

}

//moj::~moj();



//printf("M->RB.x:%.3f\tM->RB.y:%.3f\tM-LB.scan_count:%f\n",M->RB.x,M->RB.y
,M->LB.scan_count);



        M->LB.scan_count=count1;
        //ucitavanje odometrije
        M->RB.x=pp.GetXPos();
        M->RB.y=pp.GetYPos();
        M->RB.th=pp.GetYaw();  //radijani
        M->RB.v=pp.GetXSpeed(); //translacijska brzina
        M->RB.w=pp.GetYawSpeed();//rotacijska
        //postavljanje startne pozicije
         //printf("polarne.th.koordinate:%.3f\n",M->RB.th);
if (ciklus==0){
            M->start.x=M->RB.x;
            M->start.y=M->RB.y;
            pocetna_pozicija=M->start.th=M->RB.th;
            printf("M->start.x:%.3f\tM->start.y:%.3f\n",M->start.x,M->
start.y);
        }
        //crtanje starta - romb polusirine 0.1m
        gp.Color( 0,0,255,0 ); //plava linija
        col.green = 255;  //zeleno punjenje
        M->PoljeZaCrtanjeRealneTockeRomb(M->start.x,M->start.y,0.1);//0.1m
sirine romba
        gp.DrawPolygon ( M->pts, 4, 1, col);  //polje, 4 tocke, 1=punjenje,
col je boja



//printf("ciklus:%d\n",ciklus);
}
  gp.Clear();
};
moj::moj() {

    LB.scan_count=0;
}

 //prebacivanje iz lokalnih u globalne koordinate. Rx i Rth su dio pozicije
robota; tockaX i tockaY su lokalne koordinate tocke koju prebacujemo u
globalnu x koordinatu
float moj::Lokalna_u_globalnu_x(float Rx, float Rth, float tockaX, float
tockaY){
    float globalnaX;
    globalnaX=Rx+cos(Rth)*tockaX-sin(Rth)*tockaY;
    return globalnaX;
}

//prebacivanje iz lokalnih u globalne koordinate. Ry i Rth su dio pozicije
robota; tockaX i tockaY su lokalne koordinate tocke koju prebacujemo u
globalnu y koordinatu
float moj::Lokalna_u_globalnu_y(float Ry, float Rth, float tockaX, float
tockaY){
    float globalnaY;
    globalnaY=Ry+sin(Rth)*tockaX+cos(Rth)*tockaY;
    return globalnaY;
}

//prebacivanje iz globalnih u lokalne koordinate. Rx, Ry i Rth je pozicija
robota; tockaX i tockaY su globalne koordinate tocke koju prebacujemo u
lokalnu x koordinatu
float moj::Globalna_u_lokalnu_x(float Rx, float Ry, float Rth, float tockaX,
float tockaY){
    float lokalnaX;
    lokalnaX=cos(Rth)*(tockaX-Rx)+sin(Rth)*(tockaY-Ry);
    return lokalnaX;
}

//prebacivanje iz globalnih u lokalne koordinate. Rx, Ry i Rth je pozicija
robota; tockaX i tockaY su globalne koordinate tocke koju prebacujemo u
lokalnu y koordinatu
float moj::Globalna_u_lokalnu_y(float Rx, float Ry, float Rth, float tockaX,
float tockaY){
    float lokalnaY;
    lokalnaY=-sin(Rth)*(tockaX-Rx)+cos(Rth)*(tockaY-Ry);
    return lokalnaY;
}

void moj::PoljeZaCrtanjeRealneTockeRomb(double tx, double ty, double a) {
    double x,y;
    for(int i=0;i<4;i++)
    {
        x=tx+a*cos(i*M_PI/2);
        y=ty+a*sin(i*M_PI/2);
        pts[i].px=Globalna_u_lokalnu_x(RB.x, RB.y, RB.th, x, y);
        pts[i].py=Globalna_u_lokalnu_y( RB.x, RB.y, RB.th, x, y);

//printf("M->startlokalno.x:%.3f\tM->startlokalno.y:%.3f\n
",pts[i].px,pts[i].py);
 }
}
 ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

cc for the robot2:

#include "moj.h"
#include <time.h>
#define RAY 32
#include <libplayerc++/playerc++.h>
#include <iostream>
#include "math.h"
#include " args.h"
//#include<libgnomecanvas>
#include<sys/types.h>
#include <libplayerc/playerc.h>

#include<sys/ipc.h>
#include<sys/shm.h>
#include<stdio.h>

//#include " varijable.h"
 //parser

#define SHMSZ 27
float v1x;
float v1y;
    moj *G;   //pointer na klasu da se mogu pozivati podaci iz drugih cpp
datoteka


int main(int argc, char **argv)
{

    float v1;


    parse_args(argc,argv);
    using namespace PlayerCc;

    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);
    Graphics2dProxy gp(&robot, gIndex);
    OpaqueProxy op(&robot, gIndex);
    player_color_t col;//boja
    //player_point_2d_t pts[10];//malo polje tocaka x,y

    //PlayerClient robot1(gHostname, 6665);

     PlayerClient robot1(gHostname, 6668);
    Position2dProxy pp1(&robot1, gIndex);
    LaserProxy lp1(&robot1, gIndex);
     Graphics2dProxy gp1(&robot1, gIndex);
     //OpaqueProxy op1(&zeleni, gIndex);*/
    OpaqueProxy op1(&robot1, 0);
      player_opaque_data_t opData;
        uint8_t op1GetData[PLAYER_OPAQUE_MAX_SIZE];

std::cout << robot1 << std::endl;

    int ciklus=0;
    FILE    *logfile;
    G = new moj();
    if ( (logfile = fopen("mirko","w")) == NULL )
        printf("Error! mirko file couldn't be opened.");
    else fprintf(logfile,"The mirko is the log file!\n");//primjer log fajla

    fclose(logfile);
        pp.SetMotorEnable (true);


for(;;ciklus++){
    /* this blocks until new data comes; 10Hz by default */
        robot1.Read();
//Andreja, preskoci jedan ciklus jer po dva imaju jednaka ocitanja
        gp.Clear();//cisti ekran za crtanje
        //ucitavanje laserskih mjerenja
        int count1 = lp.GetCount();


moj *po= new moj;




const double pi = 3.141592654;
// this blocks until new data comes; 10Hz by default
      robot1.Read();

      // laser avoid (stolen from esben's java example)
     // int count = lp.GetCount();


        v1 = G-> setvel.v=0.2;//primjer
        G->setvel.w=0;
        pp.SetSpeed( G->setvel.v, G->setvel.w );

//v1x=v1*(G->RB.th);
v1x = v1*cos(G->RB.th);

v1y = v1*sin(G->RB.th);

//write();

  //player_opaque_data_t opData;
opData.data_count = 3;
opData.data[0]='a';
opData.data[1]='b';
opData.data[2]='c';
op1.SendCmd(&opData);




/*if(z4==1){
        M->setvel.v=0;//primjer
        M->setvel.w=1;
        pp.SetSpeed( M->setvel.v, M->setvel.w );
z4=0;
};*/


      // write commands to robot
      //pp.SetSpeed(newspeed, newturnrate);

    /*
opData.data_count = 3;
opData.data[0]=1;
opData.data[1]='b';
opData.data[2]='c';
op.SendCmd(&opData);
*/
 std::cout << "pp=" <<pp.GetXSpeed() <<
std::endl;

 std::cout << "" <<pp.GetXSpeed() <<
std::endl;

for (int j=0; j<count1; j++) {


            G->LB.point[j].x=lp.GetPoint(j).px;  //u metrima
            G->LB.point[j].y=lp.GetPoint(j).py;  //lokalne koordinate
            G->LB.scan[j].r=lp.GetRange(j);  //polarne koordinate laserskih
ocitanja
            G->LB.scan[j].th=atan2(G->LB.point [j].y,G->LB.point[j].x);//svakih
0.008727 radijana je zraka lasera od -pi/2 do pi/2 lokalno
//printf("M->LB.x:%.3f\tM->Lb.y:%.3f\tM->LB.scan[j].r:%.3f\tM->LB.scan
[j].th:%.3f\n",M->LB.point[j].x,M-> LB.point[j].y,M->LB.scan[j].r,M->LB.scan
[j].th);

float globalnax=po->Lokalna_u_globalnu_x(G->LB.scan[j].r,G->LB.scan[j].th,
G->LB.point[j].x,G->LB.point[j].y);
float globalnay=po->Lokalna_u_globalnu_y(G-> LB.scan[j].r,G->LB.scan[j].th,
G->LB.point[j].x,G->LB.point[j].y);
float globalnar = globalnax*globalnax + globalnay*globalnay;
globalnar= sqrt(globalnar);
//printf("globalnax:%.3f\tglobalnay:%.3f\n",globalnax,globalnay);
//printf("globalnar:%.3f\n", globalnar);
//printf("polarne.r.koordinate:%.3f\n", M->LB.scan[j].r);
//printf("polarne.th.koordinate:%.3f\n",M->LB.scan[j].th);

}

//moj::~moj();




//printf("M->RB.x:%.3f\tM->RB.y:%.3f\tM-LB.scan_count:%f\n",M->RB.x,M->RB.y
,M->LB.scan_count);

        G->LB.scan_count=count1;
        //ucitavanje odometrije
        G->RB.x=pp.GetXPos();
        G->RB.y=pp.GetYPos();
        G->RB.th=pp.GetYaw();  //radijani
        G->RB.v=pp.GetXSpeed(); //translacijska brzina
        G->RB.w=pp.GetYawSpeed();//rotacijska
        //postavljanje startne pozicije
         printf("polarne.th.koordinate:%.3f\n",G->RB.th);


printf("v1x: %.3f\n", v1x);
printf("v1y: %.3f\n", v1y);
/*
char *s;
int oflag;
mode_t mode;

oflag=O_RDWR;

mode=5678;

if((shm_open(*s,oflag,mode))==-1){
perror("greska");
exit(1);
}
*/

/*
popen();

pclose(FILE *stream);
*/



if (ciklus==0){
            G->start.x=G->RB.x;
            G->start.y=G->RB.y;
            G->start.th=G->RB.th;
            printf("M->start.x:%.3f\tM->start.y:%.3f\n ",G->start.x,G->
start.y);
        }
        //crtanje starta - romb polusirine 0.1m
        //gp.Color( 0,0,255,0 ); //plava linija
        //col.green = 255;  //zeleno punjenje
        //G->PoljeZaCrtanjeRealneTockeRomb(G-> start.x,G->start.y,0.1);//0.1m
sirine romba
        //gp.DrawPolygon( G->pts, 4, 1, col);  //polje, 4 tocke, 1=punjenje,
col je boja




//printf("ciklus:%d\n",ciklus);

}

gp.Clear();
};
moj::moj() {

    //LB.scan_count=0;
}