Problem with multiple navigation on Gazebo

asked 2014-01-06 22:35:09 -0600

Stefano Primatesta
402 ●18 ●20 ●25

updated 2014-02-15 23:00:40 -0600

I tried to simulate multiple turtlebot in Gazebo. It seems there are no problem, but when I tried to see view_frames I have observed that /odom is not connect to /robot1/base_footprint and /robot2/base_footprint. On Gazebo there isn't problem, but the problems are there when I do the navigation.

This is my tf_tree ( http://db.tt/FBCGz4sD )

This is my launch file

<launch>
<arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba-->
<arg name="battery"   value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/> <!-- /proc/acpi/battery/BAT0 --> 
<arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
<arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro --> 


<param name="/use_sim_time" value="true" />

<!-- start world -->
<node name="gazebo" pkg="gazebo_ros" type="gzserver" args="$(find tesi_gazebo)/worlds/empty.world" respawn="false" output="screen" />

<!-- start gui -->
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>


<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<param name="tf_prefix" value="robot1" />

<include file="$(find tesi_gazebo)/launch/includes/$(arg base).launch.xml">
<arg name="base" value="$(arg base)"/>
<arg name="stacks" value="$(arg stacks)"/>
<arg name="3d_sensor" value="$(arg 3d_sensor)"/>
<arg name="init_pose" value="-x 1 -y 1 -z 0" />
<arg name="robot_name"  value="Robot1" />
</include>

<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="robot1"/>
</node>

<!-- Fake laser -->
<node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="/camera_depth_frame"/>
<param name="range_min" value="0.45"/>
<remap from="image" to="/camera/depth/image_raw"/>
<remap from="scan" to="/scan"/>
</node>
</group>


<!-- BEGIN ROBOT 2-->
<group ns="robot2">
<param name="tf_prefix" value="robot2" />

<include file="$(find tesi_gazebo)/launch/includes/$(arg base).launch.xml">
<arg name="base" value="$(arg base)"/>
<arg name="stacks" value="$(arg stacks)"/>
<arg name="3d_sensor" value="$(arg 3d_sensor)"/>
<arg name="init_pose" value="-x 1 -y -1 -z 0" />
<arg name="robot_name"  value="Robot2" />
</include>

<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="robot2"/>
</node>

<!-- Fake laser -->
<node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
    args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="/camera_depth_frame"/>
<param name="range_min" value="0.45"/>
<remap from="image" to="/camera/depth/image_raw"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>

This is kobuki.launch.xml

<launch>
<arg name="base"/>
<arg name="stacks"/>
<arg name="3d_sensor"/>
<arg name="robot_name"/>
<arg name="init_pose"/>

<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'" />
<param name="/robot_description" command="$(arg urdf_file)" />

<!-- Gazebo model spawner -->
<node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
args="$(arg init_pose) -unpause -urdf -param /robot_description -model $(arg robot_name)"/>

<!-- Velocity muxer -->
<node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
<param name="yaml_cfg_file ...

(more)

edit retag flag offensive close merge delete

add a comment

4 Answers

Sort by » oldest newest most voted

answered 2014-02-14 01:42:18 -0600

Zephyrin
36 ●1 ●3 ●5

updated 2014-02-19 06:23:11 -0600

Hi,

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 still on Hydro ?

Question 1: 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

Don't have enouth karma to put a link.

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

~/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 ...

(more)

edit flag offensive delete link

Comments

Exactly, this is the real problem! I think we need to change the code where the nodes publish /odom and/or /map. But I haven't found where this happens.

Stefano Primatesta ( 2014-02-14 03:57:29 -0600 )edit

take a look base.yaml in kobuki_node. It specifies odom frame and base_frame for turtlebot2.

jihoonl ( 2014-02-18 11:37:03 -0600 )edit

I tried to change all odom frame and base frame for every part of my launch but it changes nothing.

Zephyrin ( 2014-02-19 00:03:46 -0600 )edit

Still have problem with move_base which don't want to connect to robot_name/base_footprint...

Zephyrin ( 2014-02-19 06:31:35 -0600 )edit

Take a look amcl. you can specify robot's odom and base frame. http://wiki.ros.org/amcl?distro=hydro example) https://github.com/turtlebot/turtlebot_apps/blob/hydro/turtlebot_navigation/launch/includes/amcl.launch.xml

jihoonl ( 2014-02-19 12:02:59 -0600 )edit

Hi Zephyrin, I use your method because works better. I've implemented the navigation but the two robots share the orientation. When robot1 move in the environment, It works correctly. But robot2 does the same rotation, although still in place. I think that the two robots share the command that performs the rotation, but I can not find the problem to be corrected.

Stefano Primatesta ( 2014-03-09 23:48:38 -0600 )edit

Hi Prima89, do you have a correct tf_transform tree ? And how do you do for your navigation ? I also remap some node of kobuki to have a correct graph on rqt but I don't have my source right now.

Zephyrin ( 2014-03-10 01:37:26 -0600 )edit

Heh guys any luck with getting multiple turtlebots to work with either AMCL or gmapping, I can get two of them to move through publishing to cmd_vel but they still share the odom. I'm unable to get two robots to work with amcl navigation because of 'waiting on transform from base_footprint to map to become available' error

cdrwolfe ( 2014-03-23 03:37:32 -0600 )edit

see more comments

answered 2014-02-12 14:36:42 -0600

jihoonl

634 ●11 ●17 ●21

navigation stack expects the connected tf tree from /map -> /odom -> /<robot_base> according to REP105(http://www.ros.org/reps/rep-0105.html)

So the /odom and /base_footprints should be connected.

edit flag offensive delete link

add a comment

answered 2014-02-11 22:51:22 -0600

Stefano Primatesta
402 ●18 ●20 ●25

updated 2014-02-19 01:47:23 -0600

Is there no one who can help me?

thanks Zephyrin!!!

Also I have found a solution. Simply I modified the file kobuki_gazebo.urdf.xacro. Because there is a problem with the plugin "kobuki_controller". I replace this plugin with "differential_drive_controller".

This is the new kobuki_gazebo.urdf.xacro

<?xml version="1.0"?>

<robot name="kobuki_sim" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="kobuki_sim">
  <gazebo reference="wheel_left_link">
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <kp>100000000.0</kp>
    <kd>10000.0</kd>
  </gazebo>

  <gazebo reference="wheel_right_link">
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <kp>100000000.0</kp>
    <kd>10000.0</kd>
  </gazebo>

  <gazebo reference="caster_front_link">
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <kp>100000000.0</kp>
    <kd>10000.0</kd>
  </gazebo>

  <gazebo reference="caster_back_link">
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <kp>100000000.0</kp>
    <kd>10000.0</kd>
  </gazebo>

  <gazebo reference="base_link">
    <sensor type="contact" name="bumpers">
      <always_on>1</always_on>
      <update_rate>50.0</update_rate>
      <visualize>true</visualize>
      <contact>
        <collision>base_footprint_collision_base_link</collision>
      </contact>
    </sensor>
  </gazebo>

  <gazebo reference="cliff_sensor_left_link">
    <sensor type="ray" name="cliff_sensor_left">
      <always_on>true</always_on>
      <update_rate>50</update_rate>
      <visualize>true</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>50</samples>
            <resolution>1.0</resolution>
            <min_angle>-0.0436</min_angle>  <!-- -2.5 degree -->
            <max_angle>0.0436</max_angle> <!-- 2.5 degree -->
          </horizontal>
      <!--            Can't use vertical rays until this bug is resolved: -->
      <!--            https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
     <!--             <vertical> -->
     <!--               <samples>50</samples> -->
     <!--               <resolution>1.0</resolution> -->
     <!--               <min_angle>-0.0436</min_angle>  -2.5 degree -->
     <!--               <max_angle>0.0436</max_angle> 2.5 degree -->
     <!--             </vertical> -->
        </scan>
        <range>
          <min>0.01</min>
          <max>0.15</max>
          <resolution>1.0</resolution>
        </range>
      </ray>
    </sensor>
  </gazebo>

  <gazebo reference="cliff_sensor_right_link">
    <sensor type="ray" name="cliff_sensor_right">
      <always_on>true</always_on>
      <update_rate>50</update_rate>
      <visualize>true</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>50</samples>
            <resolution>1.0</resolution>
            <min_angle>-0.0436</min_angle>  <!-- -2.5 degree -->
            <max_angle>0.0436</max_angle> <!-- 2.5 degree -->
          </horizontal>
      <!--            Can't use vertical rays until this bug is resolved: -->
      <!--            https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
      <!--            <vertical> -->
      <!--              <samples>50</samples> -->
      <!--              <resolution>1.0</resolution> -->
      <!--              <min_angle>-0.0436</min_angle>  -2.5 degree -->
      <!--              <max_angle>0.0436</max_angle> 2.5 degree -->
      <!--            </vertical> -->
        </scan>
        <range>
          <min>0.01</min>
          <max>0.15</max>
          <resolution>1.0</resolution>
        </range>
      </ray>
    </sensor>
  </gazebo>

  <gazebo reference="cliff_sensor_front_link">
    <sensor type="ray" name="cliff_sensor_front">
      <always_on>true</always_on>
      <update_rate>50</update_rate>
      <visualize>true</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>50</samples>
            <resolution>1.0</resolution>
            <min_angle>-0.0436</min_angle>  <!-- -2.5 degree -->
            <max_angle>0.0436</max_angle> <!-- 2.5 degree -->
          </horizontal>
      <!--            Can't use vertical rays until this bug is resolved: -->
      <!--            https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
      <!--            <vertical> -->
      <!--              <samples>50</samples> -->
      <!--              <resolution>1.0</resolution> -->
      <!--              <min_angle>-0.0436</min_angle>  -2.5 degree -->
      <!--              <max_angle>0.0436</max_angle> 2.5 degree -->
      <!--            </vertical> -->
        </scan>
        <range>
          <min>0.01</min>
          <max>0.15</max>
          <resolution>1.0</resolution>
        </range>
      </ray>
    </sensor>
  </gazebo>

  <gazebo reference="gyro_link">
   <sensor type="imu" name="imu">
    <always_on>true</always_on>
    <update_rate>50</update_rate>
    <visualize>false</visualize>
    <imu>
      <noise>
        <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev ...

(more)

edit flag offensive delete link

Comments

Hello. I have the same problems, and I don't find the solution either... Maybe we can help each other...

Carlos Hdz ( 2014-02-18 10:54:40 -0600 )edit

If you use tho odom frame and base footprint frame of gmapping didn't do the trick ? When I do that, I get the warning : Waiting on transform from turtlebot_2/base_footprint to turtlebot_2/map to become available before running costmap, tf error And after a while, target odom drop 100.00% of message. My tf tree seems to be correct and when I compare the rqt_graph with 1 robot spawn without prefix and group is seems good. I'm running out of ideas … Did you get the joint_states node distinct for each robot and connect to gazebo ?

Zephyrin ( 2014-02-19 03:42:30 -0600 )edit

add a comment

answered 2014-04-10 11:04:09 -0600

Carlos Hdz

103 ●1 ●2 ●7

updated 2014-04-10 11:05:51 -0600

Hi everyone...

I had the same problem, and I found a solution (at least for my case). I will try to explain what I do, in case of someone is interested in try it out (this will be a long answer!)..

First, i will you you the launch files that i use, and then some of the nodes I write in order to make it work... The launch files are:

<?xml version="1.0" ?>

<launch>

  <arg name="r1_x" default="1" />
  <arg name="r1_y" default="1" />
  <arg name="r2_x" default="-1" />
  <arg name="r2_y" default="1" />

  <!-- start world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  <!--         -->
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="true"/>
    <arg name="gui" value="true"/>       <!-- graphic interface -->
    <arg name="headless" value="false"/>
    <arg name="world_name" value="$(find tb_tables)/worlds/tres_mesas.world"/>
  </include>

   <!-- include robot description -->
   <param name="robot_description"
    command="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/kobuki_hexagons_kinect.urdf.xacro'" />

  <!-- BEGIN ROBOT 0-->
  <group ns="robot0">
    <param name="tf_prefix" value="robot0_tf" />
    <include file="$(find tb_tables)/launch/simulation/includes/turtlebot1.launch" >
      <arg name="init_pose" value="-x $(arg r2_x) -y $(arg r2_y) -z 0" />
      <arg name="robot_name"  value="robot0" />
    </include>
  </group>    

  <!-- BEGIN ROBOT 1-->
  <group ns="robot1">
    <param name="tf_prefix" value="robot1_tf" />
    <include file="$(find tb_tables)/launch/simulation/includes/turtlebot1.launch" >
      <arg name="init_pose" value="-x $(arg r1_x) -y $(arg r1_y) -z 0" />
      <arg name="robot_name"  value="robot1" />
    </include>
  </group>    
  </launch>

This is the auxiliar launch file (called above, inside a namespace).

  <?xml version="1.0" ?>
<launch>

    <arg name="robot_name"/>
    <arg name="init_pose"/>

    <!-- odom publisher -->
        <param name="robot_name" value=  "$(arg robot_name)" />
        <node pkg="tb_tables" type="odom_sim_2" name="odom_sim"/>

      <!-- Gazebo model spawner -->
    <node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
        args="$(arg init_pose) -unpause -urdf -param /robot_description -model $(arg robot_name) -robotNamespace $(arg robot_name) -namespace $(arg robot_name)"/>

    <!-- robot state publisher (at 1 Hz) --> 
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
     <param name="publish_frequency" type="double" value="1.0" />
    </node>

    <!-- Publish tf info -->
    <node pkg="tb_tables" name="tb_tf_broadcaster" type="tb_tf_can2">
    </node>     
</launch>

And this launch file is for running the navigation stack for both robots:

<?xml version="1.0" ?>

<launch>

  <arg name="map_file" default="$(find tb_tables)/maps/blank_map.yaml"/>
  <arg name="rviz_robot" default="robot1"/>


  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" > 
  </node>

  <!-- BEGIN ROBOT 0-->
  <group ns="robot0">
    <param name="tf_prefix" value="robot0_tf" />
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> 
    <node pkg="tb_tables" name="fake_localization" type="fake_localiza2" output="screen"/>
    <include file="$(find tb_tables)/launch/simulation/includes/move_base_sim.launch.xml" />
  </group>

  <!-- BEGIN ROBOT 1-->
  <group ns="robot1">
    <param name="tf_prefix" value="robot1_tf" />
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> 
    <node pkg="tb_tables" name="fake_localization" type="fake_localiza2" output="screen"/>
    <include file="$(find tb_tables)/launch/simulation/includes/move_base_sim.launch.xml" />
  </group>      
</launch>

You can see in this launch files that I use a node named "odom_sim". As some people have noticed, namespaces in the kobuki gazebo plugin doesn't work for many topics (being odometry one of them), so I program this node (odom_sim) in order to ... (more)

edit flag offensive delete link

Comments

Hi Carlos, I have the same problem that you just fixed for your two turtlebots. I have two turtlebots I spawned them using the kobuki.launch.xml file that comes with turtlebot sim I am new on ROS Can you please tell me where can I find inside my folders the source code that you changed. Thanks alot

Robert1 ( 2014-04-14 13:48:24 -0600 )edit

add a comment

Problem with multiple navigation on Gazebo

4 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

Problem with multiple navigation on Gazebo edit

4 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

Problem with multiple navigation on Gazebo