issue with setting pioneer robot velocity using only libgazebo and position interface (gazebo 0.10.0)

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

issue with setting pioneer robot velocity using only libgazebo and position interface (gazebo 0.10.0)

tarredondo
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</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>

   
   <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"?>


<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>
Reply | Threaded
Open this post in threaded view
|

Re: issue with setting pioneer robot velocity using only libgazebo and position interface (gazebo 0.10.0)

tarredondo
Hello All,

Using gdb, for the same scenario described in this thread I see this segmentation fault in my client app:

Program received signal SIGSEGV, Segmentation fault.
[Cambiando a Thread 0xb5f9cb70 (LWP 4316)]
0x00411a37 in boost::signals::detail::call_notification::call_notification(boost::shared_ptr<boost::signals::detail::signal_base_impl> const&) ()
   from /usr/lib/libboost_signals-mt.so.1.38.0

I see the same issue with version 1.40 of boost:

Program received signal SIGSEGV, Segmentation fault.
[Cambiando a Thread 0xb5f9bb70 (LWP 8332)]
0x00411be7 in boost::signals::detail::call_notification::call_notification(boost::shared_ptr<boost::signals::detail::signal_base_impl> const&) ()
   from /usr/lib/libboost_signals.so.1.40.0

I also see exactly the same problem after installing the software versions mentioned in this article:
http://playerstage.sourceforge.net/wiki/Install_ubuntu

I put a breakpoint in gazebo in:

(gdb) info breakpoints
Num     Type           Disp Enb Address    What
1       breakpoint     keep n   0x00ee7610 in gazebo::Simulator::PhysicsLoop()
                                       at /home/tomas/gazebo-0.10.0/server/Simulator.cc:694
5       breakpoint     keep y   0x00eaeee4 in gazebo::World::UpdateSimulationIface() at /home/tomas/gazebo-0.10.0/server/World.cc:783

Which is this code, that apparently never gets called because of the exception in the client code in Boost, hence the lack of response I guess:

780  // Process all the requests
781  for (unsigned int i=0; i < requestCount; i++)
782  {
783    SimulationRequestData *req = &(this->simIface->data->requests[i]);
784
785    switch (req->type)
786    {
787      case SimulationRequestData::UNPAUSE:

Here is the sample client code run using gdb with the exception in Boost:

tomas@tomas-desktop:~/gazebo-0.10.0/examples/libgazebo/tomas-test$ gdb tomas-test
<...>
(gdb) run
Starting program: /home/tomas/gazebo-0.10.0/examples/libgazebo/tomas-test/tomas-test
[Thread debugging using libthread_db enabled]
opening /tmp/gazebo-tomas-0/simulation.default 112 33842332
[Nuevo Thread 0xb5f9cb70 (LWP 4316)]

Breakpoint 1, main ()
    at /home/tomas/gazebo-0.10.0/examples/libgazebo/tomas-test/tomas-test.cc:26
26    simIface->Open(client, "default");
(gdb) s
gazebo::SimulationIface::Open (this=0x804d3d8, client=0x804d3b8, id=...)
    at /home/tomas/gazebo-0.10.0/libgazebo/SimIface.cc:97
97  Iface::Open(client,id);
(gdb) l
92
93 ////////////////////////////////////////////////////////////////////////////////
94 /// Open a simulation interface
95 void SimulationIface::Open(Client *client, std::string id)
96 {
97  Iface::Open(client,id);
98  this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface));
99
100  // Create the thread which waits "blockTimeUs" microseconds and
101  // then signals the goAckSignal
(gdb) print client
$1 = (class gazebo::Client *) 0x804d3b8
(gdb) print *client
$2 = {_vptr.Client = 0x159860, serverId = 0, clientId = 0, filename = {
    static npos = 4294967295,
    _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>},
      _M_p = 0x804d854 "/tmp/gazebo-tomas-0"}}, semKey = 324089338, semId = 0}
(gdb) print id
$3 = {static npos = 4294967295,
  _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x804d624 "default"}}
(gdb) s

Program received signal SIGSEGV, Segmentation fault.
[Cambiando a Thread 0xb5f9cb70 (LWP 4316)]
0x00411a37 in boost::signals::detail::call_notification::call_notification(boost::shared_ptr<boost::signals::detail::signal_base_impl> const&) ()
   from /usr/lib/libboost_signals-mt.so.1.38.0

Thanks,
Tomas




tarredondo wrote
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</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>

   
   <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"?>


<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>
Reply | Threaded
Open this post in threaded view
|

Re: issue with setting pioneer robot velocity using only libgazebo and position interface (gazebo 0.10.0)

tarredondo
In reply to this post by tarredondo
Hello all,

Just wanted to update on this issue in case it is helpful to someone else.  It is completely fixed by using the SVN version of Gazebo with OGRE 1.6.4 for some reason version 1.7.0 of OGRE caused this issue of the shared memory not getting updated.

Regards,
Tomas

tarredondo wrote
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</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>

   
   <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"?>


<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>