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, [EMAIL PROTECTED]
<[EMAIL PROTECTED]> 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
> Playerstage-gazebo@lists.sourceforge.net
> 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
Playerstage-gazebo@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
