Hello all,
Sorry to repost but the issue is not on an old build but on 0.10.0 so i
figured the title may be confusing the issue.
I am seeing an issue in which I cant set the basic pioneer robot speed with
a really simple world model using libgazebo (no Player). The gazebo
simulator starts up and simulation time is running, using my program I can
connect and see that the connection is fine because I tested reading the
pioneers robot velocity using the pioneer2dx.world file.
I am using a simple world model with only a plane and the pioneer2dx model
that comes with Gazebo. I am using Ubuntu 9.10, ode 0.11.1, ogre v1-7-0,
bullet-2.76, gazebo 0.10.0.
The code is now a simple state machine that tries to make the robot do a
square pattern. I include the code, program output, world and model files.
Thanks for any help or ideas... (any hints are much appreciated)
Tomas
REF: Code, code output, world model, pioneer model
Here is the code:
#include <gazebo/gazebo.h>
#include <stdio.h>
int main()
{
gazebo::Client *client = new gazebo::Client();
gazebo::SimulationIface *simIface = new gazebo::SimulationIface();
gazebo::PositionIface *posIface = new gazebo::PositionIface();
int serverId = 0;
int cmdCnt = 0;
/// Connect to the libgazebo server
try
{
client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
return -1;
}
/// Open the Simulation Interface
try
{
simIface->Open(client, "default");
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect to the sim interface\n" <<
e << "\n";
return -1;
}
/// Open the Position interface
try
{
posIface->Open(client, "pioneer2dx_model1::position_iface_0");
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect to the position
interface\n" << e << "\n";
return -1;
}
posIface->Lock(1);
posIface->data->cmdEnableMotors = 1;
posIface->Unlock();
while(1)
{
posIface->Lock(1);
std::cout << "Cmd Velocity (x,y,z)" << posIface->data->cmdVelocity.pos.x
<< " " << posIface->data->cmdVelocity.pos.y << " " <<
posIface->data->cmdVelocity.pos.z << std::endl;
posIface->Unlock();
posIface->Lock(1);
std::cout << "Velocity (x,y,z):" << posIface->data->velocity.pos.x << "
" << posIface->data->velocity.pos.y << " " <<
posIface->data->velocity.pos.z << std::endl;
posIface->Unlock();
posIface->Lock(1);
std::cout << "Stall flag:" << posIface->data->stall << std::endl;
posIface->Unlock();
posIface->Lock(1);
std::cout << "Roll,pitch,yaw:" << posIface->data->pose.roll << " " <<
posIface->data->pose.pitch << " " << posIface->data->pose.yaw << std::endl;
posIface->Unlock();
switch(cmdCnt)
{
case 0:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.x = 1;
posIface->Unlock();
cmdCnt++;
break;
case 1:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.x = 0;
posIface->Unlock();
cmdCnt++;
break;
case 2:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.y = 1;
posIface->Unlock();
cmdCnt++;
break;
case 3:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.y = 0;
posIface->Unlock();
cmdCnt++;
break;
case 4:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.x = -1;
posIface->Unlock();
cmdCnt++;
break;
case 5:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.x = 0;
posIface->Unlock();
cmdCnt++;
break;
case 6:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.y = -1;
posIface->Unlock();
cmdCnt++;
break;
case 7:
posIface->Lock(1);
posIface->data->cmdVelocity.pos.y = 0;
posIface->Unlock();
cmdCnt++;
break;
default:
cmdCnt=0;
break;
}
usleep(3000000);
}
posIface->Close();
delete posIface;
return 0;
}
Here is the output from the code:
Cmd Velocity (x,y,z)+0.00000 +0.00000 +0.00000
Velocity (x,y,z):+0.00000 +0.00000 +0.00000
Stall flag:+0
Roll,pitch,yaw:+0.00000 +0.00000 +0.00000
Cmd Velocity (x,y,z)+1.00000 +0.00000 +0.00000
Velocity (x,y,z):+0.00000 +0.00000 +0.00000
Stall flag:+0
Roll,pitch,yaw:+0.00000 +0.00000 +0.00000
Cmd Velocity (x,y,z)+0.00000 +0.00000 +0.00000
Velocity (x,y,z):+0.00000 +0.00000 +0.00000
Stall flag:+0
Roll,pitch,yaw:+0.00000 +0.00000 +0.00000
Cmd Velocity (x,y,z)+0.00000 +1.00000 +0.00000
Velocity (x,y,z):+0.00000 +0.00000 +0.00000
Stall flag:+0
Roll,pitch,yaw:+0.00000 +0.00000 +0.00000
Cmd Velocity (x,y,z)+0.00000 +0.00000 +0.00000
Velocity (x,y,z):+0.00000 +0.00000 +0.00000
Stall flag:+0
Roll,pitch,yaw:+0.00000 +0.00000 +0.00000
Cmd Velocity (x,y,z)-1.00000 +0.00000 +0.00000
Velocity (x,y,z):+0.00000 +0.00000 +0.00000
Stall flag:+0
Roll,pitch,yaw:+0.00000 +0.00000 +0.00000
Here is the 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: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: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.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>10e-2</cfm>
<erp>0.2</erp>
<!-- updateRate: <0 == throttle simTime to match realTime.
0 == No throttling
>0 == Frequency at which to throttle the sim -->
<updateRate>0</updateRate>
</physics:ode>
<rendering:gui>
<type>fltk</type>
<size>800 600</size>
<pos>0 0</pos>
</rendering:gui>
<rendering:ogre>
<ambient>1 1 1 1</ambient>
<shadowTechnique>stencilAdditive</shadowTechnique>
<grid>false</grid>
</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>100 100</size>
<segments>10 10</segments>
<uvTile>100 100</uvTile>
<material>Gazebo/GrayGrid</material>
<mu1>109999.0</mu1>
<mu2>1000.0</mu2>
</geom:plane>
</body:plane>
</model:physical>
<model:physical name="pioneer2dx_model1">
<xyz>1 1 0.5</xyz>
<rpy>0 0 0</rpy>
<include embedded="true">
<xi:include href="models/pioneer2dx.model" />
</include>
</model:physical>
</gazebo:world>
Here is the pioneer model used :
<?xml version="1.0"?>
<!-- Generic Pioneer2dx Model -->
<model:physical name="default_pioneer_model"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model";
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor";
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:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint";
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint";
>
<xyz>0 0 0.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<canonicalBody>chassis_body</canonicalBody>
<body:box name="chassis_body">
<xyz>0.0 0.0 0.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<geom:box name="chassis_geom">
<xyz>0 0 0.0</xyz>
<size>0.445 0.277 0.17</size>
<mass>2.0</mass>
<mu1>1</mu1>
<visual>
<size>0.5 0.277 0.17</size>
<xyz>0 0 0.04</xyz>
<rpy>0 180 0</rpy>
<mesh>Pioneer2dx/chassis.mesh</mesh>
<material>Gazebo/Pioneer2Body</material>
</visual>
<visual>
<xyz>0.015 0 0.09</xyz>
<rpy>0 0 0</rpy>
<mesh>Pioneer2at/chassis_top.mesh</mesh>
<material>Gazebo/Black</material>
</visual>
<visual>
<xyz>0.21 0.0 0.068</xyz>
<rpy>0 0 0</rpy>
<size>0.12 0.29 0.034</size>
<mesh>Pioneer2at/sonarbank.mesh</mesh>
<material>Gazebo/Gold</material>
</visual>
<visual>
<xyz>-0.178 0.0 0.068</xyz>
<rpy>0 0 180</rpy>
<size>0.12 0.29 0.034</size>
<mesh>Pioneer2at/sonarbank.mesh</mesh>
<material>Gazebo/Gold</material>
</visual>
</geom:box>
</body:box>
<body:cylinder name="left_wheel">
<xyz>0.1 -0.17 -0.0725</xyz>
<rpy>0 90 90</rpy>
<geom:cylinder name="left_wheel_geom">
<size>0.075 0.05</size>
<mass>0.5</mass>
<mu1>0.5</mu1>
<visual>
<rpy>-90 0 0</rpy>
<size>0.125 0.05 0.125</size>
<mesh>Pioneer2dx/tire.mesh</mesh>
<material>Gazebo/Black</material>
</visual>
<visual>
<rpy>-90 0 0</rpy>
<size>0.073 0.05 0.073</size>
<mesh>Pioneer2at/wheel.mesh</mesh>
<material>Gazebo/Gold</material>
</visual>
<visual>
<rpy>0 0 0</rpy>
<xyz>0 0 0.015</xyz>
<size>0.04 0.04 0.08 </size>
<mesh>unit_cylinder</mesh>
<material>Gazebo/Black</material>
</visual>
</geom:cylinder>
</body:cylinder>
<body:cylinder name="right_wheel">
<xyz>0.1 0.17 -0.0725</xyz>
<rpy>0 90 90</rpy>
<geom:cylinder name="right_wheel_geom">
<size>0.075 0.05</size>
<mass>0.5</mass>
<mu1>0.5</mu1>
<visual>
<rpy>-90 0 0</rpy>
<size>0.125 0.05 0.125</size>
<mesh>Pioneer2dx/tire.mesh</mesh>
<material>Gazebo/Black</material>
</visual>
<visual>
<rpy>-90 0 0</rpy>
<size>0.073 0.05 0.073</size>
<mesh>Pioneer2at/wheel.mesh</mesh>
<material>Gazebo/Gold</material>
</visual>
<visual>
<rpy>0 0 0</rpy>
<xyz>0 0 -0.015</xyz>
<size>0.04 0.04 0.08</size>
<mesh>unit_cylinder</mesh>
<material>Gazebo/Black</material>
</visual>
</geom:cylinder>
</body:cylinder>
<body:sphere name="castor_body">
<xyz>-0.200 0 -0.11</xyz>
<rpy>0 0 0</rpy>
<geom:sphere name="castor_geom">
<size>0.04</size>
<mass>0.5</mass>
<mu1>0.5</mu1>
<visual>
<scale>0.04 0.04 0.04</scale>
<mesh>unit_sphere</mesh>
<material>Gazebo/Black</material>
</visual>
</geom:sphere>
</body:sphere>
<joint:hinge name="left_wheel_hinge">
<body1>left_wheel</body1>
<body2>chassis_body</body2>
<anchor>left_wheel</anchor>
<anchorOffset>0 0 0.04</anchorOffset>
<axis>0 1 0</axis>
<erp>0.8</erp>
<cfm>10e-5</cfm>
</joint:hinge>
<joint:hinge name="right_wheel_hinge">
<body1>right_wheel</body1>
<body2>chassis_body</body2>
<anchor>right_wheel</anchor>
<anchorOffset>0 0 -0.04</anchorOffset>
<axis>0 1 0</axis>
<erp>0.8</erp>
<cfm>10e-5</cfm>
</joint:hinge>
<joint:ball name="ball_joint">
<body1>castor_body</body1>
<body2>chassis_body</body2>
<anchor>castor_body</anchor>
<erp>0.8</erp>
<cfm>10e-5</cfm>
</joint:ball>
<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>
</model:physical>
--
View this message in context:
http://old.nabble.com/issue-with-setting-pioneer-robot-velocity-using-only-libgazebo-and-position-interface-%28gazebo-0.10.0%29-tp28450187p28450187.html
Sent from the playerstage-gazebo mailing list archive at Nabble.com.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-gazebo mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
