tum-vision · surajmahangade · May 26, 2020 · May 26, 2020 · May 26, 2020 · May 27, 2020
diff --git a/README.md b/README.md
@@ -1,2 +1,21 @@
-tum_simulator ported to Hydro
-=============
+## tum_simulator ported to Ros Melodic and Gazebo 9
+The original code was written in hydro which can be found here :
+https://github.com/tum-vision/tum_simulator.git
+This is a ros melodic version of the code.
+Tested on Ubuntu 18.04.1, ROS Melodic and using Gazebo 9.
+How to run a simulation:
+1. create a package
+* `mkdir -p catkin_ws/src/`
+* `cd catkin_ws/src/`
+* `catkin_init_workspace`
+2. Clone the required repositories
+* `git clone https://github.com/surajmahangade/tum_simulator.git`
+* `git clone https://github.com/dsapandora/ardrone_autonomy.git`
+3. Build
+* `cd ~/catkin_ws`
+* `catkin_make`
+4. Run the simulation
+Run a simulation by executing a launch file in cvg_sim_gazebo package: 
+
+* `roslaunch cvg_sim_gazebo ardrone_testworld.launch`
+
diff --git a/cvg_sim_gazebo/launch/spawn_quadrotor.launch b/cvg_sim_gazebo/launch/spawn_quadrotor.launch
@@ -10,7 +10,7 @@
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- send the robot XML to param server -->
-   <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)'" />
+   <param name="robot_description" command="$(find xacro)/xacro '$(arg model)'" />
 
    <!-- push robot_description to factory and spawn robot in gazebo -->
    <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
@@ -26,7 +26,7 @@
      respawn="false" output="screen"/>
 
    <!-- start robot state publisher -->
-   <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" 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" type="string" value="" />
    </node>

diff --git a/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro b/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 />

diff --git a/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro b/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 -->

diff --git a/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro b/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>

diff --git a/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro b/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>

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/diffdrive_plugin_6w.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/diffdrive_plugin_6w.h
@@ -27,14 +27,16 @@
 
 #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"
+#include "gazebo/common/Events.hh"
+#include "gazebo/physics/physics.hh"
+//#include <physics/physics.hh>
 // ROS 
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
@@ -50,6 +52,7 @@
 #include <boost/thread.hpp>
 #include <boost/bind.hpp>
 
+
 namespace gazebo
 {
 
@@ -63,10 +66,10 @@ class DiffDrivePlugin6W : public ModelPlugin
 protected:
   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
   virtual void Reset();
-  virtual void Update();
+  virtual void OnUpdate(const gazebo::common::UpdateInfo &info);
 
 private:
-  void publish_odometry();
+  void publish_odometry(const gazebo::common::UpdateInfo &info);
   void GetPositionCmd();
 
   physics::LinkPtr link;

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_gps.h b/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>
@@ -49,7 +49,7 @@ class GazeboRosGps : public ModelPlugin
 protected:
   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
   virtual void Reset();
-  virtual void Update();
+  virtual void OnUpdate(const gazebo::common::UpdateInfo &info);
 
 private:
   /// \brief The parent World

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_imu.h b/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>
@@ -88,7 +88,7 @@ namespace gazebo
    protected:
       virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
       virtual void Reset();
-      virtual void Update();
+      virtual void OnUpdate(const gazebo::common::UpdateInfo &info);
 
    private:
       /// \brief The parent World
@@ -126,12 +126,12 @@ namespace gazebo
       common::Time update_period;
 
       /// \brief save current body/physics state
-      math::Quaternion orientation;
-      math::Vector3 velocity;
-      math::Vector3 accel;
-      math::Vector3 rate;
-      math::Vector3 gravity;
-      math::Vector3 gravity_body;
+      ignition::math::Quaterniond orientation;
+      ignition::math::Vector3d velocity;
+      ignition::math::Vector3d accel;
+      ignition::math::Vector3d rate;
+      ignition::math::Vector3d gravity;
+      ignition::math::Vector3d gravity_body;
 
       /// \brief Gaussian noise generator
       double GaussianKernel(double mu,double sigma);

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_magnetic.h b/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>
@@ -48,7 +48,7 @@ class GazeboRosMagnetic : public ModelPlugin
 protected:
   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
   virtual void Reset();
-  virtual void Update();
+  virtual void OnUpdate(const gazebo::common::UpdateInfo &info);
 
 private:
   /// \brief The parent World
@@ -61,7 +61,7 @@ class GazeboRosMagnetic : public ModelPlugin
   ros::Publisher publisher_;
 
   geometry_msgs::Vector3Stamped magnetic_field_;
-  gazebo::math::Vector3 magnetic_field_world_;
+  ignition::math::Vector3d magnetic_field_world_;
 
   std::string namespace_;
   std::string topic_;

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_sonar.h b/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>
@@ -50,7 +50,7 @@ class GazeboRosSonar : public SensorPlugin
 protected:
   virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
   virtual void Reset();
-  virtual void Update();
+  virtual void OnUpdate(const gazebo::common::UpdateInfo &info);
 
 private:
   /// \brief The parent World

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/reset_plugin.h b/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>
 

diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h
@@ -30,7 +30,11 @@
 #define HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H
 
 #include <sdf/sdf.hh>
-#include <math/gzmath.hh>
+//#include <gazebo/math/gzmath.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <ignition/math/Vector3.hh>
 
 namespace gazebo {
 
@@ -97,10 +101,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 {
@@ -139,11 +143,11 @@ double SensorModel_<double>::update(double dt)
 }
 
 template <>
-math::Vector3 SensorModel_<math::Vector3>::update(double dt)
+ignition::math::Vector3d SensorModel_<ignition::math::Vector3d>::update(double dt)
 {
-  current_error_.x = SensorModelInternalUpdate(current_drift_.x, drift.x, drift_frequency.x, offset.x, gaussian_noise.x, dt);
-  current_error_.y = SensorModelInternalUpdate(current_drift_.y, drift.y, drift_frequency.y, offset.y, gaussian_noise.y, dt);
-  current_error_.z = SensorModelInternalUpdate(current_drift_.z, drift.z, drift_frequency.z, offset.z, gaussian_noise.z, dt);
+  current_error_.X(SensorModelInternalUpdate(current_drift_.X(), drift.X(), drift_frequency.X(), offset.X(), gaussian_noise.X(), dt));
+  current_error_.Y(SensorModelInternalUpdate(current_drift_.Y(), drift.Y(), drift_frequency.Y(), offset.Y(), gaussian_noise.Y(), dt));
+  current_error_.Y(SensorModelInternalUpdate(current_drift_.Z(), drift.Z(), drift_frequency.Z(), offset.Z(), gaussian_noise.Z(), dt));
   return current_error_;
 }
 
@@ -155,7 +159,7 @@ void SensorModel_<T>::reset(const T& value)
 }
 
 typedef SensorModel_<double> SensorModel;
-typedef SensorModel_<math::Vector3> SensorModel3;
+typedef SensorModel_<ignition::math::Vector3d> SensorModel3;
 
 }
 

diff --git a/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h b/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>
@@ -112,8 +112,8 @@ class GazeboQuadrotorSimpleController : public ModelPlugin
   void NavdataCallback(const ardrone_autonomy::NavdataConstPtr& msg);
 
   ros::Time state_stamp;
-  math::Pose pose;
-  math::Vector3 euler, velocity, acceleration, angular_velocity;
+  ignition::math::Pose3d pose;
+  ignition::math::Vector3d euler, velocity, acceleration, angular_velocity;
 
   std::string link_name_;
   std::string namespace_;
@@ -155,7 +155,7 @@ class GazeboQuadrotorSimpleController : public ModelPlugin
     PIDController velocity_z;
   } controllers_;
 
-  math::Vector3 inertia;
+  ignition::math::Vector3d inertia;
   double mass;
 
   /// \brief save last_time

diff --git a/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h b/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>
@@ -120,8 +120,8 @@ class GazeboQuadrotorStateController : public ModelPlugin
   bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response);
 
   ros::Time state_stamp;
-  math::Pose pose;
-  math::Vector3 euler, velocity, acceleration, angular_velocity;
+  ignition::math::Pose3d pose;
+  ignition::math::Vector3d euler, velocity, acceleration, angular_velocity;
   double robot_altitude;
   // 0: Unknown, 1: Init, 2: Landed, 3: Flying, 4: Hovering, 5: Test
   // 6: Taking off, 7: Goto Fix Point, 8: Landing, 9: Looping

diff --git a/cvg_sim_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h b/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
@@ -54,7 +54,7 @@ class GazeboRosBaro : public ModelPlugin
 protected:
   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
   virtual void Reset();
-  virtual void Update();
+  virtual void OnUpdate(const gazebo::common::UpdateInfo &info);
 
 private:
   /// \brief The parent World