ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hi,

We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.

So the question is : How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint on Hydro ?

Thanks

Hi,

We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.

So the question is : How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint on Hydro ?

Thanks

Solution 1 :

I solve this problem with passing an argument to gmapping and then to use this arg as prefix for odom frame_id like this :

  <arg name="robot_name" default="turtlebot" />
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <param name="odom_frame" value="$(arg robot_name)/odom"/>
      …
 </node>

Now we get this tree :

                           map
                |                          |
    turtlebot_1/odom              turtlebot_2/odom

and

                       odom
                         |
                   base_footprint

But now I can get the connection between /turtlebot_1/odom -> /turtlebot_1/base_footprint and respectively with turtlebot_2.

Hi,

We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.

So the question is : How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint on Hydro ?

Solution 1 :

I solve this problem with passing an argument to gmapping and then to use this arg as prefix for odom frame_id like this :

  <arg name="robot_name" default="turtlebot" />
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <param name="odom_frame" value="$(arg robot_name)/odom"/>
      …
 </node>

Now we get this tree :

                           map
                |                          |
    turtlebot_1/odom              turtlebot_2/odom

and

                       odom
                         |
                   base_footprint

But now I can get the connection between /turtlebot_1/odom -> /turtlebot_1/base_footprint and respectively with turtlebot_2.

That's it I found !!

So it's the gazebo plugin's of kobuki ros which it's does not take the namespace of robot. To get two branches robot1/odom -> robot1/basefootprint and robot2/odom -> robot2/basefootprint we need to edit the plugin.

I don't know yet how I can tell to gazebo to use the modified plugin so I make a backup :

mv /opt/ros/hydro/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so.old

Then in my Ros workspace I clone the Git repot :

cd ros_workspace/src
git clone h t t p s : //github.com/yujinrobot/kobuki_desktop.git

Once you have all file you can edit the file in

ros_workspace/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp

For every publisher and subscriber I add

XXX.subscribe(node_name_ + "/XXXX");

Below all the edited file :

/**
 * @author Marcus Liebhardt
 *
 * This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
 */

#include <cmath>
#include <cstring>
#include <boost/bind.hpp>
#include <sensor_msgs/JointState.h>
#include <tf/LinearMath/Quaternion.h>
#include <gazebo/math/gzmath.hh>
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"

namespace gazebo
{

  enum {LEFT= 0, RIGHT=1};

  GazeboRosKobuki::GazeboRosKobuki() : shutdown_requested_(false)
  {
    // Make sure the ROS node for Gazebo has already been initialized
    if (!ros::isInitialized())
      {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
             << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
      }

    // Initialise variables
    wheel_speed_cmd_[LEFT] = 0.0;
    wheel_speed_cmd_[RIGHT] = 0.0;
    cliff_detected_left_ = false;
    cliff_detected_center_ = false;
    cliff_detected_right_ = false;
  }

  GazeboRosKobuki::~GazeboRosKobuki()
  {
    //  rosnode_->shutdown();
    shutdown_requested_ = true;
    // Wait for spinner thread to end
    //  ros_spinner_thread_->join();

    //  delete spinner_thread_;
    //  delete rosnode_;
  }

  void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
  {
    model_ = parent;
    if (!model_)
      {
    ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
    return;
      }
    // Get then name of the parent model and use it as node name
    std::string model_name = sdf->GetParent()->Get<std::string>("name");
    gzdbg << "Plugin model name: " << model_name << "\n";
    nh_ = ros::NodeHandle("");
    // creating a private name pace until Gazebo implements topic remappings
    nh_priv_ = ros::NodeHandle("/" + model_name);
    node_name_ = model_name;

    world_ = parent->GetWorld();
    /*
     * Prepare receiving motor power commands
     */
    motor_power_sub_ = nh_priv_.subscribe(node_name_ + "/commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
    motors_enabled_ = true;

    /*
     * Prepare joint state publishing
     */
    if (sdf->HasElement("left_wheel_joint_name"))
      {
    left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
             << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("right_wheel_joint_name"))
      {
    right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
             << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
      }
    joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
    joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
    if (!joints_[LEFT] || !joints_[RIGHT])
      {
    ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
    return;
      }
    joint_state_.header.frame_id = "Joint States";
    joint_state_.name.push_back(left_wheel_joint_name_);
    joint_state_.position.push_back(0);
    joint_state_.velocity.push_back(0);
    joint_state_.effort.push_back(0);
    joint_state_.name.push_back(right_wheel_joint_name_);
    joint_state_.position.push_back(0);
    joint_state_.velocity.push_back(0);
    joint_state_.effort.push_back(0);
    joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(node_name_ + "/joint_states", 1);

    /*
     * Prepare publishing odometry data
     */
    if (sdf->HasElement("publish_tf"))
      {
    publish_tf_ = sdf->GetElement("publish_tf")->Get<bool>();
    if (publish_tf_)
      {
        ROS_INFO_STREAM("Will publish tf." << " [" << node_name_ <<"]");
      }
    else
      {
        ROS_INFO_STREAM("Won't publish tf." << " [" << node_name_ <<"]");
      }
      }
    else
      {
    publish_tf_ = false;
    ROS_INFO_STREAM("Couldn't find the 'publish tf' parameter in the model description."
            << " Won't publish tf." << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("wheel_separation"))
      {
    wheel_sep_ = sdf->GetElement("wheel_separation")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("wheel_diameter"))
      {
    wheel_diam_ = sdf->GetElement("wheel_diameter")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("torque"))
      {
    torque_ = sdf->GetElement("torque")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    odom_pose_[0] = 0.0;
    odom_pose_[1] = 0.0;
    odom_pose_[2] = 0.0;
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>(node_name_ + "/odom", 1);
    odom_reset_sub_ = nh_priv_.subscribe(node_name_ + "/commands/reset_odometry", 10, &GazeboRosKobuki::resetOdomCB, this);

    /*
     * Prepare receiving velocity commands
     */
    if (sdf->HasElement("velocity_command_timeout"))
      {
    cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    last_cmd_vel_time_ = world_->GetSimTime();
    cmd_vel_sub_ = nh_priv_.subscribe(node_name_ + "/commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);

    /*
     * Prepare cliff sensors
     */
    std::string cliff_sensor_left_name, cliff_sensor_center_name, cliff_sensor_right_name;
    if (sdf->HasElement("cliff_sensor_left_name"))
      {
    cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("cliff_sensor_center_name"))
      {
    cliff_sensor_center_name = sdf->GetElement("cliff_sensor_center_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("cliff_sensor_right_name"))
      {
    cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                                    sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
    cliff_sensor_center_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                                      sensors::SensorManager::Instance()->GetSensor(cliff_sensor_center_name));
    cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                                     sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
    if (!cliff_sensor_left_)
      {
    ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
    return;
      }
    if (!cliff_sensor_center_)
      {
    ROS_ERROR_STREAM("Couldn't find the center cliff sensor in the model! [" << node_name_ <<"]");
    return;
      }
    if (!cliff_sensor_right_)
      {
    ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("cliff_detection_threshold"))
      {
    cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    cliff_sensor_left_->SetActive(true);
    cliff_sensor_center_->SetActive(true);
    cliff_sensor_right_->SetActive(true);
    cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>(node_name_ + "/events/cliff", 1);

    /*
     * Prepare bumper
     */
    std::string bumper_name;
    if (sdf->HasElement("bumper_name"))
      {
    bumper_name = sdf->GetElement("bumper_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
                                 sensors::SensorManager::Instance()->GetSensor(bumper_name));
    if (!bumper_)
      {
    ROS_ERROR_STREAM("Couldn't find the bumpers in the model! [" << node_name_ <<"]");
    return;
      }
    bumper_->SetActive(true);
    bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>(node_name_ + "/events/bumper", 1);

    /*
     * Prepare IMU
     */
    std::string imu_name;
    if (sdf->HasElement("imu_name"))
      {
    imu_name = sdf->GetElement("imu_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of IMU sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    imu_ = boost::shared_dynamic_cast<sensors::ImuSensor>(
                              sensors::SensorManager::Instance()->GetSensor(imu_name));
    if (!imu_)
      {
    ROS_ERROR_STREAM("Couldn't find the IMU in the model! [" << node_name_ <<"]");
    return;
      }
    imu_->SetActive(true);
    imu_pub_ = nh_priv_.advertise<sensor_msgs::Imu>(node_name_ + "/sensors/imu_data", 1);

    prev_update_time_ = world_->GetSimTime();
    ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosKobuki::OnUpdate, this));
  }

  void GazeboRosKobuki::motorPowerCB(const kobuki_msgs::MotorPowerPtr &msg)
  {
    if ((msg->state == kobuki_msgs::MotorPower::ON) && (!motors_enabled_))
      {
    motors_enabled_ = true;
    ROS_INFO_STREAM("Motors fired up. [" << node_name_ << "]");
      }
    else if ((msg->state == kobuki_msgs::MotorPower::OFF) && (motors_enabled_))
      {
    motors_enabled_ = false;
    ROS_INFO_STREAM("Motors taking a rest. [" << node_name_ << "]");
      }
  }

  void GazeboRosKobuki::cmdVelCB(const geometry_msgs::TwistConstPtr &msg)
  {
    last_cmd_vel_time_ = world_->GetSimTime();
    wheel_speed_cmd_[LEFT] = msg->linear.x - msg->angular.z * (wheel_sep_) / 2;
    wheel_speed_cmd_[RIGHT] = msg->linear.x + msg->angular.z * (wheel_sep_) / 2;
  }

  void GazeboRosKobuki::resetOdomCB(const std_msgs::EmptyConstPtr &msg)
  {
    odom_pose_[0] = 0.0;
    odom_pose_[1] = 0.0;
    odom_pose_[2] = 0.0;
  }

  void GazeboRosKobuki::OnUpdate()
  {
    /*
     * First process ROS callbacks
     */
    ros::spinOnce();

    /*
     * Update current time and time step
     */
    common::Time time_now = world_->GetSimTime();
    common::Time step_time = time_now - prev_update_time_;
    prev_update_time_ = time_now;

    /*
     * Joint states
     */
    joint_state_.header.stamp = ros::Time::now();
    joint_state_.header.frame_id = node_name_ + "/base_link";
    joint_state_.position[LEFT] = joints_[LEFT]->GetAngle(0).Radian();
    joint_state_.velocity[LEFT] = joints_[LEFT]->GetVelocity(0);
    joint_state_.position[RIGHT] = joints_[RIGHT]->GetAngle(0).Radian();
    joint_state_.velocity[RIGHT] = joints_[RIGHT]->GetVelocity(0);
    joint_state_pub_.publish(joint_state_);

    /*
     * Odometry (encoders & IMU)
     */
    odom_.header.stamp = joint_state_.header.stamp;
    odom_.header.frame_id = node_name_ + "/odom";
    odom_.child_frame_id = node_name_ + "/base_footprint";

    // Distance travelled by main wheels
    double d1, d2;
    double dr, da;
    d1 = d2 = 0;
    dr = da = 0;
    d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
    d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
    // Can see NaN values here, just zero them out if needed
    if (isnan(d1))
      {
    ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
                 << ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
    d1 = 0;
      }
    if (isnan(d2))
      {
    ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
                 << ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
    d2 = 0;
      }
    dr = (d1 + d2) / 2;
    da = (d2 - d1) / wheel_sep_; // ignored

    // Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
    vel_angular_ = imu_->GetAngularVelocity();

    // Compute odometric pose
    odom_pose_[0] += dr * cos( odom_pose_[2] );
    odom_pose_[1] += dr * sin( odom_pose_[2] );
    odom_pose_[2] += vel_angular_.z * step_time.Double();
    // Compute odometric instantaneous velocity
    odom_vel_[0] = dr / step_time.Double();
    odom_vel_[1] = 0.0;
    odom_vel_[2] = vel_angular_.z;

    odom_.pose.pose.position.x = odom_pose_[0];
    odom_.pose.pose.position.y = odom_pose_[1];
    odom_.pose.pose.position.z = 0;

    tf::Quaternion qt;
    qt.setEuler(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();
    odom_.pose.pose.orientation.w = qt.getW();

    odom_.pose.covariance[0]  = 0.1;
    odom_.pose.covariance[7]  = 0.1;
    odom_.pose.covariance[35] = 0.05;
    odom_.pose.covariance[14] = 1e6;
    odom_.pose.covariance[21] = 1e6;
    odom_.pose.covariance[28] = 1e6;

    odom_.twist.twist.linear.x = odom_vel_[0];
    odom_.twist.twist.linear.y = 0;
    odom_.twist.twist.linear.z = 0;
    odom_.twist.twist.angular.x = 0;
    odom_.twist.twist.angular.y = 0;
    odom_.twist.twist.angular.z = odom_vel_[2];
    odom_pub_.publish(odom_); // publish odom message

    if (publish_tf_)
      {
    odom_tf_.header = odom_.header;
    odom_tf_.child_frame_id = odom_.child_frame_id;
    odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
    odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
    odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
    odom_tf_.transform.rotation = odom_.pose.pose.orientation;
    tf_broadcaster_.sendTransform(odom_tf_);
      }

    /*
     * Publish IMU data
     */
    imu_msg_.header = joint_state_.header;
    math::Quaternion quat = imu_->GetOrientation();
    imu_msg_.orientation.x = quat.x;
    imu_msg_.orientation.y = quat.y;
    imu_msg_.orientation.z = quat.z;
    imu_msg_.orientation.w = quat.w;
    imu_msg_.orientation_covariance[0] = 1e6;
    imu_msg_.orientation_covariance[4] = 1e6;
    imu_msg_.orientation_covariance[8] = 0.05;
    imu_msg_.angular_velocity.x = vel_angular_.x;
    imu_msg_.angular_velocity.y = vel_angular_.y;
    imu_msg_.angular_velocity.z = vel_angular_.z;
    imu_msg_.angular_velocity_covariance[0] = 1e6;
    imu_msg_.angular_velocity_covariance[4] = 1e6;
    imu_msg_.angular_velocity_covariance[8] = 0.05;
    math::Vector3 lin_acc = imu_->GetLinearAcceleration();
    imu_msg_.linear_acceleration.x = lin_acc.x;
    imu_msg_.linear_acceleration.y = lin_acc.y;
    imu_msg_.linear_acceleration.z = lin_acc.z;
    imu_pub_.publish(imu_msg_); // publish odom message

    /*
     * Propagate velocity commands
     * TODO: Check how to simulate disabled motors, e.g. set MaxForce to zero, but then damping is important!
     */
    if (((time_now - last_cmd_vel_time_).Double() > cmd_vel_timeout_) || !motors_enabled_)
      {
    wheel_speed_cmd_[LEFT] = 0.0;
    wheel_speed_cmd_[RIGHT] = 0.0;
      }
    joints_[LEFT]->SetVelocity(0, wheel_speed_cmd_[LEFT] / (wheel_diam_ / 2.0));
    joints_[RIGHT]->SetVelocity(0, wheel_speed_cmd_[RIGHT] / (wheel_diam_ / 2.0));
    joints_[LEFT]->SetMaxForce(0, torque_);
    joints_[RIGHT]->SetMaxForce(0, torque_);

    /*
     * Cliff sensors
     * Check each sensor separately
     */
    // Left cliff sensor
    if ((cliff_detected_left_ == false) &&
    (cliff_sensor_left_->GetRange(0) >= cliff_detection_threshold_))
      {
    cliff_detected_left_ = true;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
    cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    else if ((cliff_detected_left_ == true) &&
         (cliff_sensor_left_->GetRange(0) < cliff_detection_threshold_))
      {
    cliff_detected_left_ = false;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
    cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    // Centre cliff sensor
    if ((cliff_detected_center_ == false) &&
    (cliff_sensor_center_->GetRange(0) >= cliff_detection_threshold_))
      {
    cliff_detected_center_ = true;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
    cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    else if ((cliff_detected_center_ == true) &&
         (cliff_sensor_center_->GetRange(0) < cliff_detection_threshold_))
      {
    cliff_detected_center_ = false;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
    cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    // Right cliff sensor
    if ((cliff_detected_right_ == false) &&
    (cliff_sensor_right_->GetRange(0) >= cliff_detection_threshold_))
      {
    cliff_detected_right_ = true;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
    cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    else if ((cliff_detected_right_ == true) &&
         (cliff_sensor_right_->GetRange(0) < cliff_detection_threshold_))
      {
    cliff_detected_right_ = false;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
    cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }

    /*
     * Bumpers
     */
    // In order to simulate the three bumper sensors, a contact is assigned to one of the bumpers
    // depending on its position. Each sensor covers a range of 60 degrees.
    // +90 ... +30: left bumper
    // +30 ... -30: centre bumper
    // -30 ... -90: right bumper

    // reset flags
    bumper_left_is_pressed_ = false;
    bumper_center_is_pressed_ = false;
    bumper_right_is_pressed_ = false;

    // parse contacts
    msgs::Contacts contacts;
    contacts = bumper_->GetContacts();
    math::Pose current_pose = model_->GetWorldPose();
    double robot_heading = current_pose.rot.GetYaw();

    for (int i = 0; i < contacts.contact_size(); ++i)
      {
    double rel_contact_pos =  contacts.contact(i).position(0).z() - current_pose.pos.z;
    if ((rel_contact_pos >= 0.015)
        && (rel_contact_pos <= 0.085)) // only consider contacts at the height of the bumper
      {
        // using the force normals below, since the contact position is given in world coordinates
        // also negating the normal, because it points from contact to robot centre
        double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x());
        double relative_contact_angle = global_contact_angle - robot_heading;

        if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6)))
          {
        bumper_left_is_pressed_ = true;
          }
        else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6)))
          {
        bumper_center_is_pressed_ = true;
          }
        else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2)))
          {
        bumper_right_is_pressed_ = true;
          }
      }
      }

    // check for bumper state change
    if (bumper_left_is_pressed_ && !bumper_left_was_pressed_)
      {
    bumper_left_was_pressed_ = true;
    bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
    bumper_event_pub_.publish(bumper_event_);
      }
    else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
      {
    bumper_left_was_pressed_ = false;
    bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
    bumper_event_pub_.publish(bumper_event_);
      }
    if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
      {
    bumper_center_was_pressed_ = true;
    bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
    bumper_event_pub_.publish(bumper_event_);
      }
    else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
      {
    bumper_center_was_pressed_ = false;
    bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
    bumper_event_pub_.publish(bumper_event_);
      }
    if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
      {
    bumper_right_was_pressed_ = true;
    bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
    bumper_event_pub_.publish(bumper_event_);
      }
    else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
      {
    bumper_right_was_pressed_ = false;
    bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
    bumper_event_pub_.publish(bumper_event_);
      }
  }

  void GazeboRosKobuki::spin()
  {
    while(ros::ok() && !shutdown_requested_)
      {
    ros::spinOnce();
      }
  }

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(GazeboRosKobuki);

} // namespace gazebo

Now you can compile the library

catkin_make gazebo_ros_kobuki
mv ~/ros_workspace/devel/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so

It's done.

Warning : If you want with one turtlebot, you need to change the launch file and add a prefix. Like with two robots.

It's work for me, but I really don't know if it's a good way to use multi robots on gazebo.

Hi,

We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.

So the The question is : number 1 :

How can we connected robot_name/odom -> robot_name/base_footprint on Hydro ?

The question 2 :

How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint still on Hydro ?

Solution 1 :

I solve this problem with passing an argument to gmapping and then to use this arg as prefix for odom frame_id like this :

  <arg name="robot_name" default="turtlebot" />
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <param name="odom_frame" value="$(arg robot_name)/odom"/>
      …
 </node>

Now we get this tree :

                           map
                |                          |
    turtlebot_1/odom              turtlebot_2/odom

and

                       odom
                         |
                   base_footprint

But now I can get the connection between /turtlebot_1/odom -> /turtlebot_1/base_footprint and respectively with turtlebot_2.

That's it I found !!

Question 1: So it's the gazebo plugin's of kobuki ros which it's does not take the namespace of robot. robot. To get two branches robot1/odom -> robot1/basefootprint and robot2/odom -> robot2/basefootprint we need to edit the plugin.

I don't know yet how I can tell to gazebo to use the modified plugin so I make a backup :

mv /opt/ros/hydro/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so.old

Then in my Ros workspace I clone the Git repot :

cd ros_workspace/src
~/ros_workspace/src
git clone h t t p s : //github.com/yujinrobot/kobuki_desktop.git

Don't have enouth karma to put a link.

Once you have all file you can edit the file in locate :

ros_workspace/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp
~/ros_workspace/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp

For every publisher and subscriber I add

XXX.subscribe(node_name_ + "/XXXX");

Below all the edited file :

/**
 * @author Marcus Liebhardt
 *
 * This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
 */

#include <cmath>
#include <cstring>
#include <boost/bind.hpp>
#include <sensor_msgs/JointState.h>
#include <tf/LinearMath/Quaternion.h>
#include <gazebo/math/gzmath.hh>
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"

namespace gazebo
{

  enum {LEFT= 0, RIGHT=1};

  GazeboRosKobuki::GazeboRosKobuki() : shutdown_requested_(false)
  {
    // Make sure the ROS node for Gazebo has already been initialized
    if (!ros::isInitialized())
      {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
             << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
      }

    // Initialise variables
    wheel_speed_cmd_[LEFT] = 0.0;
    wheel_speed_cmd_[RIGHT] = 0.0;
    cliff_detected_left_ = false;
    cliff_detected_center_ = false;
    cliff_detected_right_ = false;
  }

  GazeboRosKobuki::~GazeboRosKobuki()
  {
    //  rosnode_->shutdown();
    shutdown_requested_ = true;
    // Wait for spinner thread to end
    //  ros_spinner_thread_->join();

    //  delete spinner_thread_;
    //  delete rosnode_;
  }

  void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
  {
    model_ = parent;
    if (!model_)
      {
    ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
    return;
      }
    // Get then name of the parent model and use it as node name
    std::string model_name = sdf->GetParent()->Get<std::string>("name");
    gzdbg << "Plugin model name: " << model_name << "\n";
    nh_ = ros::NodeHandle("");
    // creating a private name pace until Gazebo implements topic remappings
    nh_priv_ = ros::NodeHandle("/" + model_name);
    node_name_ = model_name;

    world_ = parent->GetWorld();
    /*
     * Prepare receiving motor power commands
     */
    motor_power_sub_ = nh_priv_.subscribe(node_name_ + "/commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
    motors_enabled_ = true;

    /*
     * Prepare joint state publishing
     */
    if (sdf->HasElement("left_wheel_joint_name"))
      {
    left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
             << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("right_wheel_joint_name"))
      {
    right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
             << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
      }
    joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
    joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
    if (!joints_[LEFT] || !joints_[RIGHT])
      {
    ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
    return;
      }
    joint_state_.header.frame_id = "Joint States";
    joint_state_.name.push_back(left_wheel_joint_name_);
    joint_state_.position.push_back(0);
    joint_state_.velocity.push_back(0);
    joint_state_.effort.push_back(0);
    joint_state_.name.push_back(right_wheel_joint_name_);
    joint_state_.position.push_back(0);
    joint_state_.velocity.push_back(0);
    joint_state_.effort.push_back(0);
    joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(node_name_ + "/joint_states", 1);

    /*
     * Prepare publishing odometry data
     */
    if (sdf->HasElement("publish_tf"))
      {
    publish_tf_ = sdf->GetElement("publish_tf")->Get<bool>();
    if (publish_tf_)
      {
        ROS_INFO_STREAM("Will publish tf." << " [" << node_name_ <<"]");
      }
    else
      {
        ROS_INFO_STREAM("Won't publish tf." << " [" << node_name_ <<"]");
      }
      }
    else
      {
    publish_tf_ = false;
    ROS_INFO_STREAM("Couldn't find the 'publish tf' parameter in the model description."
            << " Won't publish tf." << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("wheel_separation"))
      {
    wheel_sep_ = sdf->GetElement("wheel_separation")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("wheel_diameter"))
      {
    wheel_diam_ = sdf->GetElement("wheel_diameter")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("torque"))
      {
    torque_ = sdf->GetElement("torque")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    odom_pose_[0] = 0.0;
    odom_pose_[1] = 0.0;
    odom_pose_[2] = 0.0;
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>(node_name_ + "/odom", 1);
    odom_reset_sub_ = nh_priv_.subscribe(node_name_ + "/commands/reset_odometry", 10, &GazeboRosKobuki::resetOdomCB, this);

    /*
     * Prepare receiving velocity commands
     */
    if (sdf->HasElement("velocity_command_timeout"))
      {
    cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    last_cmd_vel_time_ = world_->GetSimTime();
    cmd_vel_sub_ = nh_priv_.subscribe(node_name_ + "/commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);

    /*
     * Prepare cliff sensors
     */
    std::string cliff_sensor_left_name, cliff_sensor_center_name, cliff_sensor_right_name;
    if (sdf->HasElement("cliff_sensor_left_name"))
      {
    cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("cliff_sensor_center_name"))
      {
    cliff_sensor_center_name = sdf->GetElement("cliff_sensor_center_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("cliff_sensor_right_name"))
      {
    cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                                    sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
    cliff_sensor_center_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                                      sensors::SensorManager::Instance()->GetSensor(cliff_sensor_center_name));
    cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                                     sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
    if (!cliff_sensor_left_)
      {
    ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
    return;
      }
    if (!cliff_sensor_center_)
      {
    ROS_ERROR_STREAM("Couldn't find the center cliff sensor in the model! [" << node_name_ <<"]");
    return;
      }
    if (!cliff_sensor_right_)
      {
    ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
    return;
      }
    if (sdf->HasElement("cliff_detection_threshold"))
      {
    cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->Get<double>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    cliff_sensor_left_->SetActive(true);
    cliff_sensor_center_->SetActive(true);
    cliff_sensor_right_->SetActive(true);
    cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>(node_name_ + "/events/cliff", 1);

    /*
     * Prepare bumper
     */
    std::string bumper_name;
    if (sdf->HasElement("bumper_name"))
      {
    bumper_name = sdf->GetElement("bumper_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
                                 sensors::SensorManager::Instance()->GetSensor(bumper_name));
    if (!bumper_)
      {
    ROS_ERROR_STREAM("Couldn't find the bumpers in the model! [" << node_name_ <<"]");
    return;
      }
    bumper_->SetActive(true);
    bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>(node_name_ + "/events/bumper", 1);

    /*
     * Prepare IMU
     */
    std::string imu_name;
    if (sdf->HasElement("imu_name"))
      {
    imu_name = sdf->GetElement("imu_name")->Get<std::string>();
      }
    else
      {
    ROS_ERROR_STREAM("Couldn't find the name of IMU sensor in the model description!"
             << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
      }
    imu_ = boost::shared_dynamic_cast<sensors::ImuSensor>(
                              sensors::SensorManager::Instance()->GetSensor(imu_name));
    if (!imu_)
      {
    ROS_ERROR_STREAM("Couldn't find the IMU in the model! [" << node_name_ <<"]");
    return;
      }
    imu_->SetActive(true);
    imu_pub_ = nh_priv_.advertise<sensor_msgs::Imu>(node_name_ + "/sensors/imu_data", 1);

    prev_update_time_ = world_->GetSimTime();
    ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosKobuki::OnUpdate, this));
  }

  void GazeboRosKobuki::motorPowerCB(const kobuki_msgs::MotorPowerPtr &msg)
  {
    if ((msg->state == kobuki_msgs::MotorPower::ON) && (!motors_enabled_))
      {
    motors_enabled_ = true;
    ROS_INFO_STREAM("Motors fired up. [" << node_name_ << "]");
      }
    else if ((msg->state == kobuki_msgs::MotorPower::OFF) && (motors_enabled_))
      {
    motors_enabled_ = false;
    ROS_INFO_STREAM("Motors taking a rest. [" << node_name_ << "]");
      }
  }

  void GazeboRosKobuki::cmdVelCB(const geometry_msgs::TwistConstPtr &msg)
  {
    last_cmd_vel_time_ = world_->GetSimTime();
    wheel_speed_cmd_[LEFT] = msg->linear.x - msg->angular.z * (wheel_sep_) / 2;
    wheel_speed_cmd_[RIGHT] = msg->linear.x + msg->angular.z * (wheel_sep_) / 2;
  }

  void GazeboRosKobuki::resetOdomCB(const std_msgs::EmptyConstPtr &msg)
  {
    odom_pose_[0] = 0.0;
    odom_pose_[1] = 0.0;
    odom_pose_[2] = 0.0;
  }

  void GazeboRosKobuki::OnUpdate()
  {
    /*
     * First process ROS callbacks
     */
    ros::spinOnce();

    /*
     * Update current time and time step
     */
    common::Time time_now = world_->GetSimTime();
    common::Time step_time = time_now - prev_update_time_;
    prev_update_time_ = time_now;

    /*
     * Joint states
     */
    joint_state_.header.stamp = ros::Time::now();
    joint_state_.header.frame_id = node_name_ + "/base_link";
    joint_state_.position[LEFT] = joints_[LEFT]->GetAngle(0).Radian();
    joint_state_.velocity[LEFT] = joints_[LEFT]->GetVelocity(0);
    joint_state_.position[RIGHT] = joints_[RIGHT]->GetAngle(0).Radian();
    joint_state_.velocity[RIGHT] = joints_[RIGHT]->GetVelocity(0);
    joint_state_pub_.publish(joint_state_);

    /*
     * Odometry (encoders & IMU)
     */
    odom_.header.stamp = joint_state_.header.stamp;
    odom_.header.frame_id = node_name_ + "/odom";
    odom_.child_frame_id = node_name_ + "/base_footprint";

    // Distance travelled by main wheels
    double d1, d2;
    double dr, da;
    d1 = d2 = 0;
    dr = da = 0;
    d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
    d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
    // Can see NaN values here, just zero them out if needed
    if (isnan(d1))
      {
    ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
                 << ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
    d1 = 0;
      }
    if (isnan(d2))
      {
    ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
                 << ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
    d2 = 0;
      }
    dr = (d1 + d2) / 2;
    da = (d2 - d1) / wheel_sep_; // ignored

    // Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
    vel_angular_ = imu_->GetAngularVelocity();

    // Compute odometric pose
    odom_pose_[0] += dr * cos( odom_pose_[2] );
    odom_pose_[1] += dr * sin( odom_pose_[2] );
    odom_pose_[2] += vel_angular_.z * step_time.Double();
    // Compute odometric instantaneous velocity
    odom_vel_[0] = dr / step_time.Double();
    odom_vel_[1] = 0.0;
    odom_vel_[2] = vel_angular_.z;

    odom_.pose.pose.position.x = odom_pose_[0];
    odom_.pose.pose.position.y = odom_pose_[1];
    odom_.pose.pose.position.z = 0;

    tf::Quaternion qt;
    qt.setEuler(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();
    odom_.pose.pose.orientation.w = qt.getW();

    odom_.pose.covariance[0]  = 0.1;
    odom_.pose.covariance[7]  = 0.1;
    odom_.pose.covariance[35] = 0.05;
    odom_.pose.covariance[14] = 1e6;
    odom_.pose.covariance[21] = 1e6;
    odom_.pose.covariance[28] = 1e6;

    odom_.twist.twist.linear.x = odom_vel_[0];
    odom_.twist.twist.linear.y = 0;
    odom_.twist.twist.linear.z = 0;
    odom_.twist.twist.angular.x = 0;
    odom_.twist.twist.angular.y = 0;
    odom_.twist.twist.angular.z = odom_vel_[2];
    odom_pub_.publish(odom_); // publish odom message

    if (publish_tf_)
      {
    odom_tf_.header = odom_.header;
    odom_tf_.child_frame_id = odom_.child_frame_id;
    odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
    odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
    odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
    odom_tf_.transform.rotation = odom_.pose.pose.orientation;
    tf_broadcaster_.sendTransform(odom_tf_);
      }

    /*
     * Publish IMU data
     */
    imu_msg_.header = joint_state_.header;
    math::Quaternion quat = imu_->GetOrientation();
    imu_msg_.orientation.x = quat.x;
    imu_msg_.orientation.y = quat.y;
    imu_msg_.orientation.z = quat.z;
    imu_msg_.orientation.w = quat.w;
    imu_msg_.orientation_covariance[0] = 1e6;
    imu_msg_.orientation_covariance[4] = 1e6;
    imu_msg_.orientation_covariance[8] = 0.05;
    imu_msg_.angular_velocity.x = vel_angular_.x;
    imu_msg_.angular_velocity.y = vel_angular_.y;
    imu_msg_.angular_velocity.z = vel_angular_.z;
    imu_msg_.angular_velocity_covariance[0] = 1e6;
    imu_msg_.angular_velocity_covariance[4] = 1e6;
    imu_msg_.angular_velocity_covariance[8] = 0.05;
    math::Vector3 lin_acc = imu_->GetLinearAcceleration();
    imu_msg_.linear_acceleration.x = lin_acc.x;
    imu_msg_.linear_acceleration.y = lin_acc.y;
    imu_msg_.linear_acceleration.z = lin_acc.z;
    imu_pub_.publish(imu_msg_); // publish odom message

    /*
     * Propagate velocity commands
     * TODO: Check how to simulate disabled motors, e.g. set MaxForce to zero, but then damping is important!
     */
    if (((time_now - last_cmd_vel_time_).Double() > cmd_vel_timeout_) || !motors_enabled_)
      {
    wheel_speed_cmd_[LEFT] = 0.0;
    wheel_speed_cmd_[RIGHT] = 0.0;
      }
    joints_[LEFT]->SetVelocity(0, wheel_speed_cmd_[LEFT] / (wheel_diam_ / 2.0));
    joints_[RIGHT]->SetVelocity(0, wheel_speed_cmd_[RIGHT] / (wheel_diam_ / 2.0));
    joints_[LEFT]->SetMaxForce(0, torque_);
    joints_[RIGHT]->SetMaxForce(0, torque_);

    /*
     * Cliff sensors
     * Check each sensor separately
     */
    // Left cliff sensor
    if ((cliff_detected_left_ == false) &&
    (cliff_sensor_left_->GetRange(0) >= cliff_detection_threshold_))
      {
    cliff_detected_left_ = true;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
    cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    else if ((cliff_detected_left_ == true) &&
         (cliff_sensor_left_->GetRange(0) < cliff_detection_threshold_))
      {
    cliff_detected_left_ = false;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
    cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    // Centre cliff sensor
    if ((cliff_detected_center_ == false) &&
    (cliff_sensor_center_->GetRange(0) >= cliff_detection_threshold_))
      {
    cliff_detected_center_ = true;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
    cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    else if ((cliff_detected_center_ == true) &&
         (cliff_sensor_center_->GetRange(0) < cliff_detection_threshold_))
      {
    cliff_detected_center_ = false;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
    cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    // Right cliff sensor
    if ((cliff_detected_right_ == false) &&
    (cliff_sensor_right_->GetRange(0) >= cliff_detection_threshold_))
      {
    cliff_detected_right_ = true;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
    cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }
    else if ((cliff_detected_right_ == true) &&
         (cliff_sensor_right_->GetRange(0) < cliff_detection_threshold_))
      {
    cliff_detected_right_ = false;
    cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
    cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
    // convert distance back to an AD reading
    cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
    cliff_event_pub_.publish(cliff_event_);
      }

    /*
     * Bumpers
     */
    // In order to simulate the three bumper sensors, a contact is assigned to one of the bumpers
    // depending on its position. Each sensor covers a range of 60 degrees.
    // +90 ... +30: left bumper
    // +30 ... -30: centre bumper
    // -30 ... -90: right bumper

    // reset flags
    bumper_left_is_pressed_ = false;
    bumper_center_is_pressed_ = false;
    bumper_right_is_pressed_ = false;

    // parse contacts
    msgs::Contacts contacts;
    contacts = bumper_->GetContacts();
    math::Pose current_pose = model_->GetWorldPose();
    double robot_heading = current_pose.rot.GetYaw();

    for (int i = 0; i < contacts.contact_size(); ++i)
      {
    double rel_contact_pos =  contacts.contact(i).position(0).z() - current_pose.pos.z;
    if ((rel_contact_pos >= 0.015)
        && (rel_contact_pos <= 0.085)) // only consider contacts at the height of the bumper
      {
        // using the force normals below, since the contact position is given in world coordinates
        // also negating the normal, because it points from contact to robot centre
        double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x());
        double relative_contact_angle = global_contact_angle - robot_heading;

        if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6)))
          {
        bumper_left_is_pressed_ = true;
          }
        else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6)))
          {
        bumper_center_is_pressed_ = true;
          }
        else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2)))
          {
        bumper_right_is_pressed_ = true;
          }
      }
      }

    // check for bumper state change
    if (bumper_left_is_pressed_ && !bumper_left_was_pressed_)
      {
    bumper_left_was_pressed_ = true;
    bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
    bumper_event_pub_.publish(bumper_event_);
      }
    else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
      {
    bumper_left_was_pressed_ = false;
    bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
    bumper_event_pub_.publish(bumper_event_);
      }
    if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
      {
    bumper_center_was_pressed_ = true;
    bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
    bumper_event_pub_.publish(bumper_event_);
      }
    else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
      {
    bumper_center_was_pressed_ = false;
    bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
    bumper_event_pub_.publish(bumper_event_);
      }
    if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
      {
    bumper_right_was_pressed_ = true;
    bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
    bumper_event_pub_.publish(bumper_event_);
      }
    else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
      {
    bumper_right_was_pressed_ = false;
    bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
    bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
    bumper_event_pub_.publish(bumper_event_);
      }
  }

  void GazeboRosKobuki::spin()
  {
    while(ros::ok() && !shutdown_requested_)
      {
    ros::spinOnce();
      }
  }

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(GazeboRosKobuki);

} // namespace gazebo

Now you can compile the library

catkin_make gazebo_ros_kobuki
mv ~/ros_workspace/devel/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so

It's done.

Warning : : If you want to use with one turtlebot, you need to change the launch file and add a prefix. Like with two robots.

It's work for me, but I really don't know if it's a good way to use multi robots on gazebo. If you run rqt_graph you can see that gazebo publish into robot_name/joint_states and robot_name/odom. It's a good news for use.

Question 2 : To get the gmapping publish the map on correct topics and correct frame we need to specified the odom_frame and base_frame of robot.

Then we need to modify the output_frame_id of the lazer scan to become robot_name/camera_depth_frame otherwise gmapping push information into camera_depth_frame which is connected to nothing.

On Rviz all robots are in same place another stuff to do... but we can see the map of robot1 and robot2 !

Here is the launch files I use to put many robot in Gazebo and to view it with Rviz correctly.

Environment :

 <launch>
  <!-- Gazebo config -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="false"/>
    <arg name="gui" value="false"/>
    <arg name="world_name" value="$(find exploration)/worlds/gazebo_z.world"/>
    <!--                                                                                                                                              
    <arg name="world_name" value="worlds/willowgarage.world"/>                                                                                        
    -->
  </include>
  <!-- Launch for multi robots -->
  <include file="$(find exploration)/config/robot/robots.launch.xml" />
</launch>

Now the launch for multi-robots :

<launch>
  <!-- Plusieurs robots -->
  <!-- Turtlebot configuration pour tout les robots -->
  <arg name="base" default="$(optenv TURTLEBOT_BASE kobuki)"/>
  <arg name="battery" default="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>
  <arg name="stacks" default="$(optenv TURTLEBOT_STACKS hexagons)"/>
  <arg name="3d_sensor" default="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>

  <arg name="model" default="$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro" />

   <!-- send the robot XML to param server -->
   <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)'" />

  <!-- Bring up robot1 -->
  <group ns="turtlebot_1">
    <include file="$(find exploration)/config/robot/robot.launch.xml" >
      <arg name="robot_name"  value="turtlebot_1" />
    </include>
  </group>

  <group ns="turtlebot_2">                                                                                                                            
    <include file="$(find exploration)/config/robot/robot.launch.xml" >
      <arg name="x" value="1.0" />
      <arg name="robot_name"  value="turtlebot_2" />
    </include>
  </group>
</launch>

Robot launch

<launch>

  <!-- Un robot -->
  <arg name="x" default="0.0"/>
  <arg name="y" default="0.0"/>
  <arg name="z" default="0.0"/>
  <arg name="robot_name" default="turtlebot" />

  <!-- Gazebo model spawner for turtlebot -->
  <node name="spawn_$(arg robot_name)" pkg="gazebo_ros" type="spawn_model"
        args="-param /robot_description                                                                                                               
              -urdf                                                                                                                                   
              -unpause                                                                                                                                
              -x $(arg x)                                                                                                                             
              -y $(arg y)                                                                                                                             
              -z $(arg z)                                                                                                                             
              -model $(arg robot_name)                                                                                                                
              " respawn="false" output="screen" >
  </node>

  <!-- Modification du mobile_base en robot_name pour correspondre avec notre robot -->
  <node pkg="nodelet" type="nodelet" name="$(arg robot_name)_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg robot_name)_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
    <remap from="cmd_vel_mux/output" to="commands/velocity"/>
  </node>
  <!-- bumper2pc.launch.xml -->
  <node pkg="nodelet" type="nodelet" name="bumper2pointcloud" args="load kobuki_bumper2pc/Bumper2PcNodelet $(arg robot_name)_nodelet_manager">
    <param name="pointcloud_radius" value="0.24"/>
    <remap from="bumper2pointcloud/pointcloud"   to="sensors/bumper_pointcloud"/>
    <remap from="bumper2pointcloud/core_sensors" to="sensors/core"/>
  </node>
<!-- End bumper2pc.launch.xml -->

  <!-- robot_state_publisher  -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
    <param name="tf_prefix" type="string" value="$(arg robot_name)" />
  </node>

  <!-- fake lazer -->
  <node pkg="nodelet" type="nodelet" name="$(arg robot_name)_laserscan_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet $(arg robot_name)_laserscan_nodelet_manager">
    <param name="scan_height" value="10"/>
    <param name="output_frame_id" value="$(arg robot_name)/camera_depth_frame"/>
    <param name="range_min" value="0.45"/>
    <remap from="image" to="camera/depth/image_raw"/>
    <remap from="scan" to="scan"/>
 <!-- Velocity smoother -->
  <node pkg="nodelet" type="nodelet" name="$(arg robot_name)_navigation_velocity_smoother" args="load yocs_velocity_smoother/VelocitySmootherNodelet \
$(arg robot_name)_nodelet_manager">
    <rosparam file="$(find turtlebot_bringup)/param/defaults/smoother.yaml" command="load"/>
    <remap from="$(arg robot_name)_navigation_velocity_smoother/smooth_cmd_vel" to="cmd_vel_mux/input/navi"/>
    <remap from="$(arg robot_name)_navigation_velocity_smoother/odometry" to="odom"/>
    <remap from="$(arg robot_name)_navigation_velocity_smoother/robot_cmd_vel" to="commands/velocity"/>
  </node>
  <!-- End Velocity smoother -->

  <!-- Safety controller-->
  <node pkg="nodelet" type="nodelet" name="$(arg robot_name)_kobuki_safety_controller" args="load kobuki_safety_controller/SafetyControllerNodelet $(\
arg robot_name)_nodelet_manager">
    <remap from="$(arg robot_name)_kobuki_safety_controller/cmd_vel" to="cmd_vel_mux/input/safety_controller"/>
    <remap from="$(arg robot_name)_kobuki_safety_controller/events/bumper" to="events/bumper"/>
    <remap from="$(arg robot_name)_kobuki_safety_controller/events/cliff" to="events/cliff"/>
    <remap from="$(arg robot_name)_kobuki_safety_controller/events/wheel_drop" to="events/wheel_drop"/>
  </node>
  <!-- End Safety controller -->

  <!-- Gmapping -->
  <include file="$(find exploration)/config/gmapping_gazebo/gmapping.launch" >
    <arg name="scan_topic" value="scan" />
    <arg name="robot_name" value="$(arg robot_name)" />
  </include>
  <!--                                                                                                                                                
      <include file="$(find exploration)/config/move_base_gazebo/move_base.xml" />
  -->
</launch>

And gmapping :

<launch>
  <arg name="scan_topic" default="scan" />
  <arg name="robot_name" default="turtlebot" />
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <param name="odom_frame" value="$(arg robot_name)/odom"/>
    <param name="base_frame" value="$(arg robot_name)/base_footprint"/>
    <param name="map_frame" value="/map" />
    <param name="output_frame_id" value="$(arg robot_name)/camera_depth_frame"/>
    <param name="map_update_interval" value="5.0"/>
    <param name="maxUrange" value="3.2"/>
    <param name="maxRange" value="20.0" />
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.5"/>
    <param name="angularUpdate" value="0.436"/>
    <param name="temporalUpdate" value="10.0"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="80"/>
    <param name="xmin" value="-5.0"/>
    <param name="ymin" value="-5.0"/>
    <param name="xmax" value="5.0"/>
    <param name="ymax" value="5.0"/>

    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    <remap from="scan" to="$(arg scan_topic)"/>
    <!-- from our old setup:                                                                                                                          
         <param name="transform_publish_period" value="0.05" />                                                                                       
    -->
  </node>
</launch>