tum-vision · angelsantamaria · Feb 10, 2017 · Feb 10, 2017 · Feb 10, 2017 · Feb 10, 2017
diff --git a/README.md b/README.md
@@ -1,2 +1,45 @@
-tum_simulator ported to Hydro
-=============
+## tum_simulator ported to Kinetic.  
+
+Tested on Ubuntu 16.04.1, ROS Kinetic and using Gazebo 7.  
+
+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 barometer 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 run a simulation:  
+
+0. Install dependencies:  
+
+    * Hector quadrotor: As an example you can install all hector suite running `sudo apt-get install ros-<your_version>-hector-*`  
+    * ardrone_autonomy: run udo apt-get install ros-<your_version>-ardrone_autonomy`  
+
+1. Download this `tum_simulator` package.  
+
+2. Build packages with `catkin_make` in the workspace root folder.  
+
+3. Start a ros master by typing the following command in console: `roscore`  
+
+4. Run a simulation by executing a launch file in cvg_test_sim package: `roslaunch cvg_sim_gazebo ardrone_testworld.launch`  
+
+5. You can manipulate the quadrocopter in gazebo simulator by downloading the ardrone_helpers package and using a joy-stick after calling this command:`roslaunch ardrone_joystick teleop.launch`  
+
+
+**Information for manipulation via joystick:**
+
+1. The `L1` button starts the quadrocopter. It also works as a deadman button so that the robot will land if you release it during flight.  
+
+2. The left stick can be used to control the vx/vy-velocity. Keep in mind that these velocities are given in the local frame of the drone!  
+
+3. The right stick controls the yaw-rate and the altitude.  
+
+4. The select button can be used to switch between the two cameras. This can also be done by executing `rosservice call /ardrone/togglecam`.  
+
diff --git a/cvg_sim_gazebo/launch/empty_world.launch b/cvg_sim_gazebo/launch/empty_world.launch
@@ -1,8 +1,8 @@
 <launch>
   <param name="/use_sim_time" value="true" />
-  <node name="empty_world_server" pkg="gazebo" type="gazebo" args="$(find cvg_sim_gazebo)/worlds/empty.world" respawn="false" output="screen">
+  <node name="empty_world_server" pkg="gazebo_ros" type="gazebo" args="$(find cvg_sim_gazebo)/worlds/empty.world" respawn="false" output="screen">
 
   </node>
-  <node name="gazebo_gui" pkg="gazebo" type="gui" respawn="false" output="screen"/>
+  <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
 </launch>
 
diff --git a/cvg_sim_gazebo/launch/land_station1.launch b/cvg_sim_gazebo/launch/land_station1.launch
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
 <launch>
     <param name="land_station1" textfile="$(find cvg_sim_gazebo)/objects/station_marker1.model" /> 
-    <node name="spawn_station1" pkg="gazebo" type="spawn_model" args="-param land_station1 -gazebo -model land_station1 -x 0.0 -y 2.0 -z 0.0" output="screen" /> 
+    <node name="spawn_station1" pkg="gazebo_ros" type="spawn_model" args="-param land_station1 -gazebo -model land_station1 -x 0.0 -y 2.0 -z 0.0" output="screen" /> 
 </launch>
diff --git a/cvg_sim_gazebo/launch/land_station2.launch b/cvg_sim_gazebo/launch/land_station2.launch
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
 <launch>
     <param name="land_station2" textfile="$(find cvg_sim_gazebo)/objects/station_marker2.model" /> 
-    <node name="spawn_station2" pkg="gazebo" type="spawn_model" args="-param land_station2 -gazebo -model land_station2  -x 0.0 -y 8.0 -z 5.0 -R 1.57 -P 0.0 -Y 0.0" output="screen" /> 
+    <node name="spawn_station2" pkg="gazebo_ros" type="spawn_model" args="-param land_station2 -gazebo -model land_station2  -x 0.0 -y 8.0 -z 5.0 -R 1.57 -P 0.0 -Y 0.0" output="screen" /> 
 </launch>
diff --git a/cvg_sim_gazebo/launch/rolling_landscape_120m.launch b/cvg_sim_gazebo/launch/rolling_landscape_120m.launch
@@ -4,10 +4,10 @@
 
   <param name="/use_sim_time" value="true" />
 
-  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find cvg_sim_gazebo)/worlds/rolling_landscape_120m.world" respawn="false" output="screen"/>
+  <node name="gazebo" pkg="gazebo_ros" type="gazebo" args="$(find cvg_sim_gazebo)/worlds/rolling_landscape_120m.world" respawn="false" output="screen"/>
     <!-- start gui -->
   <group if="$(arg gui)">
-    <node name="gazebo_gui" pkg="gazebo" type="gui" respawn="false" output="screen" required="true"/>
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen" required="true"/>
   </group>
 
 </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)' --inorder" />
 
    <!-- 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/launch/spawn_quadrotor_2.launch b/cvg_sim_gazebo/launch/spawn_quadrotor_2.launch
@@ -4,10 +4,10 @@
  <launch>
    <arg name="model" default="$(find cvg_sim_gazebo)/urdf/quadrotor.urdf.xacro"/>
    <!-- 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)' --inorder" />
 
    <!-- push robot_description to factory and spawn robot in gazebo -->
-   <node name="spawn_robot" pkg="gazebo" type="spawn_model"
+   <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
    args="-param robot_description
    -urdf
    -x -2.5
@@ -18,7 +18,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/launch/test.launch b/cvg_sim_gazebo/launch/test.launch
@@ -4,10 +4,10 @@
 
   <param name="/use_sim_time" value="true" />
 
-  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find cvg_sim_gazebo)/worlds/tum_kitchen.world" respawn="false" output="screen"/>
+  <node name="gazebo" pkg="gazebo_ros" type="gazebo" args="$(find cvg_sim_gazebo)/worlds/tum_kitchen.world" respawn="false" output="screen"/>
     <!-- start gui -->
   <group if="$(arg gui)">
-    <node name="gazebo_gui" pkg="gazebo" type="gui" respawn="false" output="screen" required="true"/>
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen" required="true"/>
   </group>
 
 </launch>

diff --git a/cvg_sim_gazebo/package.xml b/cvg_sim_gazebo/package.xml
@@ -42,7 +42,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>gazebo_ros</build_depend>
   <run_depend>gazebo_ros</run_depend>
-  <run_depend>cvg_sim_gazebo_plugins</run_depend>
+  <!--<run_depend>cvg_sim_gazebo_plugins</run_depend>-->
 
   <export>
     <gazebo gazebo_media_path="${prefix}" />

diff --git a/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro
@@ -3,11 +3,10 @@
 <robot name="quadrotor"
     xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
     xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-    xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+xmlns:xacro="http://ros.org/wiki/xacro">
 
     <!-- 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
@@ -1,17 +1,22 @@
 <?xml version="1.0"?>
 
 <robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
-xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-xmlns:xacro="http://ros.org/wiki/xacro"
->
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <include filename="$(find cvg_sim_gazebo_plugins)/urdf/quadrotor_plugins.urdf.xacro" />
-  <property name="pi" value="3.1415926535897931" />
+  <xacro:include filename="$(find cvg_sim_gazebo_plugins)/urdf/quadrotor_plugins.urdf.xacro" />
 
   <!-- Main quadrotor link -->
   <xacro:macro name="quadrotor_base_macro">
-    <link name="base_link">
+
+    <!-- Dump Link with no inertia -->
+    <link name="base_link"/>
+    <joint name="body_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="body_link"/>
+    </joint>
+
+    <link name="body_link">
       <inertial>
         <mass value="1.477" />
         <origin xyz="0 0 0" />
@@ -33,9 +38,7 @@ xmlns:xacro="http://ros.org/wiki/xacro"
       </collision>
     </link>
 
-
-
-    <gazebo reference="base_link">
+    <gazebo reference="body_link">
       <turnGravityOff>false</turnGravityOff>
     </gazebo>
 

diff --git a/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro
@@ -3,42 +3,41 @@
 <robot name="quadrotor_hokuyo_utm30lx"
 xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
 xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+xmlns:xacro="http://ros.org/wiki/xacro">
 
-    <property name="M_PI" value="3.1415926535897931" />
+    <xacro: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
@@ -2,15 +2,13 @@
 
 <robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
 xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-  xmlns:xacro="http://ros.org/wiki/xacro"
-  >  
+xmlns:xacro="http://ros.org/wiki/xacro">  
 
-    <property name="M_PI" value="3.1415926535897931" />
+    <xacro:property name="M_PI" value="3.1415926535897931" />
 
     <xacro:macro name="generic_camera" params="name sim_name parent *origin update_rate res_x res_y image_format hfov">
       <joint name="${name}_joint" type="fixed">
-        <insert_block name="origin" />
+        <xacro:insert_block name="origin" />
         <parent link="${parent}"/>
         <child link="${name}_link"/>
       </joint>
@@ -92,7 +90,7 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
           <frameName>ardrone_base_${name}cam</frameName>
         </plugin>
       </sensor>
-      <turnGravityOff>true</turnGravityOff>
+      <turnGravityOff>false</turnGravityOff>
       <material>PR2/Blue</material>
     </gazebo>
     </xacro:macro>

diff --git a/cvg_sim_gazebo/urdf/sensors/hokuyo_utm30lx.urdf.xacro b/cvg_sim_gazebo/urdf/sensors/hokuyo_utm30lx.urdf.xacro
@@ -2,12 +2,11 @@
 
 <robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
 xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-  xmlns:xacro="http://ros.org/wiki/xacro"
-  >  
+xmlns:xacro="http://ros.org/wiki/xacro"> 
+
     <xacro:macro name="hokuyo_utm30lx" params="name parent *origin ros_topic update_rate ray_count min_angle max_angle">
       <joint name="${name}_joint" type="fixed">
-	<insert_block name="origin" />
+	<xacro:insert_block name="origin" />
 	<parent link="${parent}"/>
 	<child link="${name}_frame"/>
       </joint>

diff --git a/cvg_sim_gazebo/urdf/sensors/sonar_sensor.urdf.xacro b/cvg_sim_gazebo/urdf/sensors/sonar_sensor.urdf.xacro
@@ -2,9 +2,8 @@
 
 <robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
 xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
-xmlns:xacro="http://ros.org/wiki/xacro"
->
+xmlns:xacro="http://ros.org/wiki/xacro">
+
   <xacro:macro name="sonar_sensor" params="name parent *origin ros_topic update_rate min_range max_range field_of_view ray_count">
     <joint name="${name}_joint" type="fixed">
       <xacro:insert_block name="origin" />

diff --git a/cvg_sim_gazebo_plugins/CMakeLists.txt b/cvg_sim_gazebo_plugins/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(cvg_sim_gazebo_plugins)
 
+set (CMAKE_CXX_STANDARD 11)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -86,7 +88,10 @@ generate_messages(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-  LIBRARIES diffdrive_plugin_6w reset_plugin hector_gazebo_ros_imu hector_gazebo_ros_magnetic hector_gazebo_ros_gps hector_gazebo_ros_sonar hector_gazebo_ros_baro hector_gazebo_quadrotor_simple_controller hector_gazebo_quadrotor_state_controller
+  LIBRARIES 
+  reset_plugin 
+  hector_gazebo_ros_baro 
+  hector_gazebo_quadrotor_simple_controller hector_gazebo_quadrotor_state_controller
  CATKIN_DEPENDS image_transport
  DEPENDS system_lib
 )
@@ -107,19 +112,9 @@ include_directories(
 )
 
 ## Declare a cpp library
-add_library(diffdrive_plugin_6w src/diffdrive_plugin_6w.cpp)
-target_link_libraries(diffdrive_plugin_6w ${Boost_LIBRARIES})
 
 add_library(reset_plugin src/reset_plugin.cpp)
 
-add_library(hector_gazebo_ros_imu src/gazebo_ros_imu.cpp)
-add_dependencies(hector_gazebo_ros_imu cvg_sim_gazebo_plugins_gencpp)
-target_link_libraries(hector_gazebo_ros_imu ${Boost_LIBRARIES})
-
-add_library(hector_gazebo_ros_magnetic src/gazebo_ros_magnetic.cpp)
-add_library(hector_gazebo_ros_gps src/gazebo_ros_gps.cpp)
-add_library(hector_gazebo_ros_sonar src/gazebo_ros_sonar.cpp)
-
 add_library(hector_gazebo_ros_baro src/gazebo_ros_baro.cpp)
 add_dependencies(hector_gazebo_ros_baro cvg_sim_msgs_gencpp)