Merge branch 'waypoint-handler-devel'

*add support for discrete waypoints, launch params
*fix minor typo in lqg
*add comments and recorder to launch file

Author: ajshank

freyja_configfiles/package.xml

@@ -2,7 +2,7 @@
 <package>
   <name>freyja_configfiles</name>
   <version>0.1.0</version>
-  <description>The freyja_configfiles package</description>
+  <description>The freyja package containing launch and config files</description>
 
   <maintainer email="ashankar@cse.unl.edu">aj</maintainer>
   <license>GPLv3</license>

freyja_controller.launch

@@ -1,28 +1,35 @@
 <launch>
   <!-- vim: set ft=xml noet : -->
-	
+
   <!-- Common launch file for Freyja.
-  Supports both AscTec and Pixhawk-based vehicles. Can
-  also do velocity-only control (ignores position refs).
+    Supports both AscTec and Pixhawk-based vehicles. Can
+    also do velocity-only control (ignores position refs).
     ~ aj // Nimbus Lab.
   -->
 
-  <!-- allowed autopilots: "asctec" or "arducopter" --> 
-  <!-- must provide vicon_topic -->
-  <arg name="autopilot"       default="arducopter"/>
-  <arg name="vicon_topic"/>
+  <!-- allowed autopilots: "asctec" or "arducopter" -->
+  <arg name="autopilot"                 default="arducopter" />
+  <arg name="vicon_topic" />
+
   <!-- use "device:baudrate" for apm, "device" for asctec -->
-  <arg name="comm_port"       default="/dev/ttyUSB0:57600"/>
-  <arg name="total_mass"      default="0.95"/>
-  <arg name="thrust_scaler"   default="200.0"/>
-	
-  <!-- true if providing your own trajectory source -->
-  <arg name="use_external_trajectory" default="false"/>
-  <arg name="use_velctrl_only" default="false"/>
-  <arg name="bias_compensation" default="auto" />
-  <arg name="start_rosbag" default="false"/>
-	
-	
+  <arg name="comm_port"                 default="/dev/ttyUSB0:57600"/>
+  <arg name="total_mass"                default="0.95"/>
+  <arg name="thrust_scaler" 		        default="200.0"/>
+
+  <!-- launch a handler for discrete waypoints -->
+  <arg name="use_waypoint_handler"      default="false" />
+
+  <!-- advanced settings -->
+  <arg name="use_velctrl_only"          default="false"/>
+  <arg name="bias_compensation"         default="false" />
+
+  <!-- Used for examples -->
+  <arg name="use_examples"              default="false"/>
+  <arg name="example_number"            default="0" />
+
+  <arg name="start_rosbag"              default="false" />
+
+
   <!-- state source can be: "apm", "asctec", "vicon" or "onboard_camera" -->
   <!-- implemented filters: "median", "gauss" and "lwma" -->
   <node name="state_manager" pkg="state_manager" type="state_manager_node">
@@ -31,23 +38,17 @@
     <param name="vicon_topic"   type="string"   value="$(arg vicon_topic)" />
   </node>
 
-  <!-- an example trajectory node for reference -->
-  <node name="trajectory_provider" pkg="freyja_trajectory_provider" type="trajectory_provider_node"
-        if="$(eval not use_external_trajectory)">
-    <!-- "hover", "circle1", "circle2", "circle3" -->
-    <param name="example_traj_type" type="string" value="hover"/>
-  </node>
-  
+
   <!-- control node (see velctrl flag above) -->
   <node name="lqg_controller" pkg="lqg_control" type="lqg_control_node"
   			output="screen" if="$(eval not use_velctrl_only)">
-	  <param name="controller_rate"   type="int"    value="45"  />
-  	<param name="total_mass"        type="double" value="$(arg total_mass)"/>
-  	<param name="use_stricter_gains" type="bool"  value="false" />
-  	<param name="estimator_rate"    type="int"    value="50" />
-  	<param name="bias_compensation" type="string" value="$(arg bias_compensation)" />
-  	<param name="mass_estimation"   type="bool"   value="true" />
-  	<param name="mass_correction"   type="bool"   value="false" />
+	  <param name="controller_rate"     type="int"    value="45"  />
+  	<param name="total_mass"          type="double" value="$(arg total_mass)"/>
+  	<param name="use_stricter_gains"  type="bool"   value="false" />
+  	<param name="estimator_rate"      type="int"    value="50" />
+  	<param name="bias_compensation"   type="string" value="$(arg bias_compensation)" />
+  	<param name="mass_estimation"     type="bool"   value="true" />
+  	<param name="mass_correction"     type="bool"   value="false" />
   </node>
 
   <node name="lqr_vel_controller" pkg="lqr_control" type="lqr_vel_ctrl_node"
@@ -56,9 +57,10 @@
     <param name="total_mass"        type="double" value="$(arg total_mass)" />
   </node>
 
+
   <!-- autopilot communication node -->
   <node name="asctec_handler" pkg="asctec_handler" type="asctec_handler_node"
-  			if="$(eval autopilot=='asctec')">
+        if="$(eval autopilot=='asctec')">
     <param name="serial_port" value="/dev/ttyUSB0" />
   </node>
 
@@ -67,11 +69,25 @@
     <node name="apm_handler" pkg="apm_handler" type="apm_handler_node">
       <param name="thrust_scaler" type="double" value="$(arg thrust_scaler)" />
     </node>
+
     <include file="$(find freyja_configfiles)/apm_nimbus.launch">
       <arg name="fcu_url" value="$(arg comm_port)"/>
     </include>
   </group>
+
+
+  <!-- handler for discrete waypoints -->
+  <node name="waypoint_manager" pkg="waypoint_manager" type="waypoint_manager_node" output="screen"
+				if="$(eval use_waypoint_handler)"> 
+	</node>
+
+
+  <!-- examples -->
+  <include file="$(find freyja_examples)/launch/example.launch" if="$(eval use_examples)" >
+    <arg name="example_number" value="$(arg example_number)"/>
+  </include>
 
   <!-- record stuff -->
   <node name="recorder" pkg="rosbag" type="record" args="-a" if="$(eval start_rosbag)"/>
+  
 </launch>

freyja_trajectory_provider/CMakeLists.txt renamed to freyja_examples/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(freyja_trajectory_provider)
+project(freyja_examples)
 set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3" )
 
 ## Find catkin macros and libraries
@@ -31,18 +31,10 @@ include_directories(
 )
 
 # simple trajectory provider
-add_executable(trajectory_provider_node src/trajectory_provider.cpp)
-add_dependencies(trajectory_provider_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_executable(temporal_provider_node src/temporal_provider.cpp)
+add_dependencies(temporal_provider_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-target_link_libraries(trajectory_provider_node
+target_link_libraries(temporal_provider_node
    ${catkin_LIBRARIES}
  )
-
-
-# vision-based guidance for approach
-# add_executable(trajectory_shaper_node src/trajectory_shaper.cpp)
-# add_dependencies(trajectory_shaper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-# target_link_libraries(trajectory_shaper_node
-#   ${catkin_LIBRARIES}
-# )
+ 

freyja_examples/launch/example.launch

@@ -0,0 +1,27 @@
+<launch>
+  <!-- vim: set ft=xml noet : -->
+
+  <arg name="example_number" default="0"/>
+
+  <node name="temporal_provider_node" pkg="freyja_examples" type="temporal_provider_node"
+        if="$(eval example_number==0)" >
+    <param name="example_traj_type" type="string" value="hover"/>
+  </node>
+
+  <node name="temporal_provider_node" pkg="freyja_examples" type="temporal_provider_node"
+        if="$(eval example_number==1)" >
+    <param name="example_traj_type" type="string" value="circle1"/>
+  </node>
+
+  <!-- Example 4/5: use_waypoint_handler:=true in freyja_controller.launch -->
+  <node name="waypoint_provider_node" pkg="freyja_examples" type="waypoint_provider.py"
+        if="$(eval example_number==2)" >
+    <param name="/waypoint_mode" value="0" type="int" />
+	</node>
+
+  <node name="waypoint_provider_node" pkg="freyja_examples" type="waypoint_provider.py"
+        if="$(eval example_number==3)" >
+    <param name="/waypoint_mode" value="1" type="int" />
+	</node>
+	
+</launch>

freyja_trajectory_provider/package.xml renamed to freyja_examples/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package>
-  <name>freyja_trajectory_provider</name>
+  <name>freyja_examples</name>
   <version>0.0.0</version>
-  <description>The trajectory_provider package</description>
+  <description>The examples package showing use cases</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->

freyja_trajectory_provider/src/trajectory_provider.cpp renamed to freyja_examples/src/temporal_provider.cpp

@@ -109,129 +109,6 @@ TrajRef getDefaultReference( const ros::Duration &cur_time )
   return ref_state;
 }
 
-/*
-  Ascend at point A, hover, go to B ..
-  hover at B, descend, ascend at B, hover ..
-  go to A, hover, descend at A .. Repeat.
-  
-TrajRef getCurrentReference( const ros::Duration &cur_time )
-{
-  TrajRef ref_state;
-  float t = cur_time.toSec();
-  float mod_t = std::fmod( t, 72.0 );
-  float xpos, ypos, zpos, xvel, yvel, zvel;
-  ref_state.header.stamp = ros::Time::now();
-  if( mod_t < 5.0 )
-  {
-    // ascend at A
-    xpos = -2.0;
-    ypos = 0.0;
-    xvel = 0.0;
-    yvel = 0.0;
-    zpos = -mod_t*9.0/50.0 - 0.1;
-    zvel = -9.0/50.0;
-  }
-  else if( mod_t < 8.0 )
-  {
-    // hover at A
-    xpos = -2.0;
-    ypos = 0.0;
-    xvel = 0.0;
-    yvel = 0.0;
-    zpos = -1.0;
-    zvel = 0.0;
-  }
-  else if( mod_t < 28.0 )
-  {
-    // from A to B
-    float lat_timer = mod_t - 8.0;
-    xpos = lat_timer * 0.2 - 2.0;
-    ypos = 0.0;
-    xvel = 0.2;
-    yvel = 0.0;
-    zpos = -1.0;
-    zvel = 0.0;
-  }
-  else if( mod_t < 31.0 )
-  {
-    // hover at B
-    xpos = 2.0;
-    ypos = 0.0;
-    xvel = 0.0;
-    yvel = 0.0;
-    zpos = -1.0;
-    zvel = 0.0;
-  }
-  else if( mod_t < 36.0 )
-  {
-    // descend at B
-    float desc_timer = mod_t - 31.0;
-    xpos = 2.0;
-    ypos = 0.0;
-    xvel = 0.0;
-    yvel = 0.0;
-    zpos = desc_timer*9.0/50.0 - 1.0;
-    zvel = 9.0/50.0;
-  }
-  else if( mod_t < 41.0 )
-  {
-    // ascend at B
-    float asc_timer = mod_t - 36.0;
-    xpos = 2.0;
-    ypos = 0.0;
-    xvel = 0.0;
-    yvel = 0.0;
-    zpos = -asc_timer*9.0/50.0 - 0.1;
-    zvel = -9.0/50.0;
-  }
-  else if( mod_t < 44.0 )
-  {
-    // hover at B
-    xpos = 2.0; ypos = 0.0;
-    xvel = yvel = 0.0;
-    zpos = -1.0;
-    zvel = 0.0;
-  }
-  else if( mod_t < 64.0 )
-  {
-    // from B to A
-    float lat_timer = mod_t - 44.0;
-    xpos = -lat_timer*0.2 + 2.0;
-    ypos = 0.0;
-    xvel = -0.2;
-    yvel = 0.0;
-    zpos = -1.0;
-    zvel = 0.0;
-  }
-  else if( mod_t < 67.0 )
-  {
-    // hover at A
-    xpos = -2.0; ypos = 0.0;
-    xvel = 0.0; yvel = 0.0;
-    zpos = -1.0;
-    zvel = 0.0;
-  }
-  else if( mod_t < 72.0 )
-  {
-    // descend at B
-   float desc_timer = mod_t - 67.0;
-   xpos = -2.0; ypos = 0.0;
-   xvel = 0.0; yvel = 0.0;
-   zpos = desc_timer*9.0/50.0 - 1.0;
-   zvel = 9.0/50.0;
-  }
-  
-  ref_state.pn = ypos;
-  ref_state.pe = xpos;
-  ref_state.pd = zpos;
-  ref_state.vn = yvel;
-  ref_state.ve = xvel;
-  ref_state.vd = zvel;
-  ref_state.yaw = 0.0;
-  return ref_state;
-}
-*/
-
 int main( int argc, char** argv )
 {
   ros::init( argc, argv, ROS_NODE_NAME );
@@ -273,4 +150,3 @@ int main( int argc, char** argv )
 
   return 0;
 }
-

freyja_examples/src/temporal_provider.py

@@ -0,0 +1 @@
+# TODO