Migrate launch to new control system structure

hector_quadrotor_controllers/launch/controller.launch

@@ -1,13 +1,16 @@
 <launch>
+
+  <arg name="controllers" default="
+    controller/attitude
+    controller/velocity
+    controller/position"/>
+
   <rosparam ns="controller" file="$(find hector_quadrotor_controllers)/params/controller.yaml" />
   <rosparam ns="limits" file="$(find hector_quadrotor_controllers)/params/limits.yaml" />
   <rosparam file="$(find hector_quadrotor_controllers)/params/params.yaml" />
 
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
-    controller/attitude
-    controller/velocity
-    controller/position
-    --shutdown-timeout 3"/>
+    $(arg controllers) --shutdown-timeout 3"/>
 
   <node name="pose_action" pkg="hector_quadrotor_actions" type="pose_action" output="screen">
     <remap from="pose" to="ground_truth_to_tf/pose" />

hector_quadrotor_controllers/params/controller.yaml

@@ -16,26 +16,28 @@ position:
 velocity:
     type: hector_quadrotor_controllers/VelocityController
     x: &vel_xy
-        p: 0.5
+        p: 1.0
         i: 0.0
         d: 0.0
         publish_state: true
     y: *vel_xy
     z:
-        p: 10.0
-        i: 0.0
+        p: 5.0
+        i: 1.0
         d: 0.0
+        antiwindup: true
+        i_clamp: 5.0
         publish_state: true
 attitude:
     type: hector_quadrotor_controllers/AttitudeController
     roll: &rollpitch
-        p: 300.0
+        p: 100.0
         i: 0.0
         d: 20.0
         publish_state: true
     pitch: *rollpitch
     yawrate:
-        p: 20.0
+        p: 10.0
         i: 0.0
-        d: 0.0
+        d: 0.5
         publish_state: true

hector_quadrotor_controllers/params/limits.yaml

@@ -10,23 +10,23 @@ pose:
 twist:
     linear: # m/s
         x:
-            max: &vel_xy_limit 1.0
+            max: &vel_xy_limit 2.0
         y:
             max: *vel_xy_limit
         z:
-            max: 0.5
+            max: 1.0
     angular: # rad/s
         z:
-            max: 1.5708
+            max: 3.14
 wrench:
     force: # N
         z:
             min: 0.0
-            max: 15.7
+            max: 100.0
     torque: # Nm
         x:
-            max: 1.0
+            max: 10.0
         y:
-            max: 1.0
+            max: 10.0
         z:
             max: 1.0

hector_quadrotor_controllers/src/attitude_controller.cpp

@@ -115,6 +115,8 @@ namespace hector_quadrotor_controllers
       yawrate_command_ = yawrate_limiter_->limit(yawrate_command_);
       thrust_command_ = thrust_limiter_->limit(thrust_command_);
 
+
+      // TODO move estop to gazebo plugin
       if(time > attitude_command_.header.stamp + ros::Duration(command_timeout_) ||
          time > yawrate_command_.header.stamp + ros::Duration(command_timeout_) ||
          time > thrust_command_.header.stamp + ros::Duration(command_timeout_) )

hector_quadrotor_demo/launch/indoor_slam_gazebo.launch

@@ -1,26 +1,35 @@
 <?xml version="1.0"?>
 
 <launch>
-   
+
   <!-- Start Gazebo with wg world running in (max) realtime -->
   <include file="$(find hector_gazebo_worlds)/launch/willow_garage.launch"/>
-   
+
   <!-- Spawn simulated quadrotor uav -->
   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
-    <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/> 
+    <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>
+    <arg name="controllers" value="
+        controller/attitude
+        controller/velocity
+        controller/position
+        "/>
   </include>
-   
+
   <!-- Start SLAM system -->
   <include file="$(find hector_mapping)/launch/mapping_default.launch">
     <arg name="odom_frame" value="world"/>
   </include>
- 
+
   <!-- Start GeoTIFF mapper -->
   <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch">
-    <arg name="trajectory_publish_rate" value="4"/> 
+    <arg name="trajectory_publish_rate" value="4"/>
   </include>
-  
+
   <!-- Start rviz visualization with preset config -->
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/indoor_slam.rviz"/>
-
+
+  <include file="$(find hector_quadrotor_teleop)/launch/xbox_controller.launch" >
+    <arg name="control_mode" value="position"/>
+  </include>
+
 </launch>

hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch

@@ -1,16 +1,25 @@
 <?xml version="1.0"?>
 
 <launch>
-   
+
   <!-- Start Gazebo with wg world running in (max) realtime -->
   <include file="$(find hector_gazebo_worlds)/launch/rolling_landscape_120m.launch"/>
-   
+
   <!-- Spawn simulated quadrotor uav -->
   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
-    <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/> 
+    <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>
+    <arg name="controllers" value="
+        controller/attitude
+        controller/velocity
+        controller/position
+        "/>
   </include>
-  
+
   <!-- Start rviz visualization with preset config -->
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/outdoor_flight.rviz"/>
-
+
+  <include file="$(find hector_quadrotor_teleop)/launch/xbox_controller.launch" >
+    <arg name="control_mode" value="position"/>
+  </include>
+
 </launch>

hector_quadrotor_demo/rviz_cfg/outdoor_flight.rviz

@@ -8,8 +8,9 @@ Panels:
         - /Status1
         - /LaserScan1/Autocompute Value Bounds1
         - /Camera1/Visibility1
+        - /Marker1
       Splitter Ratio: 0.5
-    Tree Height: 408
+    Tree Height: 403
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -131,9 +132,18 @@ Visualization Manager:
       Visibility:
         Grid: false
         LaserScan: true
+        Marker: true
         RobotModel: false
         Value: true
       Zoom Factor: 1
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /command/pose_marker
+      Name: Marker
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -159,26 +169,31 @@ Visualization Manager:
     Current:
       Class: rviz/Orbit
       Distance: 28.8231
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
       Focal Point:
         X: 0
         Y: 0
         Z: 0
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.630398
+      Pitch: 0.870398
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.16541
+      Yaw: 1.20541
     Saved: ~
 Window Geometry:
   Camera:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1026
+  Height: 1041
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000012e0000035ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000227000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026e000001320000001600ffffff000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000012e00000362fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000229000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000272000001330000001900ffffff000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000044fc0100000002fb0000000800540069006d00650100000000000007800000025800fffffffb0000000800540069006d006501000000000000045000000000000000000000064c0000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -187,6 +202,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1631
-  X: 49
-  Y: 24
+  Width: 1920
+  X: 0
+  Y: 0

hector_quadrotor_gazebo/launch/spawn_quadrotor.launch

@@ -3,8 +3,11 @@
 <launch>
   <arg name="name" default="quadrotor"/>
   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro"/>
+  <arg name="controllers" default="
+    controller/attitude
+    controller/velocity
+    controller/position"/>
   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
-  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
 
   <arg name="x" default="0.0"/>
   <arg name="y" default="0.0"/>
@@ -19,20 +22,11 @@
   <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
 
   <!-- send the robot XML to param server -->
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
-  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
-  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
-  <param name="world_frame" type="string" value="$(arg world_frame)"/>
-
-  <!-- push robot_description to factory and spawn robot in gazebo -->
-  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
-        args="-param robot_description
-           -urdf
-           -x $(arg x)
-           -y $(arg y)
-           -z $(arg z)
-           -model $(arg name)"
-        respawn="false" output="screen"/>
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)' base_link_frame:=$(arg tf_prefix)/base_link" />
+  <param name="base_link_frame" type="string" value="$(arg tf_prefix)/base_link"/>
+  <param name="base_stabilized_frame" type="string" value="$(arg tf_prefix)/base_stabilized"/>
+  <param name="base_footprint_frame" type="string" value="$(arg tf_prefix)/base_footprint"/>
+  <param name="world_frame" type="string" value="world"/>
 
   <!-- start robot state publisher -->
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
@@ -42,33 +36,47 @@
   <!-- publish state and tf -->
   <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="/$(arg world_frame)" />
+    <param name="frame_id" value="$(arg world_frame)" />
     <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
     <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
   </node>
   <group if="$(arg use_pose_estimation)">
     <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
       <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
-      <param name="nav_frame" value="/$(arg tf_prefix)/nav" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
       <param name="publish_world_nav_transform" value="true" />
       <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
       <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
     </node>
   </group>
 
-  <!-- spawn controller -->
   <group if="$(arg use_ground_truth_for_control)">
-    <param name="controller/state_topic" value="" />
-    <param name="controller/imu_topic" value="" />
+    <param name="state_topic" value="" />
+    <param name="imu_topic" value="" />
   </group>
   <group unless="$(arg use_ground_truth_for_control)">
-    <param name="controller/state_topic" value="state" />
-    <param name="controller/imu_topic" value="imu" />
+    <param name="state_topic" value="state" />
+    <param name="imu_topic" value="imu" />
   </group>
-  <include file="$(arg controller_definition)" />
 
+  <!-- load controllers -->
+  <include file="$(find hector_quadrotor_controllers)/launch/controller.launch">
+    <arg name="controllers" value="$(arg controllers)"/>
+  </include>
+
+  <!-- load aerodynamic and motor parameters -->
   <arg name="motors" default="robbe_2827-34_epp1045" />
   <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
   <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
 
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
 </launch>

hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in

@@ -2,8 +2,8 @@
 
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
-  <arg name="base_link_frame" default="base_link"/>
+  <xacro:arg name="world_frame" default="world"/>
+  <xacro:arg name="base_link_frame" default="base_link"/>
 
   <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_sensors.gazebo.xacro" />
   <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_controller.gazebo.xacro" />

hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro

@@ -11,9 +11,15 @@
         <bodyName>base_link</bodyName>
         <frameId>$(arg base_link_frame)</frameId>
         <controlRate>100.0</controlRate>
-        <!-- <controlTolerance>0.01</controlTolerance> -->
+        <controlTolerance>0.01</controlTolerance>
         <controlDelay>0.01</controlDelay>
-        <motorStatusRate>100.0</motorStatusRate>
+        <motorStatusRate>0.0</motorStatusRate>
+        <triggerTopic></triggerTopic>
+        <topicName></topicName>
+        <pwmTopicName></pwmTopicName>
+        <supplyTopic></supplyTopic>
+        <wrenchTopic></wrenchTopic>
+        <statusTopic></statusTopic>
       </plugin>
     </gazebo>
   </xacro:macro>