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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
