Added the possibility to have simulations with multiple UAVs

cvg_sim_gazebo/launch/testworld_with_three_ardrone.launch

@@ -0,0 +1,99 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Include the world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find cvg_sim_gazebo)/worlds/ardrone_testworld.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+
+  <!-- send the robot XML to param server -->
+  <arg name="model_urdf" value="$(find cvg_sim_gazebo)/urdf/quadrotor_sensors.urdf.xacro"/>
+
+  <!-- Spawn 1st simulated quadrotor uav -->
+  <group ns="ardrone_1">  
+    <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model_urdf)'"/>
+    <node name="spawn_robot_ardrone" pkg="gazebo_ros" type="spawn_model"
+          args="-param robot_description
+                -urdf
+                -x 0.0 
+                -y 0
+                -z 0.5
+                -R 0
+                -P 0 
+                -Y 0
+                -model ardrone_1"
+           respawn="false" output="screen"/>
+
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+      <param name="publish_frequency" type="double" value="50.0" />
+      <param name="tf_prefix" value="ardrone_1" />
+    </node>
+    <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+      <param name="odometry_topic" value="ground_truth/state" />
+      <param name="frame_id" value="nav" />
+      <param name="tf_prefix" value="ardrone_1" />
+    </node>
+  </group> 
+
+
+  <!-- Spawn 2nd simulated quadrotor uav -->
+  <group ns="ardrone_2">
+    <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model_urdf)'"/>
+    <node name="spawn_robot_ardrone_2" pkg="gazebo_ros" type="spawn_model"
+          args="-param robot_description
+                -urdf
+                -x 2.0 
+                -y 0
+                -z 0.5
+                -R 0
+                -P 0 
+                -Y 0
+                -model ardrone_2"
+          respawn="false" output="screen"/>
+
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher_2" output="screen" >
+      <param name="publish_frequency" type="double" value="50.0" />
+      <param name="tf_prefix" value="ardrone_2" />
+    </node>
+
+    <node name="ground_truth_to_tf_2" pkg="message_to_tf" type="message_to_tf" output="screen">
+      <param name="odometry_topic" value="ground_truth/state" />
+      <param name="frame_id" value="nav" />
+      <param name="tf_prefix" value="ardrone_2" />
+    </node>
+  </group>
+
+  <!-- Spawn 3rd simulated quadrotor uav -->
+  <group ns="ardrone_3">
+    <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model_urdf)'"/>
+    <node name="spawn_robot_ardrone_3" pkg="gazebo_ros" type="spawn_model"
+          args="-param robot_description
+                -urdf
+                -x 1.0 
+                -y 0
+                -z 0.5
+                -R 0
+                -P 0 
+                -Y 0
+                -model ardrone_3"
+          respawn="false" output="screen"/>
+
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher_3" output="screen" >   
+      <param name="publish_frequency" type="double" value="50.0" />
+      <param name="tf_prefix" value="ardrone_3"/>
+    </node>
+
+    <node name="ground_truth_to_tf_3" pkg="message_to_tf" type="message_to_tf" output="screen">
+      <param name="odometry_topic" value="ground_truth/state" />
+      <param name="frame_id" value="nav" />
+      <param name="tf_prefix" value="ardrone_3"/>
+    </node> 
+  </group>
+
+</launch>

cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro

@@ -87,8 +87,8 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
 	  <cameraName>/${sim_name}/${name}</cameraName>
           <alwaysOn>true</alwaysOn>
           <updateRate>${update_rate}</updateRate>
-          <imageTopicName>/${sim_name}/${name}/image_raw</imageTopicName>
-          <cameraInfoTopicName>/${sim_name}/${name}/camera_info</cameraInfoTopicName>
+          <imageTopicName>${sim_name}/${name}/image_raw</imageTopicName>
+          <cameraInfoTopicName>${sim_name}/${name}/camera_info</cameraInfoTopicName>
           <frameName>ardrone_base_${name}cam</frameName>
         </plugin>
       </sensor>

cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h

@@ -117,6 +117,8 @@ class GazeboQuadrotorSimpleController : public ModelPlugin
 
   std::string link_name_;
   std::string namespace_;
+  std::string robot_namespace_;
+  std::string node_namespace_;
   std::string velocity_topic_;
   std::string imu_topic_;
   std::string state_topic_;

cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h

@@ -130,6 +130,8 @@ class GazeboQuadrotorStateController : public ModelPlugin
 
   std::string link_name_;
   std::string namespace_;
+  std::string robot_namespace_;
+  std::string node_namespace_;
   std::string velocity_topic_;
   std::string takeoff_topic_;
   std::string land_topic_;

cvg_sim_gazebo_plugins/src/gazebo_ros_gps.cpp

@@ -63,15 +63,7 @@ void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   else
     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
 
-  if (!_sdf->HasElement("bodyName"))
-  {
-    link = _model->GetLink();
-    link_name_ = link->GetName();
-  }
-  else {
-    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
-    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
-  }
+  link =  _model->GetChildLink("base_link");
 
   if (!link)
   {

cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp

@@ -99,15 +99,7 @@ void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   else
     robotNamespace = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
 
-  if (!_sdf->HasElement("bodyName"))
-  {
-    link = _model->GetLink();
-    linkName = link->GetName();
-  }
-  else {
-    linkName = _sdf->GetElement("bodyName")->Get<std::string>();
-    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(linkName));
-  }
+  link =  _model->GetChildLink("base_link");
 
   // assert that the body by linkName exists
   if (!link)

cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp

@@ -68,15 +68,7 @@ void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   else
     topic_ = _sdf->GetElement("topicName")->Get<std::string>();
 
-  if (!_sdf->HasElement("bodyName"))
-  {
-    link = _model->GetLink();
-    link_name_ = link->GetName();
-  }
-  else {
-    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
-    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
-  }
+  link =  _model->GetChildLink("base_link");
 
   if (!link)
   {

cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp

@@ -72,17 +72,29 @@ void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::Elemen
 
   // load parameters
   if (!_sdf->HasElement("robotNamespace"))
-    namespace_.clear();
+    robot_namespace_.clear();
   else
-    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
+    robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
+
+  namespace_ = robot_namespace_;
+
+  if (!_sdf->HasElement("node_namespace"))
+  {
+    node_namespace_.clear();
+  }
+  else
+  {
+    this->node_namespace_ = this->robot_namespace_ + _sdf->GetElement("node_namespace")->GetValueString() + "/";
+    namespace_ = namespace_ + node_namespace_;
+  }
 
   if (!_sdf->HasElement("topicName"))
     velocity_topic_ = "cmd_vel";
   else
     velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
 
   if (!_sdf->HasElement("navdataTopic"))
-    navdata_topic_ = "/ardrone/navdata";
+    navdata_topic_ = "navdata";
   else
     navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();
 
@@ -96,15 +108,7 @@ void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::Elemen
   else
     state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();
 
-  if (!_sdf->HasElement("bodyName"))
-  {
-    link = _model->GetLink();
-    link_name_ = link->GetName();
-  }
-  else {
-    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
-    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
-  }
+  link =  _model->GetChildLink("base_link");
 
   if (!link)
   {

cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp

@@ -50,32 +50,44 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element
 
   // load parameters
   if (!_sdf->HasElement("robotNamespace"))
-    namespace_.clear();
+    robot_namespace_.clear();
   else
-    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
+    robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
+
+  namespace_ = robot_namespace_;
+
+  if (!_sdf->HasElement("node_namespace"))
+  {
+    node_namespace_.clear();
+  }
+  else
+  {
+    this->node_namespace_ = this->robot_namespace_ + _sdf->GetElement("node_namespace")->GetValueString() + "/";
+    namespace_ = namespace_ + node_namespace_;
+  }
 
   if (!_sdf->HasElement("topicName"))
     velocity_topic_ = "cmd_vel";
   else
     velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
-
+  
   if (!_sdf->HasElement("takeoffTopic"))
-    takeoff_topic_ = "/ardrone/takeoff";
+    takeoff_topic_ = "takeoff";
   else
     takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get<std::string>();
 
-  if (!_sdf->HasElement("/ardrone/land"))
-    land_topic_ = "/ardrone/land";
+  if (!_sdf->HasElement("landTopic"))
+    land_topic_ = "land";
   else
     land_topic_ = _sdf->GetElement("landTopic")->Get<std::string>();
 
   if (!_sdf->HasElement("resetTopic"))
-    reset_topic_ = "/ardrone/reset";
+    reset_topic_ = "reset";
   else
     reset_topic_ = _sdf->GetElement("resetTopic")->Get<std::string>();
 
   if (!_sdf->HasElement("navdataTopic"))
-    navdata_topic_ = "/ardrone/navdata";
+    navdata_topic_ = "navdata";
   else
     navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();
 
@@ -94,15 +106,8 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element
   else
     state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();
 
-  if (!_sdf->HasElement("bodyName"))
-  {
-    link = _model->GetLink();
-    link_name_ = link->GetName();
-  }
-  else {
-    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
-    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
-  }
+
+  link =  _model->GetChildLink("base_link");
 
   if (!link)
   {
@@ -194,7 +199,7 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element
 
   // for camera control
   // switch camera server
-  std::string toggleCam_topic  = "ardrone/togglecam";
+  std::string toggleCam_topic  = "togglecam";
   ros::AdvertiseServiceOptions toggleCam_ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
     toggleCam_topic,
     boost::bind(&GazeboQuadrotorStateController::toggleCamCallback, this, _1,_2),
@@ -204,9 +209,9 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element
   toggleCam_service = node_handle_->advertiseService(toggleCam_ops);
 
   // camera image data
-  std::string cam_out_topic  = "/ardrone/image_raw";
-  std::string cam_front_in_topic = "/ardrone/front/image_raw";
-  std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
+  std::string cam_out_topic  = "image_raw";
+  std::string cam_front_in_topic = "front/image_raw";
+  std::string cam_bottom_in_topic = "bottom/image_raw";
   std::string in_transport = "raw";
 
   camera_it_ = new image_transport::ImageTransport(*node_handle_);
@@ -223,9 +228,9 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element
     ros::VoidPtr(), in_transport);
 
   // camera image data
-  std::string cam_info_out_topic  = "/ardrone/camera_info";
-  std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
-  std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";
+  std::string cam_info_out_topic  = "camera_info";
+  std::string cam_info_front_in_topic = "front/camera_info";
+  std::string cam_info_bottom_in_topic = "bottom/camera_info";
 
   camera_info_publisher_ = node_handle_->advertise<sensor_msgs::CameraInfo>(cam_info_out_topic,1);
 

cvg_sim_gazebo_plugins/urdf/quadrotor_sensors.urdf.xacro

@@ -18,7 +18,7 @@ xmlns:xacro="http://ros.org/wiki/xacro"
         <updateRate>100.0</updateRate>
         <bodyName>base_link</bodyName>
         <frameId>ardrone_base_link</frameId>
-        <topicName>/ardrone/imu</topicName>
+        <topicName>ardrone/imu</topicName>
         <rpyOffsets>0 0 0</rpyOffsets> <!-- deprecated -->
         <gaussianNoise>0</gaussianNoise>  <!-- deprecated -->
         <accelDrift>0.5 0.5 0.5</accelDrift>

message_to_tf/src/message_to_tf.cpp

@@ -7,6 +7,7 @@
 std::string g_odometry_topic;
 std::string g_pose_topic;
 std::string g_imu_topic;
+std::string g_tf_prefix;
 std::string g_frame_id;
 std::string g_footprint_frame_id;
 std::string g_position_frame_id;
@@ -122,12 +123,19 @@ int main(int argc, char** argv) {
   priv_nh.getParam("odometry_topic", g_odometry_topic);
   priv_nh.getParam("pose_topic", g_pose_topic);
   priv_nh.getParam("imu_topic", g_imu_topic);
+  priv_nh.getParam("tf_prefix", g_tf_prefix);
   priv_nh.getParam("frame_id", g_frame_id);
   priv_nh.getParam("footprint_frame_id", g_footprint_frame_id);
   priv_nh.getParam("position_frame_id", g_position_frame_id);
   priv_nh.getParam("stabilized_frame_id", g_stabilized_frame_id);
   priv_nh.getParam("child_frame_id", g_child_frame_id);
 
+  //add tf_prefix
+  g_footprint_frame_id.insert(0, g_tf_prefix);
+  g_position_frame_id.insert(0, g_tf_prefix);
+  g_stabilized_frame_id.insert(0, g_tf_prefix);
+  g_child_frame_id.insert(0, g_tf_prefix);
+
   br = new tf::TransformBroadcaster;
 
   ros::NodeHandle node;