Simulation Interface GetPose2d and GetPose3d issues

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|

Simulation Interface GetPose2d and GetPose3d issues

sandytf@alumni.usc.edu
I think I may have found an issue with the Simulation Interface while using Player 2.1.1 and Gazebo revision 6982.  I was able to set the position of a Pioneer2DX with both SetPose2d and SetPose3d but neither of the equivalent get functions would operate correctly.  The GetPose2d function resulted in a error message being displayed in the gazebo terminal about an unknown request type.  GetPose3d simply crashed player and my test application.  After looking at the gazebo source, I managed to make a few changes to get the functions to work correctly.  I have included a patch of my changes.  The patch also includes the addition of a #include <string.h> to one file for an unrelated reason.  I am using GCC 4.3.1 so standard header files need to be added to nearly every application to get them to compile.  I have also included my world file, player cfg file, and my minimal test application.

Sandy

Patch File:
Index: player/SimulationInterface.cc
===================================================================
--- player/SimulationInterface.cc (revision 6982)
+++ player/SimulationInterface.cc (working copy)
@@ -162,7 +162,7 @@
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_SIMULATION_REQ_GET_POSE2D, this->device_addr))
   {
-    gazebo::SimulationRequestData *gzReq = NULL;
+    gazebo::SimulationRequestData * gzReq = NULL;
     player_simulation_pose2d_req_t *req =
       (player_simulation_pose2d_req_t*)(data);
 
@@ -175,6 +175,7 @@
     strcpy((char*)gzReq->modelName, req->name);
     this->iface->Unlock();
+    
   }
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_SIMULATION_REQ_GET_PROPERTY,
@@ -250,8 +251,8 @@
         {
           player_simulation_pose3d_req_t req;
 
-          strcpy(req.name, response->modelName);
-          req.name_count = strlen(req.name);
+          //strcpy(req.name, response->modelName);
+          req.name_count = 0;//strlen(req.n ame);
 
           req.pose.px = response->modelPose.pos.x;
           req.pose.py = response->modelPose.pos.y;
@@ -271,8 +272,8 @@
         {
           player_simulation_pose2d_req_t req;
 
-          strcpy(req.name, response->modelName);
-          req.name_count = strlen(req.name);
+          //strcpy(req.name, response->modelName);
+          req.name_count = 0;//strlen(req.name) + 1;
 
           req.pose.px = response->modelPose.pos.x;
         &n bsp; req.pose.py = response->modelPose.pos.y;
Index: server/gui/StatusBar.cc
===================================================================
--- server/gui/StatusBar.cc (revision 6982)
+++ server/gui/StatusBar.cc (working copy)
@@ -25,6 +25,7 @@
  */
 
 #include <stdio.h>
+#include <string.h>
 #include <FL/Fl_Value_Output.H>
 #include <FL/Fl_Output.H>
 #include <FL/Fl_Button.H>
Index: server/World.cc
===================================================================
--- server/World.cc (revision 6982)
+++ server/World.cc (working copy)
@@ -517,8 +517,6 @@
 // Update the simulation interface
 void World::UpdateSimulationIface()
 {
-  SimulationRequestData *response = NULL;
-
   //TODO: Move this method to simulator? Hard because of the models
   this->simIface->Lock(1);
 < BR>@@ -528,7 +526,7 @@
     return;
   }
 
-  response = this->simIface->data->responses;
+  //response = this->simIface->data->responses;
 
   this->simIface->data->simTime = Simulator::Instance()->GetSimTime();
   this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime();
@@ -549,6 +547,7 @@
   for (unsigned int i=0; i < requestCount; i++)
   {
     SimulationRequestData *req = &(this->simIface->data->requests[i]);
+    SimulationRequestData *response = &(this->simIface->data->responses[this->simIface->data->responseCount]);
 
     switch (req->type)
     {
@@ -608,7 +607,7 @@
             response->mod elPose.pitch = rot.y;
             response->modelPose.yaw = rot.z;
 
-            response++;
+            //response++;
             this->simIface->data->responseCount += 1;
           }
           else
@@ -641,14 +640,42 @@
           break;
         }
 
+      case SimulationRequestData::GET_POSE2D:
+ {
+          Model *model = this->GetModelByName((char*)req->modelName);
+       &nbs p;  if (model)
+          {
+            Pose3d pose = model->GetPose();
+     Vector3 rot = pose.rot.GetAsEuler();  
+
+     response->type = req->type;
+    
+     strcpy(response->modelName, req->modelName);
+            response->modelPose.pos.x = pose.pos.x;
+            response->modelPose.pos.y = pose.pos.y;    
+            response->modelPose.pos.z = pose.pos.z;
+
+            response->modelPose.roll = rot.x;
+            respons e->modelPose.pitch = rot.y;
+            response->modelPose.yaw = rot.z;
+
+     //response++;
+     this->simIface->data->responseCount += 1;
+   }
+          else
+          {
+            gzerr(0) << "Invalid model name[" << req->modelName << "] in simulation interface Get Pose 2d Request.\n";
+          }
+          break;
+        }
+
       default:
         gzerr(0) << "Unknown simulation iface request[" << req->type << "]\n";
 &nbs p;       break;
     }
-
-    this->simIface->data->requestCount = 0;
   }
 
+  this->simIface->data->requestCount = 0;
   this->simIface->Unlock();
 }

Test Application:
#include <stdio.h>
#include <libplayerc++/playerc++.h>
#include <assert.h>
int main(){
PlayerCc::PlayerClient playerClient("localhost");
PlayerCc::Position2dProxy position(&playerClient, 0);
PlayerCc::SimulationProxy simulation(&playerClient, 0);
#if(1)
double x, y, a;
while(1){
playerClient.Read();
printf("Enter new pose 2D (x, y, a):\n");
scanf("%lf %lf %lf", &x, &y, &a);
simulation.SetPose2d((char*)"pioneer2dx_model1", x, y, a);
simulation.GetPose2d((char*)"pioneer2dx_model1", x, y, a);
printf("New position is (%lf, %lf, %lf)\n", x, y, a);
}
#else
double x, y, z, roll, pitch, yaw, time;
while(1){
playerClient.Read();
printf("Enter new pose 3D (x y z roll pitch yaw):\n");
scanf("%lf %lf %lf %lf %lf %lf", &x, &y, &z, &roll, &pitch, &yaw);
simulation.SetPose3d((char*)"pioneer2dx_model1", x, y, z, roll, pitch, yaw);
simulation.GetPose3d((char*)"pioneer2dx_model1", x, y, z, roll, pitch, yaw, time);
printf("New position is (%lf, %lf, %lf, %lf, %lf, %lf) at time: %lf\n", x, y, z, roll, pitch, yaw, time);
}
#endif
}
World File:
<?xml version="1.0"?>
<gazebo:world
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
<verbosity>5</verbosity>
<physics:ode>
<stepTime>0.03</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>10e-5</cfm>
<erp>0.3</erp>
</physics:ode>
<rendering:gui>
<type>fltk</type>
<size>800 600</size>
<pos>0 0</pos>
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
</rendering:gui>
<rendering:ogre>
<ambient>0.5 0.5 0.5 1.0</ambient>
</rendering:ogre>
<!-- Ground Plane -->
<model:physical name="plane1_model">
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
<static>true</static>
<body:plane name="plane1_body">
<geom:plane name="plane1_geom">
<normal>0 0 1</normal>
<size>2000 2000</size>
<segments>10 10</segments>
<uvTile>100 100</uvTile>
<material>Gazebo/Grey</material>
</geom:plane>
</body:plane>
</model:physical>
<!--
Include the complete model described in the .model file
This assumes the root node is a <model:...>
-->
<!-- <include embedded="false">
<xi:include href="pioneer2dx.model" />
</include>
-->
<model:physical name="pioneer2dx_model1">
<xyz>0 0 0.145</xyz>
<rpy>0.0 0.0 0.0</rpy>
<controller:differential_position2d name="controller1">
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
<wheelSeparation>0.39</wheelSeparation>
<wheelDiameter>0.15</wheelDiameter>
<torque>5</torque>
<interface:position name="position_iface_0"/>
</controller:differential_position2d>
<include embedded="true">
<xi:include href="models/pioneer2dx.model" />
</include>
</model:physical>
<!-- White Directional light -->
<model:renderable name="directional_white">
<light>
<type>directional</type>
<direction>0 -0.6 -0.4</direction>
<diffuseColor>1.0 1.0 1.0</diffuseColor>
<specularColor>0.2 0.2 0.2</specularColor>
<attenuation>1000 1.0 0.0 0</attenuation>
</light>
</model:renderable>
</gazebo:world>

Player cfg file:
driver
(
name "gazebo"
provides ["simulation:0"]
plugin "/home/freedmst/local/gazebo/lib/libgazeboplugin"
server_id "default"
)
driver
(
name "gazebo"
provides ["position2d:0"]
gz_id "position_iface_0"
)
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-gazebo mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
Reply | Threaded
Open this post in threaded view
|

Re: Simulation Interface GetPose2d and GetPose3d issues

Nate Koenig-4
Hello Sandy,

Thanks for the patch. The latest SVN version of Gazebo should be fixed.

-nate

On Thu, Aug 28, 2008 at 12:59 PM, [hidden email]
<[hidden email]> wrote:

> I think I may have found an issue with the Simulation Interface while using Player 2.1.1 and Gazebo revision 6982.  I was able to set the position of a Pioneer2DX with both SetPose2d and SetPose3d but neither of the equivalent get functions would operate correctly.  The GetPose2d function resulted in a error message being displayed in the gazebo terminal about an unknown request type.  GetPose3d simply crashed player and my test application.  After looking at the gazebo source, I managed to make a few changes to get the functions to work correctly.  I have included a patch of my changes.  The patch also includes the addition of a #include <string.h> to one file for an unrelated reason.  I am using GCC 4.3.1 so standard header files need to be added to nearly every application to get them to compile.  I have also included my world file, player cfg file, and my minimal test application.
>
> Sandy
>
> Patch File:
> Index: player/SimulationInterface.cc
> ===================================================================
> --- player/SimulationInterface.cc (revision 6982)
> +++ player/SimulationInterface.cc (working copy)
> @@ -162,7 +162,7 @@
>   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
>                                  PLAYER_SIMULATION_REQ_GET_POSE2D, this->device_addr))
>   {
> -    gazebo::SimulationRequestData *gzReq = NULL;
> +    gazebo::SimulationRequestData * gzReq = NULL;
>     player_simulation_pose2d_req_t *req =
>       (player_simulation_pose2d_req_t*)(data);
>
> @@ -175,6 +175,7 @@
>     strcpy((char*)gzReq->modelName, req->name);
>     this->iface->Unlock();
> +
>   }
>   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
>                                  PLAYER_SIMULATION_REQ_GET_PROPERTY,
> @@ -250,8 +251,8 @@
>         {
>           player_simulation_pose3d_req_t req;
>
> -          strcpy(req.name, response->modelName);
> -          req.name_count = strlen(req.name);
> +          //strcpy(req.name, response->modelName);
> +          req.name_count = 0;//strlen(req.n ame);
>
>           req.pose.px = response->modelPose.pos.x;
>           req.pose.py = response->modelPose.pos.y;
> @@ -271,8 +272,8 @@
>         {
>           player_simulation_pose2d_req_t req;
>
> -          strcpy(req.name, response->modelName);
> -          req.name_count = strlen(req.name);
> +          //strcpy(req.name, response->modelName);
> +          req.name_count = 0;//strlen(req.name) + 1;
>
>           req.pose.px = response->modelPose.pos.x;
>         &n bsp; req.pose.py = response->modelPose.pos.y;
> Index: server/gui/StatusBar.cc
> ===================================================================
> --- server/gui/StatusBar.cc (revision 6982)
> +++ server/gui/StatusBar.cc (working copy)
> @@ -25,6 +25,7 @@
>  */
>
>  #include <stdio.h>
> +#include <string.h>
>  #include <FL/Fl_Value_Output.H>
>  #include <FL/Fl_Output.H>
>  #include <FL/Fl_Button.H>
> Index: server/World.cc
> ===================================================================
> --- server/World.cc (revision 6982)
> +++ server/World.cc (working copy)
> @@ -517,8 +517,6 @@
>  // Update the simulation interface
>  void World::UpdateSimulationIface()
>  {
> -  SimulationRequestData *response = NULL;
> -
>   //TODO: Move this method to simulator? Hard because of the models
>   this->simIface->Lock(1);
>  < BR>@@ -528,7 +526,7 @@
>     return;
>   }
>
> -  response = this->simIface->data->responses;
> +  //response = this->simIface->data->responses;
>
>   this->simIface->data->simTime = Simulator::Instance()->GetSimTime();
>   this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime();
> @@ -549,6 +547,7 @@
>   for (unsigned int i=0; i < requestCount; i++)
>   {
>     SimulationRequestData *req = &(this->simIface->data->requests[i]);
> +    SimulationRequestData *response = &(this->simIface->data->responses[this->simIface->data->responseCount]);
>
>     switch (req->type)
>     {
> @@ -608,7 +607,7 @@
>             response->mod elPose.pitch = rot.y;
>             response->modelPose.yaw = rot.z;
>
> -            response++;
> +            //response++;
>             this->simIface->data->responseCount += 1;
>           }
>           else
> @@ -641,14 +640,42 @@
>           break;
>         }
>
> +      case SimulationRequestData::GET_POSE2D:
> + {
> +          Model *model = this->GetModelByName((char*)req->modelName);
> +       &nbs p;  if (model)
> +          {
> +            Pose3d pose = model->GetPose();
> +     Vector3 rot = pose.rot.GetAsEuler();
> +
> +     response->type = req->type;
> +
> +     strcpy(response->modelName, req->modelName);
> +            response->modelPose.pos.x = pose.pos.x;
> +            response->modelPose.pos.y = pose.pos.y;
> +            response->modelPose.pos.z = pose.pos.z;
> +
> +            response->modelPose.roll = rot.x;
> +            respons e->modelPose.pitch = rot.y;
> +            response->modelPose.yaw = rot.z;
> +
> +     //response++;
> +     this->simIface->data->responseCount += 1;
> +   }
> +          else
> +          {
> +            gzerr(0) << "Invalid model name[" << req->modelName << "] in simulation interface Get Pose 2d Request.\n";
> +          }
> +          break;
> +        }
> +
>       default:
>         gzerr(0) << "Unknown simulation iface request[" << req->type << "]\n";
>  &nbs p;       break;
>     }
> -
> -    this->simIface->data->requestCount = 0;
>   }
>
> +  this->simIface->data->requestCount = 0;
>   this->simIface->Unlock();
>  }
>
> Test Application:
> #include <stdio.h>
> #include <libplayerc++/playerc++.h>
> #include <assert.h>
> int main(){
> PlayerCc::PlayerClient playerClient("localhost");
> PlayerCc::Position2dProxy position(&playerClient, 0);
> PlayerCc::SimulationProxy simulation(&playerClient, 0);
> #if(1)
> double x, y, a;
> while(1){
> playerClient.Read();
> printf("Enter new pose 2D (x, y, a):\n");
> scanf("%lf %lf %lf", &x, &y, &a);
> simulation.SetPose2d((char*)"pioneer2dx_model1", x, y, a);
> simulation.GetPose2d((char*)"pioneer2dx_model1", x, y, a);
> printf("New position is (%lf, %lf, %lf)\n", x, y, a);
> }
> #else
> double x, y, z, roll, pitch, yaw, time;
> while(1){
> playerClient.Read();
> printf("Enter new pose 3D (x y z roll pitch yaw):\n");
> scanf("%lf %lf %lf %lf %lf %lf", &x, &y, &z, &roll, &pitch, &yaw);
> simulation.SetPose3d((char*)"pioneer2dx_model1", x, y, z, roll, pitch, yaw);
> simulation.GetPose3d((char*)"pioneer2dx_model1", x, y, z, roll, pitch, yaw, time);
> printf("New position is (%lf, %lf, %lf, %lf, %lf, %lf) at time: %lf\n", x, y, z, roll, pitch, yaw, time);
> }
> #endif
> }
> World File:
> <?xml version="1.0"?>
> <gazebo:world
> xmlns:xi="http://www.w3.org/2001/XInclude"
> xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
> xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
> xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
> xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
> xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
> xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
> xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
> xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
> xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
> xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
> xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
> xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
> xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
> <verbosity>5</verbosity>
> <physics:ode>
> <stepTime>0.03</stepTime>
> <gravity>0 0 -9.8</gravity>
> <cfm>10e-5</cfm>
> <erp>0.3</erp>
> </physics:ode>
> <rendering:gui>
> <type>fltk</type>
> <size>800 600</size>
> <pos>0 0</pos>
> <xyz>0 0 0</xyz>
> <rpy>0 0 0</rpy>
> </rendering:gui>
> <rendering:ogre>
> <ambient>0.5 0.5 0.5 1.0</ambient>
> </rendering:ogre>
> <!-- Ground Plane -->
> <model:physical name="plane1_model">
> <xyz>0 0 0</xyz>
> <rpy>0 0 0</rpy>
> <static>true</static>
> <body:plane name="plane1_body">
> <geom:plane name="plane1_geom">
> <normal>0 0 1</normal>
> <size>2000 2000</size>
> <segments>10 10</segments>
> <uvTile>100 100</uvTile>
> <material>Gazebo/Grey</material>
> </geom:plane>
> </body:plane>
> </model:physical>
> <!--
> Include the complete model described in the .model file
> This assumes the root node is a <model:...>
> -->
> <!-- <include embedded="false">
> <xi:include href="pioneer2dx.model" />
> </include>
> -->
> <model:physical name="pioneer2dx_model1">
> <xyz>0 0 0.145</xyz>
> <rpy>0.0 0.0 0.0</rpy>
> <controller:differential_position2d name="controller1">
> <leftJoint>left_wheel_hinge</leftJoint>
> <rightJoint>right_wheel_hinge</rightJoint>
> <wheelSeparation>0.39</wheelSeparation>
> <wheelDiameter>0.15</wheelDiameter>
> <torque>5</torque>
> <interface:position name="position_iface_0"/>
> </controller:differential_position2d>
> <include embedded="true">
> <xi:include href="models/pioneer2dx.model" />
> </include>
> </model:physical>
> <!-- White Directional light -->
> <model:renderable name="directional_white">
> <light>
> <type>directional</type>
> <direction>0 -0.6 -0.4</direction>
> <diffuseColor>1.0 1.0 1.0</diffuseColor>
> <specularColor>0.2 0.2 0.2</specularColor>
> <attenuation>1000 1.0 0.0 0</attenuation>
> </light>
> </model:renderable>
> </gazebo:world>
>
> Player cfg file:
> driver
> (
> name "gazebo"
> provides ["simulation:0"]
> plugin "/home/freedmst/local/gazebo/lib/libgazeboplugin"
> server_id "default"
> )
> driver
> (
> name "gazebo"
> provides ["position2d:0"]
> gz_id "position_iface_0"
> )
> -------------------------------------------------------------------------
> This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
> Build the coolest Linux based applications with Moblin SDK & win great prizes
> Grand prize is a trip for two to an Open Source event anywhere in the world
> http://moblin-contest.org/redirect.php?banner_id=100&url=/
> _______________________________________________
> Playerstage-gazebo mailing list
> [hidden email]
> https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
>

-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-gazebo mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo