Robotics StackExchange | Archived questions

Problem adding gazebo_ros_control plugin in sdf model and running in gazebo

Hello. I have a problem in my sdf model.I am trying add this code to my robot model sdf:

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/my_robot2</robotNamespace>

    </plugin>

I am running this on ubuntu 12.04 with ROS Hydro and Gazebo1.9. I do this on terminal: §optirun gazebo I use optirun because I have two graphics boards (Bumblebee). After having the gazebo running, and I try to insert my model in gazebo through the insert option of gazebo. But I receive this error when inserting the model and the gazebo close.

[FATAL] [1394020657.190138065]: You must call ros::init() before creating the first NodeHandle
[FATAL] [1394020657.190343598]: BREAKPOINT HIT
    file = /tmp/buildd/ros-hydro-roscpp-1.9.54-0precise-20140130-2139/src/libros/node_handle.cpp
    line=151

Here is my model.sdf:

<?xml version="1.0"?>
<sdf version="1.4">
  <model name="my_robot2">
    <static>false</static>

    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/my_robot2</robotNamespace> 
    </plugin>


    <link name="chassis">
      <pose>0 0 0.16 0 0 0</pose>
      <inertial> 
        <mass>5.0</mass>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.445 0.277 0.17</size>
          </box>
        </geometry>
      </collision>
      <collision name="castor_collision">
        <pose>-0.200 0 -0.12 0 0 0</pose>
        <geometry>
          <sphere>
            <radius>0.04</radius>
          </sphere>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0</mu>
              <mu2>0</mu2>
              <slip1>1.0</slip1>
              <slip2>1.0</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name="visual">
        <pose>0 0 0.04 0 0 0</pose>
        <geometry>
          <mesh>
            <uri>model://pioneer2dx/meshes/chassis.dae</uri>
          </mesh>
        </geometry>
      </visual>
      <visual name="castor_visual">
        <pose>-0.200 0 -0.12 0 0 0</pose>
        <geometry>
          <sphere>
            <radius>0.04</radius>
          </sphere>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/FlatBlack</name>
          </script>
        </material>
      </visual>
    </link>
    <link name="right_wheel">
      <pose>0.1 -.17 0.11 0 1.5707 1.5707</pose>
      <inertial>
        <mass>0.1</mass>
      </inertial>
      <collision name="collision">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>100000.0</mu>
              <mu2>100000.0</mu2>
              <slip1>0.0</slip1>
              <slip2>0.0</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/FlatBlack</name>
          </script>
        </material>
      </visual>
    </link>
    <link name="left_wheel">
      <pose>0.1 .17 0.11 0 1.5707 1.5707</pose>
      <inertial>
        <mass>0.5</mass>
      </inertial>
      <collision name="collision">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>100000.0</mu>
              <mu2>100000.0</mu2>
              <slip1>0.0</slip1>
              <slip2>0.0</slip2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>0.11</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/FlatBlack</name>
          </script>
        </material>
      </visual>
    </link>
    <joint type="revolute" name="left_wheel_hinge">
      <pose>0 0 -0.03 0 0 0</pose>
      <child>left_wheel</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
    <joint type="revolute" name="right_wheel_hinge">
      <pose>0 0 0.03 0 0 0</pose>
      <child>right_wheel</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>

<!--
    <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
      <robotNamespace>/myrobot2</robotNamespace>
      <alwaysOn>true</alwaysOn>
      <updateRate>100</updateRate>
      <leftJoint>left_wheel_hinge</leftJoint>
      <rightJoint>right_wheel_hinge</rightJoint>
      <torque>5</torque>
      <wheelSeparation>0.39</wheelSeparation>
      <wheelDiameter>0.15</wheelDiameter>
      <commandTopic>p3dx/cmd_vel</commandTopic>
      <odometryTopic>p3dx/odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>-->

    <plugin filename="libDiffDrivePlugin.so" name="diff_drive">
      <left_joint>left_wheel_hinge</left_joint>
      <right_joint>right_wheel_hinge</right_joint>
      <torque>5</torque>
    </plugin>
    <include>
      <uri>model://noisy_laser</uri>
      <pose>0.2 0 0.3 0 0 0</pose>
    </include>
    <joint name="hokuyo_joint" type="revolute">
      <child>hokuyo::laser_link</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>
  </model>
</sdf>

If I erase that code of gazeboroscontrol plugin, the gazebo can insert the model without nay problem. Can anyone find the problem?? I search on web but only tutorials that explain sdf, are of the gazebosim, but they don't explain how insert plugin to sdf files. For me the tutorials of modeling a robot in sdf are too incomplete. I think that I am putting the code in wrong way.

I have another question, if rviz suport SDF models or only URDF models? If I can load my model in rviz with sdf model?

Thanks for your time.

Asked by Raul Gui on 2014-03-05 07:13:43 UTC

Comments

Answers

Hi,

I don't know if this helps but can you try to start gazebo with the script from the gazebo_ros pkg (you might have to edit it for optirun...)

This script forces gazebo to start with some gazebo plugins which might be needed for gazebo_ros_control (I quess the initialize call will be done in one of those plugins which means gazebo_ros_control does not has to call initialize again...)

Asked by evilBiber on 2014-03-13 07:26:25 UTC

Comments

As evilBiber said, the problem likely isn't in your model file, but rather that you must run gazebo as a ROS node for plugin integration to work. For example, given the world file below:

<?xml version="1.0" ?>
<sdf version='1.4'>
  <world name='default'>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <cast_shadows>0</cast_shadows>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Grey</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
    </model>
    <physics type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
      <gravity>0 0 -9.8</gravity>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <model name='pioneer2dx'>
      <link name='chassis'>
        <pose>0 0 0.16 0 -0 0</pose>
        <inertial>
          <mass>5.67</mass>
          <inertia>
            <ixx>0.07</ixx>
            <iyy>0.08</iyy>
            <izz>0.1</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.445 0.277 0.17</size>
            </box>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <bounce/>
            <friction>
              <ode/>
            </friction>
            <contact>
              <ode/>
            </contact>
          </surface>
        </collision>
        <collision name='castor_collision'>
          <pose>-0.2 0 -0.12 0 -0 0</pose>
          <geometry>
            <sphere>
              <radius>0.04</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0</mu>
                <mu2>0</mu2>
                <slip1>1</slip1>
                <slip2>1</slip2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <pose>0 0 0.04 0 -0 0</pose>
          <geometry>
            <mesh>
              <uri>model://pioneer2dx/meshes/chassis.dae</uri>
            </mesh>
          </geometry>
        </visual>
        <visual name='castor_visual'>
          <pose>-0.2 0 -0.12 0 -0 0</pose>
          <geometry>
            <sphere>
              <radius>0.04</radius>
            </sphere>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/FlatBlack</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <link name='right_wheel'>
        <pose>0.1 -0.17 0.11 0 1.5707 1.5707</pose>
        <inertial>
          <mass>1.5</mass>
          <inertia>
            <ixx>0.0051</ixx>
            <iyy>0.0051</iyy>
            <izz>0.009</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100000</mu>
                <mu2>100000</mu2>
                <slip1>0</slip1>
                <slip2>0</slip2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/FlatBlack</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <link name='left_wheel'>
        <pose>0.1 0.17 0.11 0 1.5707 1.5707</pose>
        <inertial>
          <mass>1.5</mass>
          <inertia>
            <ixx>0.0051</ixx>
            <iyy>0.0051</iyy>
            <izz>0.009</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100000</mu>
                <mu2>100000</mu2>
                <slip1>0</slip1>
                <slip2>0</slip2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <geometry>
            <cylinder>
              <radius>0.11</radius>
              <length>0.05</length>
            </cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/FlatBlack</name>
            </script>
          </material>
        </visual>
        <velocity_decay>
          <linear>0</linear>
          <angular>0</angular>
        </velocity_decay>
        <self_collide>0</self_collide>
        <kinematic>0</kinematic>
        <gravity>1</gravity>
      </link>
      <joint name='left_wheel_hinge' type='revolute'>
        <pose>0 0 -0.03 0 -0 0</pose>
        <child>left_wheel</child>
        <parent>chassis</parent>
        <axis>
          <xyz>0 1 0</xyz>
          <limit>
            <lower>-1e+16</lower>
            <upper>1e+16</upper>
          </limit>
        </axis>
      </joint>
      <joint name='right_wheel_hinge' type='revolute'>
        <pose>0 0 0.03 0 -0 0</pose>
        <child>right_wheel</child>
        <parent>chassis</parent>
        <axis>
          <xyz>0 1 0</xyz>
          <limit>
            <lower>-1e+16</lower>
            <upper>1e+16</upper>
          </limit>
        </axis>
      </joint>
      <plugin name='differential_drive_controller' filename='libgazebo_ros_diff_drive.so'>
        <alwaysOn>true</alwaysOn>
        <updateRate>100</updateRate>
        <leftJoint>left_wheel_hinge</leftJoint>
        <rightJoint>right_wheel_hinge</rightJoint>
        <wheelSeparation>0.39</wheelSeparation>
        <wheelDiameter>0.15</wheelDiameter>
        <torque>5</torque>
        <commandTopic>cmd_vel</commandTopic>
        <odometryTopic>odom</odometryTopic>
        <odometryFrame>odom</odometryFrame>
        <robotBaseFrame>chassis</robotBaseFrame>
      </plugin>
      <pose>0 0 0 0 -0 0</pose>
      <static>0</static>
    </model>
    <state world_name='default'>
      <sim_time>85 304000000</sim_time>
      <real_time>85 849190220</real_time>
      <wall_time>1432260579 736436496</wall_time>
      <model name='ground_plane'>
        <pose>0 0 0 0 -0 0</pose>
        <link name='link'>
          <pose>0 0 0 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
      <model name='pioneer2dx'>
        <pose>-0.103826 0.027961 2e-06 -2e-06 -8e-06 -0.094162</pose>
        <link name='chassis'>
          <pose>-0.103827 0.027961 0.160002 -2e-06 -8e-06 -0.094162</pose>
          <velocity>-0.000797 0.002229 0.003363 -0.024657 -0.007301 0.0023</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
        <link name='left_wheel'>
          <pose>0.011714 0.187806 0.110002 1.38258 1.57033 2.85912</pose>
          <velocity>-0.000816 0.001565 0.000275 -0.014307 -0.006987 0.002012</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
        <link name='right_wheel'>
          <pose>-0.020254 -0.150688 0.110002 -1.56886 1.51434 -0.092322</pose>
          <velocity>-0.000347 0.001559 0.008433 -0.014387 -0.003243 0.000126</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
    </state>
    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>5 -5 2 0 0.275643 2.35619</pose>
        <view_controller>orbit</view_controller>
      </camera>
    </gui>
  </world>
</sdf>

Save it somewhere in your local filesystem as pioneer2dx_ros.world. Then open a terminal window and start the ROS middleware by entering:

roscore

Open another terminal window, cd to the folder containing the pioneer2dx_ros.world and enter:

rosrun gazebo_ros gazebo -file pioneer2dx_ros.world

You should now have a Gazebo window opened with a Pioneer 2DX placed in the middle of an empty world.

Finally, open a third terminal window (it's the last one, I promise) and check if the differential drive topics have been published:

rostopic list

The following topics should be visible, among others:

/pioneer2dx/cmd_vel
/pioneer2dx/odom

You should now be able to get the robot moving by publishing messages to the /pioneer2dx/cmd_vel topic, e.g.

rostopic pub -1 /pioneer2dx/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}'

Should get the robot running in a loop.

Asked by xperroni on 2015-05-21 21:53:41 UTC

Comments

What about including ros_control in the sdf file? If i include the ros_control plugin in the sdf file, I get a error: Could not find the 'robot' element in the xml file . An sdf file for a model does not contain the "robot" tag, but instead "model". Does anyone know how to go about this?

Asked by l0g1x on 2015-12-28 17:23:32 UTC