Hi all,

A few days ago, I wrote asking about a way to make the way a robot follows be printed in the map. I got to make an approach, which is working at some points. Part of the code is shown below. SimulationProxy prints some points in the way and SimulationProxy allows the printing to take the correct direccion, making "centroid" look to the same direction, "robot" first takes. However, depending on the complexity of the trajectory the robot has to accomplish each time, the printing will be done right or will take a slight deflection. That means than the printed way will be a couple of milimeters above or below from the way that the robot is making. This depends on the moment SetPose2d is called, when the robot is spinning to take the correct direction from the starting point to the first goal.

How can I solve this?

Was anybody able to do the printing stuff correctly? I'm open to other suggestions or ideas.

Many thanks in advance!

Iņigo

PlayerClient robot(gHostname, gPort); 
PlayerClient centroid(gHostname, gPort);
Position2dProxy odom(&robot, 0);
SimulationProxy deviceProxy2(&centroid, 0);
PlannerProxy *planner;
Graphics2dProxy grafikoa(&robot, 0);

     first = 0;
     while(!planner->GetPathDone()){ 
              robot.Read();
              points[0].px= odom.GetXPos() - hasx;
              points[0].py= odom.GetYPos() - hasy;
              if(!first)
              {
                      deviceProxy2.SetPose2d ("centroid", hasx, hasy, DTOR(odom.GetYaw()));
                      first = 1;
              }
              grafikoa.DrawPoints(points, 1);
              usleep(100000);
      }