indigo changes

README.md

@@ -1,2 +1,49 @@
-tum_simulator ported to Hydro
+tum_simulator on Indigo
 =============
+
+These packages are used to simulate the flying robot Ardrone in ROS environment using gazebo simulator. Totally they are 4 packages. Their functions are descript as below:
+
+1. cvg_sim_gazebo: contains object models, sensor models, quadrocopter models, flying environment information and individual launch files for each objects and pure environment without any other objects.
+
+2. cvg_sim_gazebo_plugins: contains gazebo plugins for the quadrocopter model. quadrotor_simple_controller is used to control the robot motion and deliver navigation information, such as: /ardrone/navdata. Others are plugins for sensors in the quadrocopter, such as: IMU sensor, sonar sensor, GPS sensor.
+
+3. message_to_tf: is a package used to create a ros node, which transfers the ros topic /ground_truth/state to a /tf topic.
+
+4. cvg_sim_msgs: contains message forms for the simulator.
+
+Some packages are based on the tu-darmstadt-ros-pkg by Stefan Kohlbrecher, TU Darmstadt.
+
+How to install the simulator:
+
+1. Create a workspace for the simulator
+
+    ```
+    mkdir -p ~/tum_simulator_ws/src
+    cd  ~/tum_simulator_ws/src
+    catkin_init_workspace
+    ```
+2. Download dependencies
+
+    ```
+    git clone https://github.com/AutonomyLab/ardrone_autonomy.git	# The AR.Drone ROS driver
+    git clone https://github.com/occomco/tum_simulator.git
+    cd ..
+    rosdep install --from-paths src --ignore-src --rosdistro indigo -y
+    ```
+3. Build the simulator
+
+    ```
+    catkin_make
+    ```
+4. Source the environment
+
+    ```
+    source devel/setup.bash
+    ```
+How to run a simulation:
+
+1. Run a simulation by executing a launch file in cvg_sim_gazebo package:
+
+    ```
+    roslaunch cvg_sim_gazebo ardrone_testworld.launch
+    ```

cvg_sim_gazebo/urdf/quadrotor.urdf.xacro

@@ -7,7 +7,7 @@
     xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
 
     <!-- Included URDF Files -->
-    <include filename="$(find cvg_sim_gazebo)/urdf/quadrotor_base.urdf.xacro" />
+    <xacro:include filename="$(find cvg_sim_gazebo)/urdf/quadrotor_base.urdf.xacro" />
 
     <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
     <quadrotor_base_macro />

cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro

@@ -6,7 +6,7 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
 xmlns:xacro="http://ros.org/wiki/xacro"
 >
 
-  <include filename="$(find cvg_sim_gazebo_plugins)/urdf/quadrotor_plugins.urdf.xacro" />
+  <xacro:include filename="$(find cvg_sim_gazebo_plugins)/urdf/quadrotor_plugins.urdf.xacro" />
   <property name="pi" value="3.1415926535897931" />
 
   <!-- Main quadrotor link -->

cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro

@@ -9,36 +9,36 @@ xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
     <property name="M_PI" value="3.1415926535897931" />
 
     <!-- Included URDF Files -->
-    <include filename="$(find cvg_sim_gazebo)/urdf/quadrotor_base.urdf.xacro" />
+    <xacro:include filename="$(find cvg_sim_gazebo)/urdf/quadrotor_base.urdf.xacro" />
 
     <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
     <quadrotor_base_macro />
 
 
     <!-- Sonar height sensor -->
-    <include filename="$(find cvg_sim_gazebo)/urdf/sensors/sonar_sensor.urdf.xacro" />
+    <xacro:include filename="$(find cvg_sim_gazebo)/urdf/sensors/sonar_sensor.urdf.xacro" />
     <xacro:sonar_sensor name="sonar" parent="base_link" ros_topic="sonar_height" update_rate="10" min_range="0.01" max_range="3.0" field_of_view="${40*M_PI/180}" ray_count="3">
       <origin xyz="-0.15 0.0 0.0" rpy="0 ${90*M_PI/180} 0"/>
     </xacro:sonar_sensor>
 
 
     <!-- Hokuyo UTM-30LX mounted upside down below the quadrotor body 
-    <include filename="$(find cvg_sim_gazebo)/urdf/sensors/hokuyo_utm30lx.urdf.xacro" />
+    <xacro:include filename="$(find cvg_sim_gazebo)/urdf/sensors/hokuyo_utm30lx.urdf.xacro" />
     <xacro:hokuyo_utm30lx name="laser0" parent="base_link" ros_topic="scan" update_rate="40" ray_count="1081" min_angle="135" max_angle="-135">
       <origin xyz="0.0 0.0 0.08" rpy="${M_PI} 0 0"/>
     </xacro:hokuyo_utm30lx>-->
 
     <!-- The following two cameras should be united to one! -->
     <!-- Forward facing camera -->
-    <include filename="$(find cvg_sim_gazebo)/urdf/sensors/generic_camera.urdf.xacro" />
+    <xacro:include filename="$(find cvg_sim_gazebo)/urdf/sensors/generic_camera.urdf.xacro" />
     <xacro:generic_camera name="front" sim_name="ardrone" parent="base_link" update_rate="60" res_x="640" res_y="360" image_format="R8G8B8" hfov="${81*M_PI/180}">
       <origin xyz="0.21 0.0 0.01" rpy="0 0 0"/>
     </xacro:generic_camera>
 
     <!-- Downward facing camera -->
-    <include filename="$(find cvg_sim_gazebo)/urdf/sensors/generic_camera.urdf.xacro" />
+    <xacro:include filename="$(find cvg_sim_gazebo)/urdf/sensors/generic_camera.urdf.xacro" />
     <xacro:generic_camera name="bottom" sim_name="ardrone" parent="base_link" update_rate="60" res_x="640" res_y="360" image_format="R8G8B8" hfov="${81*M_PI/180}">
-      <origin xyz="0.15 0.0 0.0" rpy="0 -${M_PI/2} 0"/>
+      <origin xyz="0.15 0.0 0.0" rpy="0 ${M_PI/2} 0"/>
     </xacro:generic_camera>
 
 </robot>

cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro

@@ -93,7 +93,7 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
         </plugin>
       </sensor>
       <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
+      <material>Gazebo/Blue</material>
     </gazebo>
     </xacro:macro>
   </robot>

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/diffdrive_plugin_6w.h

@@ -27,13 +27,13 @@
 
 #include <map>
 
-#include "common/Plugin.hh"
-#include "common/Time.hh"
-//#include "physics/physics.h"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Time.hh"
+//#include "gazebo/physics/physics.h"
 //#include "transport/TransportTypes.hh"
 //#include "msgs/MessageTypes.hh"
-//#include "common/Time.hh"
-//#include "common/Events.hh"
+//#include "gazebo/common/Time.hh"
+//#include "gazebo/common/Events.hh"
 
 // ROS 
 #include <ros/ros.h>

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_gps.h

@@ -29,8 +29,8 @@
 #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_GPS_H
 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_GPS_H
 
-#include "common/Plugin.hh"
-#include "common/Time.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Time.hh"
 
 #include <ros/ros.h>
 #include <sensor_msgs/NavSatFix.h>

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_imu.h

@@ -64,8 +64,8 @@
 #include <ros/advertise_options.h>
 #endif
 
-#include "common/Plugin.hh"
-#include "common/Time.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Time.hh"
 
 #include <ros/ros.h>
 #include <boost/thread/mutex.hpp>

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_magnetic.h

@@ -29,8 +29,8 @@
 #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_MAGNETIC_H
 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_MAGNETIC_H
 
-#include "common/Plugin.hh"
-#include "common/Time.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Time.hh"
 
 #include <ros/ros.h>
 #include <geometry_msgs/Vector3Stamped.h>

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_sonar.h

@@ -29,8 +29,8 @@
 #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H
 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H
 
-#include "common/Plugin.hh"
-#include "common/Time.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Time.hh"
 
 #include <ros/callback_queue.h>
 #include <ros/ros.h>

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/reset_plugin.h

@@ -29,7 +29,7 @@
 #ifndef HECTOR_GAZEBO_PLUGINS_RESET_PLUGIN_H
 #define HECTOR_GAZEBO_PLUGINS_RESET_PLUGIN_H
 
-#include "common/Plugin.hh"
+#include "gazebo/common/Plugin.hh"
 
 #include <ros/ros.h>
 

cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h

@@ -30,7 +30,7 @@
 #define HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
 
 #include <sdf/sdf.hh>
-#include <math/gzmath.hh>
+#include <gazebo/math/gzmath.hh>
 
 namespace gazebo {
 
@@ -97,10 +97,10 @@ void SensorModel_<T>::Load(sdf::ElementPtr _sdf, const std::string& prefix)
     _gaussian_noise  = _sdf->GetElement(prefix + "GaussianNoise");
   }
 
-  if (_offset          && !_offset->GetValue()->Get(offset))                   offset = _offset->GetValueDouble();
-  if (_drift           && !_drift->GetValue()->Get(drift))                     drift = _drift->GetValueDouble();
-  if (_drift_frequency && !_drift_frequency->GetValue()->Get(drift_frequency)) drift_frequency = _drift_frequency->GetValueDouble();
-  if (_gaussian_noise  && !_gaussian_noise->GetValue()->Get(gaussian_noise))   gaussian_noise = _gaussian_noise->GetValueDouble();
+  if (_offset          && !_offset->GetValue()->Get(offset))                   offset = _offset->Get<double>();
+  if (_drift           && !_drift->GetValue()->Get(drift))                     drift = _drift->Get<double>();
+  if (_drift_frequency && !_drift_frequency->GetValue()->Get(drift_frequency)) drift_frequency = _drift_frequency->Get<double>();
+  if (_gaussian_noise  && !_gaussian_noise->GetValue()->Get(gaussian_noise))   gaussian_noise = _gaussian_noise->Get<double>();
 }
 
 namespace {

cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h

@@ -46,7 +46,7 @@
 #define HECTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H
 
 #include "gazebo/gazebo.hh"
-#include "common/Plugin.hh"
+#include "gazebo/common/Plugin.hh"
 
 #include <ros/callback_queue.h>
 #include <ros/ros.h>

cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h

@@ -15,7 +15,7 @@
 #define HECTOR_GAZEBO_PLUGINS_quadrotor_state_controller_H
 
 #include "gazebo/gazebo.hh"
-#include "common/Plugin.hh"
+#include "gazebo/common/Plugin.hh"
 
 #include <ros/callback_queue.h>
 #include <ros/ros.h>
@@ -140,6 +140,14 @@ class GazeboQuadrotorStateController : public ModelPlugin
   std::string sonar_topic_;
   std::string state_topic_;
 
+  std::string cam_out_topic_;
+  std::string cam_front_in_topic_;
+  std::string cam_bottom_in_topic_;
+
+  std::string cam_info_out_topic_;
+  std::string cam_info_front_in_topic_;
+  std::string cam_info_bottom_in_topic_;
+
   // extra parameters for robot control.
   bool m_isFlying;
   bool m_takeoff;

cvg_sim_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h

@@ -30,7 +30,7 @@
 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_BARO_H
 
 #include "gazebo/gazebo.hh"
-#include "common/Plugin.hh"
+#include "gazebo/common/Plugin.hh"
 
 #include <ros/ros.h>
 #ifdef USE_MAV_MSGS

cvg_sim_gazebo_plugins/src/diffdrive_plugin_6w.cpp

@@ -34,8 +34,8 @@
 #include <assert.h>
 
 #include <hector_gazebo_plugins/diffdrive_plugin_6w.h>
-#include "common/Events.hh"
-#include "physics/physics.hh"
+#include "gazebo/common/Events.hh"
+#include "gazebo/physics/physics.hh"
 
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
@@ -65,7 +65,7 @@ DiffDrivePlugin6W::DiffDrivePlugin6W()
 // Destructor
 DiffDrivePlugin6W::~DiffDrivePlugin6W()
 {
-  event::Events::DisconnectWorldUpdateStart(updateConnection);
+  event::Events::DisconnectWorldUpdateBegin(updateConnection);
   delete transform_broadcaster_;
   rosnode_->shutdown();
   callback_queue_thread_.join();
@@ -81,21 +81,21 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   if (!_sdf->HasElement("robotNamespace"))
     robotNamespace.clear();
   else
-    robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
+    robotNamespace = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
 
   if (!_sdf->HasElement("topicName"))
     topicName = "cmd_vel";
   else
-    topicName = _sdf->GetElement("topicName")->GetValueString();
+    topicName = _sdf->GetElement("topicName")->Get<std::string>();
 
   if (!_sdf->HasElement("bodyName"))
   {
     link = _model->GetLink();
     linkName = link->GetName();
   }
   else {
-    linkName = _sdf->GetElement("bodyName")->GetValueString();
-    link = boost::shared_dynamic_cast<physics::Link>(world->GetEntity(_sdf->GetElement("bodyName")->GetValueString()));
+    linkName = _sdf->GetElement("bodyName")->Get<std::string>();
+    link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(_sdf->GetElement("bodyName")->Get<std::string>()));
   }
 
   // assert that the body by linkName exists
@@ -105,12 +105,12 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
     return;
   }
 
-  if (_sdf->HasElement("frontLeftJoint"))  joints[FRONT_LEFT]  = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->GetValueString());
-  if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->GetValueString());
-  if (_sdf->HasElement("midLeftJoint"))    joints[MID_LEFT]    = _model->GetJoint(_sdf->GetElement("midLeftJoint")->GetValueString());
-  if (_sdf->HasElement("midRightJoint"))   joints[MID_RIGHT]   = _model->GetJoint(_sdf->GetElement("midRightJoint")->GetValueString());
-  if (_sdf->HasElement("rearLeftJoint"))   joints[REAR_LEFT]   = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->GetValueString());
-  if (_sdf->HasElement("rearRightJoint"))  joints[REAR_RIGHT]  = _model->GetJoint(_sdf->GetElement("rearRightJoint")->GetValueString());
+  if (_sdf->HasElement("frontLeftJoint"))  joints[FRONT_LEFT]  = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->Get<std::string>());
+  if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->Get<std::string>());
+  if (_sdf->HasElement("midLeftJoint"))    joints[MID_LEFT]    = _model->GetJoint(_sdf->GetElement("midLeftJoint")->Get<std::string>());
+  if (_sdf->HasElement("midRightJoint"))   joints[MID_RIGHT]   = _model->GetJoint(_sdf->GetElement("midRightJoint")->Get<std::string>());
+  if (_sdf->HasElement("rearLeftJoint"))   joints[REAR_LEFT]   = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->Get<std::string>());
+  if (_sdf->HasElement("rearRightJoint"))  joints[REAR_RIGHT]  = _model->GetJoint(_sdf->GetElement("rearRightJoint")->Get<std::string>());
 
   if (!joints[FRONT_LEFT])  ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front left joint");
   if (!joints[FRONT_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front right joint");
@@ -122,17 +122,17 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   if (!_sdf->HasElement("wheelSeparation"))
     wheelSep = 0.34;
   else
-    wheelSep = _sdf->GetElement("wheelSeparation")->GetValueDouble();
+    wheelSep = _sdf->GetElement("wheelSeparation")->Get<double>();
 
   if (!_sdf->HasElement("wheelDiameter"))
     wheelDiam = 0.15;
   else
-    wheelDiam = _sdf->GetElement("wheelDiameter")->GetValueDouble();
+    wheelDiam = _sdf->GetElement("wheelDiameter")->Get<double>();
 
   if (!_sdf->HasElement("torque"))
     torque = 10.0;
   else
-    torque = _sdf->GetElement("torque")->GetValueDouble();
+    torque = _sdf->GetElement("torque")->Get<double>();
 
   // start ros node
   if (!ros::isInitialized())