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