Multi-Gazebo Kobukis not updating orientations

[jihoonl]

KGP - Kobuki Gazebo Plugin

• Start two gazebo kobukis (segbot + kobuki is ok)
• Send commands to cmd_vel of one kobuki
• KGP catches these and moves around properly in gazebo world
• KGP does not post rotations to the odom topic (translations are there)
• KGP does not publish rotation information on base_footprint-odom tf.

[jihoonl]

This issue is related.

http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/

[slivingston]

#31 that I opened in May 2014 is also related.

[stonier]

• Kobuki is manually calculating its differential drive output
• There is a gazebo diff drive plugin - should we be using that?

[bit-pirate]

There is a gazebo diff drive plugin - should we be using that?

If you do that, then you need to split things up. I.e. develop a second plugin which handles Kobuki's sensor I/O.

[stonier]

Historical point : we didn't use that diff drive plugin because it wasn't there when the kobuki gazebo plugin was made.

In conclusion: doesn't sound like work worth chasing that gives us any extra benefits.

[slivingston]

@jihoonl , I think the correction is to use tf::Quaternion::setRPY() instead of setEuler(). Try applying the patch below. Note there are many different conventions in use for "Euler angles", which makes the setEuler() function somewhat ambiguous. Quick links to relevant tf API documentation are:

• setRPY
• setEuler

For comparison, the differential drive plugin in the gazebo_ros_pkgs repository uses setRPY. The preceding link is for the tip of indigo-devel branch of gazebo_ros_pkgs at time of writing.

I have tested the below patch for the case of spawning two Kobuki models without using the rocon infrastructure.

diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp
index 81340fc..17ac762 100644
--- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp
+++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp
@@ -105,7 +105,7 @@ void GazeboRosKobuki::updateOdometry(common::Time& step_time)
   odom_.pose.pose.position.z = 0;

   tf::Quaternion qt;
-  qt.setEuler(0,0,odom_pose_[2]);
+  qt.setRPY(0,0,odom_pose_[2]);
   odom_.pose.pose.orientation.x = qt.getX();
   odom_.pose.pose.orientation.y = qt.getY();
   odom_.pose.pose.orientation.z = qt.getZ();

[jihoonl]

@slivingston Thanks. I will test and get back to the issue.

[slivingston]

Update: The above patch is not sufficient, and indeed, we would not expect it to be because there is apparently correct behavior in the case of a single Kobuki. I have nearly identified the bug and will report soon.

[jihoonl]

Yeah I have just tested the patch is not sufficient to fix this issue.

To test this in easy way with concert,

Example Concert : Office sim Solution

1. gazebo_concert with two turtlebot.
2. add concert_service_indoor_2d_map_prep/indoor_2d_map_prep service.[The up-to-date source as of now!]
3. run Concert Make a Map through rocon_remocon

[slivingston]

The bug is due to the different Kobuki plugins using the same ImuSensor. I am creating a patch now.

[jihoonl]

Awesome Thanks! :)

[slivingston]

diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp
index 3e0ef50..5bf4ff1 100644
--- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp
+++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp
@@ -295,7 +295,7 @@ bool GazeboRosKobuki::prepareIMU()
     return false;
   }
   imu_ = boost::dynamic_pointer_cast<sensors::ImuSensor>(
-            sensors::SensorManager::Instance()->GetSensor(imu_name));
+     sensors::get_sensor(world_->GetName()+"::"+node_name_+"::base_footprint::"+imu_name));
   if (!imu_)
   {
     ROS_ERROR_STREAM("Couldn't find the IMU in the model! [" << node_name_ <<"]");

The basic problem is that the sensor name, as obtained on line 289 of gazebo_ros_kobuki_loads.cpp is not scoped. Thus it is the same when multiple models are spawned using the same URDF file, namely kobuki_gazebo.urdf.xacro, which declares the imu name at line 186. Hence, the sensor obtained by sensors::SensorManager::Instance()->GetSensor() is subject to a race condition. I propose avoiding this by using the above patch, which gets the sensor using a fully scoped name that includes the name of the model associated with the Kobuki Gazebo plugin.

The relevant Gazebo API documentation is gazebo::sensors::get_sensor().

[jihoonl]

@slivingston Could you make a pull request?

[slivingston]

I just opened #39

[jihoonl]

This has been resolved witih #39