[Playerstage-users] Problem about playerc error queue overflow discarding packets

[Ruoyu T. <mik...@gm...>]

Hi Everyone,

I am a starter to use player/stage. Right now I am trying to use
player/stage control multiple robots. But from the simulation result, the
motion of robots don't meet my algorithm. That is, it seems that their
forwardSpeed and turnSpeed don't change which against my code.

Then I try to use gdb to set up break point to find something wrong for my
code. At the beginning, it is fine because I can find the change of their
forwardSpeed and turnSpeed.  Then "playerc error queue overflow discarding
packets" will occur. When I want to get the forwardSpeed and turnSpeed
again, it shows that "Program received signal SIGILL, Illegal Instruction".
It makes me fail to do the debug test in the next steps. So I assume that
the reason of no change of forwardSpeed and turnSpeed is caused by queue
overflow and I have no idea how to solve it.

The version of player is 2.1.2. The version of stage is 2.1.1. The OS is
ubuntu 10.0.4 in Oracle VM VirtuaBox Manager.

Please give me your help. If you need more information, please feel free to
contact me. Below is control code and driver code.

Control Code

#include <stdio.h>
#include <libplayerc++/playerc++.h>
#include <unistd.h>
#include <time.h>
#include <math.h>





using namespace PlayerCc;

double positionX[8],test1[8],test2[8];
double positionY[8];
double vx[8],vy[8];
double tempX,tempY;
int n;

void Wander(double *forwardSpeed, double *turnSpeed)
{

    double fspeed, tspeed;
    double x,y,r,theta1,theta2,directionX,directionY,temp;
    double positionXX[8],positionYY[8],fv,d0,z;
    int i;

    fv=5;d0=0.3;z=0.5;

    for(i=0;i<8;i++)
    {
      if(i==n) continue;
      x=positionX[i]-positionX[n];
      y=positionY[i]-positionY[n];

      //to solve length of distance
      r=sqrt(x*x+y*y);
      if (r==0) continue;
      //to get force for vehicle i to n
      temp=(z/r)-(z*d0/(r*r));

      //to get the direction unit vector of force
      directionX=x/r;
      directionY=y/r;

      //to get force in x vector and y vector seperately
     tempX=tempX+temp*directionX;
      tempY=tempY+temp*directionY;
     }

     //to get total force in x vector and y vector seperately
     tempX=tempX-vx[n]*fv;
     tempY=tempY-vy[n]*fv;

     //to get velocity at dt
     vx[n]=vx[n]+tempX*0.01;
     vy[n]=vy[n]+tempY*0.01;

     //to get position at dt
     positionXX[n]=positionX[n]+vx[n]*0.01;
     positionYY[n]=positionY[n]+vy[n]*0.01;

     //to get w and v at dt
     theta2=atan2(positionYY[n],positionXX[n]);
     theta1=atan2(positionY[n],positionX[n]);
     tspeed=(theta2-theta1)/0.01;
     fspeed=sqrt(vx[n]*vx[n]+vy[n]*vy[n]);

    *forwardSpeed=fspeed;
    *turnSpeed=tspeed;

     //initiate valriable
     positionX[n]=positionXX[n];
     positionY[n]=positionYY[n];
     positionXX[n]=0;tempX=0;
     positionYY[n]=0;tempY=0;
     test1[n]=fspeed;
     test2[n]=tspeed;
}


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



   PlayerClient robot("localhost",6665);

   Position2dProxy p2dProx1(&robot,0);
   Position2dProxy p2dProx2(&robot,1);
   Position2dProxy p2dProx3(&robot,2);
   Position2dProxy p2dProx4(&robot,3);
   Position2dProxy p2dProx5(&robot,4);
   Position2dProxy p2dProx6(&robot,5);
   Position2dProxy p2dProx7(&robot,6);
   Position2dProxy p2dProx8(&robot,7);
   SimulationProxy sim(&robot,0);



   double forwardSpeed, turnSpeed;

   srand(time(NULL));

   //enable motors
   p2dProx1.SetMotorEnable(1);

   //request geometries
   p2dProx1.RequestGeom();



   p2dProx2.SetMotorEnable(1);

   //request geometries
   p2dProx2.RequestGeom();

   p2dProx3.SetMotorEnable(1);

   //request geometries
   p2dProx3.RequestGeom();

    p2dProx4.SetMotorEnable(1);

   //request geometries
   p2dProx4.RequestGeom();

    p2dProx5.SetMotorEnable(1);

   //request geometries
   p2dProx5.RequestGeom();

   p2dProx6.SetMotorEnable(1);

   //request geometries
   p2dProx6.RequestGeom();

    p2dProx7.SetMotorEnable(1);

   //request geometries
   p2dProx7.RequestGeom();

    p2dProx8.SetMotorEnable(1);

   //request geometries
   p2dProx8.RequestGeom();


   positionX[0]=1;
   positionY[0]=0;

    positionX[1]=0;
   positionY[1]=1;


    positionX[2]=-1;
   positionY[2]=0;


     positionX[3]=0;
   positionY[3]=-1;


    positionX[4]=0;
   positionY[4]=0;


    positionX[5]=1.5;
   positionY[5]=-1;



    positionX[6]=-1;
   positionY[6]=2.5;


    positionX[7]=-1;
   positionY[7]=-1;
   n=0;


   while(true)
   {
     //read from the proxies
     robot.Read();


    //get position




     //robot1

     n=0;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx1.SetSpeed(forwardSpeed,dtor(turnSpeed));

     n=1;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx2.SetSpeed(forwardSpeed,dtor(turnSpeed));

    n=2;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx3.SetSpeed(forwardSpeed,dtor(turnSpeed));

     n=3;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx4.SetSpeed(forwardSpeed,dtor(turnSpeed));

     n=4;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx5.SetSpeed(forwardSpeed,dtor(turnSpeed));

     n=5;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx6.SetSpeed(forwardSpeed,dtor(turnSpeed));

    n=6;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx7.SetSpeed(forwardSpeed,dtor(turnSpeed));

    n=7;Wander(&forwardSpeed,&turnSpeed);
     //set motors
     p2dProx8.SetSpeed(forwardSpeed,dtor(turnSpeed));





     sleep(1);
    }


   return 0;
}


Driver Code

driver
(

   name "stage"
   plugin "libstageplugin"

   provides ["simulation:0"]
   worldfile "empty1.world"
)


driver
(
  name "stage"
  provides ["map:0" ]
  model "cave"
)

driver
(
   name "stage"
   provides ["6665:position2d:0"]
   model "robot1"
)

driver
(
   name "stage"
   provides ["6665:position2d:1"]
   model "robot2"
)

driver
(
   name "stage"
   provides ["6665:position2d:2"]
   model "robot3"
)

driver
(
   name "stage"
   provides ["6665:position2d:3"]
   model "robot4"
   alwayson 1
)

driver
(
   name "stage"
   provides ["6665:position2d:4"]
   model "robot5"
)

driver
(
   name "stage"
   provides ["6665:position2d:5"]
   model "robot6"
)

driver
(
   name "stage"
   provides ["6665:position2d:6"]
   model "robot7"
)

driver
(
   name "stage"
   provides ["6665:position2d:7"]
   model "robot8"
)


-- 
Regards,
Ruoyu Tan