Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hi...

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 publish the odometry information in an namespaced topic ("robot1/odom" & "robot2/odom"). For obtaining the information of the robot position, I use the "/gazebo/get_model_state" service. The code I use is this:

#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <control_msgs/JointControllerState.h>
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/ModelState.h>
#include <ros/param.h>
#include <boost/assign/list_of.hpp>

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub;
  ros::Rate r(25.0);

  while(n.ok()){

    ros::spinOnce(); // check for incoming messages

    //
    odom_pub    = n.advertise<nav_msgs::Odometry>("odom", 10);

    std::string name;
    ros::param::get( "robot_name", name);
    //std::cout << name << std::endl;

    // take pose info from gazebo
    ros::ServiceClient client = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    gazebo_msgs::GetModelState getmodelstate;

    client.waitForExistence();

    getmodelstate.request.model_name = name;
    client.call(getmodelstate);


    //odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = getmodelstate.response.pose.position.x;
    odom.pose.pose.position.y = getmodelstate.response.pose.position.y;
    odom.pose.pose.orientation.z = getmodelstate.response.pose.orientation.z;
    odom.pose.pose.orientation.w = getmodelstate.response.pose.orientation.w;
    odom.pose.covariance =  boost::assign::list_of(1e-1) (0)  (0)  (0)  (0)  (0)
                                                   (0) (1e-1) (0)  (0)  (0)  (0)
                                                   (0)   (0) (1e6) (0)  (0)  (0)
                                                   (0)   (0)  (0) (1e6) (0)  (0)
                                                   (0)   (0)  (0)  (0) (1e6) (0)
                                                   (0)   (0)  (0)  (0)  (0) (5e-2) ;
    //set the velocity
    odom.child_frame_id = "base_footprint"; //base_footprint  base_link
    odom.twist.twist.linear.x = getmodelstate.response.twist.linear.x;
    odom.twist.twist.angular.z = getmodelstate.response.twist.angular.z;


    //publish the message
    odom_pub.publish(odom);

    r.sleep();
  }
}

Now that we have odometry information of each robot, we can write a node that publish the "odom" TF transform (I called this node "tb_tf_broadcaster" in the launch file) The code is:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <ros/param.h>
#include <tf/tf.h>

void odom_callback(const nav_msgs::OdometryConstPtr& msg){
    // Publish /odom to base_footprint transform
  static tf::TransformBroadcaster br;
  tf::Transform BaseFootprintTransf;
  BaseFootprintTransf.setOrigin(tf::Vector3(msg->pose.pose.position.x,msg->pose.pose.position.y, 0.0));
  tf::Quaternion q;
  tf::quaternionMsgToTF(msg->pose.pose.orientation, q);
  BaseFootprintTransf.setRotation(q);

  std::string tf_name;
  ros::param::get( "tf_prefix", tf_name);
  std::string odom_ = std::string(tf_name) + "/odom";
  std::string base_footprint_ = std::string(tf_name) + "/base_footprint";

  br.sendTransform(tf::StampedTransform(BaseFootprintTransf, ros::Time::now(),odom_, base_footprint_));

  // Publish base_footprint transform to odom
  //static tf::TransformBroadcaster br2;
  //tf::Transform OdomTransf;
  //br2.sendTransform(tf::StampedTransform(OdomTransf, ros::Time::now(),"/origin", odom_));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "turtlebot_tf_broadcaster");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("odom", 100, &odom_callback);
  ros::spin();
  return 0;
};

Finally, if you want to simulate de AMCL localization, you can use instead the "Fake_localization" node. This node is computationally less expensive than the AMCL node but it also works perfectly with the rest of the navigation stack modules (move_base). As AMCL this node publish TF transformation between map and robot1 and odom

In my case i had to modified the original code in order to work with two robots

#include <ros/ros.h>
#include <ros/time.h>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <angles/angles.h>

#include "ros/console.h"

#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"

#include <iostream>

class FakeOdomNode
{
  public:
    FakeOdomNode(void)
    {
      m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1,true);
      m_particlecloudPub = m_nh.advertise<geometry_msgs::PoseArray>("particlecloud",1,true);
      m_tfServer = new tf::TransformBroadcaster();  
      m_tfListener = new tf::TransformListener();

      m_base_pos_received = false;

      ros::NodeHandle private_nh("~");
      private_nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
      private_nh.param("base_frame_id", base_frame_id_, std::string("base_link"));
      private_nh.param("global_frame_id", global_frame_id_, std::string("/map")); // "/map"
      private_nh.param("delta_x", delta_x_, 0.0);
      private_nh.param("delta_y", delta_y_, 0.0);
      private_nh.param("delta_yaw", delta_yaw_, 0.0);      
      private_nh.param("transform_tolerance", transform_tolerance_, 0.1);

      // get our tf prefix
      std::string tf_prefix = tf::getPrefixParam(private_nh);
      global_frame_id_ = tf::resolve(tf_prefix, global_frame_id_);
      base_frame_id_ = tf::resolve(tf_prefix, base_frame_id_);
      odom_frame_id_ = tf::resolve(tf_prefix, odom_frame_id_);

      m_particleCloud.header.stamp = ros::Time::now();
      m_particleCloud.header.frame_id = global_frame_id_;
      m_particleCloud.poses.resize(1);
      ros::NodeHandle nh;

      // messages (erase)
      ROS_INFO("odom_frame_id: %s", odom_frame_id_.c_str());
      ROS_INFO("base_frame_id: %s", base_frame_id_.c_str());
      ROS_INFO("global_frame_id: %s", global_frame_id_.c_str());

      m_offsetTf = tf::Transform(tf::createQuaternionFromRPY(0, 0, -delta_yaw_ ), tf::Point(-delta_x_, -delta_y_, 0.0));

      stuff_sub_ = nh.subscribe("odom", 100, &FakeOdomNode::stuffFilter, this); //base_pose_ground_truth
      filter_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh, "", 100); //""
      filter_ = new tf::MessageFilter<nav_msgs::Odometry>(*filter_sub_, *m_tfListener, base_frame_id_, 100);
      filter_->registerCallback(boost::bind(&FakeOdomNode::update, this, _1));


      // subscription to "2D Pose Estimate" from RViz:
      m_initPoseSub = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(nh, "initialpose", 1);
      m_initPoseFilter = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*m_initPoseSub, *m_tfListener, global_frame_id_, 1);
      m_initPoseFilter->registerCallback(boost::bind(&FakeOdomNode::initPoseReceived, this, _1));
    }

    ~FakeOdomNode(void)
    {
      if (m_tfServer)
        delete m_tfServer; 
    }


  private:
    ros::NodeHandle m_nh;
    ros::Publisher m_posePub;
    ros::Publisher m_particlecloudPub;
    message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseSub;
    tf::TransformBroadcaster       *m_tfServer;
    tf::TransformListener          *m_tfListener;
    tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseFilter;
    tf::MessageFilter<nav_msgs::Odometry>* filter_;
    ros::Subscriber stuff_sub_; 
    message_filters::Subscriber<nav_msgs::Odometry>* filter_sub_;

    double delta_x_, delta_y_, delta_yaw_;
    bool m_base_pos_received;
    double transform_tolerance_;

    nav_msgs::Odometry  m_basePosMsg;
    geometry_msgs::PoseArray      m_particleCloud;
    geometry_msgs::PoseWithCovarianceStamped      m_currentPos;
    tf::Transform m_offsetTf;

    //parameter for what odom to use
    std::string odom_frame_id_;
    std::string base_frame_id_;
    std::string global_frame_id_;

  public:
    void stuffFilter(const nav_msgs::OdometryConstPtr& odom_msg){
      //we have to do this to force the message filter to wait for transforms
      //from odom_frame_id_ to base_frame_id_ to be available at time odom_msg.header.stamp
      //really, the base_pose_ground_truth should come in with no frame_id b/c it doesn't make sense
      boost::shared_ptr<nav_msgs::Odometry> stuff_msg(new nav_msgs::Odometry);
      *stuff_msg = *odom_msg;
      stuff_msg->header.frame_id = odom_frame_id_;
      filter_->add(stuff_msg);
    }

    void update(const nav_msgs::OdometryConstPtr& message){
      //  Callback to register with tf::MessageFilter to be called when transforms are available
      tf::Pose txi;
      tf::poseMsgToTF(message->pose.pose, txi);
      txi = m_offsetTf * txi;

      tf::Stamped<tf::Pose> odom_to_map;
      try
      {
        m_tfListener->transformPose(odom_frame_id_, tf::Stamped<tf::Pose>(txi.inverse(), message->header.stamp, base_frame_id_), odom_to_map);
      }
      catch(tf::TransformException &e)
      {
        ROS_ERROR("Failed to transform to %s from %s: %s\n", odom_frame_id_.c_str(), base_frame_id_.c_str(), e.what());
        return;
      }

      m_tfServer->sendTransform(tf::StampedTransform(odom_to_map.inverse(),
                                                     message->header.stamp + ros::Duration(transform_tolerance_),
                                                     global_frame_id_, odom_frame_id_));

      tf::Pose current;
      tf::poseMsgToTF(message->pose.pose, current);

      //also apply the offset to the pose
      current = m_offsetTf * current;

      geometry_msgs::Pose current_msg;
      tf::poseTFToMsg(current, current_msg);

      // Publish localized pose
      m_currentPos.header = message->header;
      m_currentPos.header.frame_id = global_frame_id_;
      m_currentPos.pose.pose = current_msg;
      m_posePub.publish(m_currentPos);

      // The particle cloud is the current position. Quite convenient.
      m_particleCloud.header = m_currentPos.header;
      m_particleCloud.poses[0] = m_currentPos.pose.pose;
      m_particlecloudPub.publish(m_particleCloud);
    }

    void initPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
      tf::Pose pose;
      tf::poseMsgToTF(msg->pose.pose, pose);

      if (msg->header.frame_id != global_frame_id_){
        ROS_WARN("Frame ID of \"initialpose\" (%s) is different from the global frame %s", msg->header.frame_id.c_str(), global_frame_id_.c_str());
      }

      // set offset so that current pose is set to "initialpose"    
      tf::StampedTransform baseInMap;
      try{
        m_tfListener->lookupTransform(base_frame_id_, global_frame_id_, msg->header.stamp, baseInMap);
      } catch(tf::TransformException){
        ROS_WARN("Failed to lookup transform!");
        return;
      }

      tf::Transform delta = pose * baseInMap;
      m_offsetTf = delta * m_offsetTf;

    }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fake_localization");

  FakeOdomNode odom;

  ros::spin();

  return 0;
}

I hope that someone found this useful...

Greetings!