Merge branch 'master' into waypoint-handler-devel

Author: ajshank

freyja_controller.launch

@@ -9,12 +9,12 @@
 
   <!-- allowed autopilots: "asctec" or "arducopter" -->
   <arg name="autopilot"                 default="arducopter" />
-  <arg name="vicon_topic"               default="/vicon/F1/F1" />
+  <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"/>
+  <arg name="thrust_scaler" 		        default="200.0"/>
 
   <!-- launch a handler for discrete waypoints -->
   <arg name="use_waypoint_handler"      default="false" />
@@ -27,28 +27,33 @@
   <arg name="use_examples"              default="false"/>
   <arg name="example_number"            default="0" />
 
+
   <!-- 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">
-    <param name="state_source" type="string" value="vicon" />
-    <param name="filter_type" type="string" value="median" />
-    <param name="vicon_topic" type="string" value="$(arg vicon_topic)" />
+    <param name="state_source"  type="string"   value="vicon" />
+    <param name="filter_type"   type="string"   value="median" />
+    <param name="vicon_topic"   type="string"   value="$(arg vicon_topic)" />
   </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" value="45" type="int" />
-    <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="70" />
-    <param name="bias_compensation" type="bool" value="$(arg bias_compensation)" />
+  			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" />
   </node>
 
   <node name="lqr_vel_controller" pkg="lqr_control" type="lqr_vel_ctrl_node"
-        if="$(eval use_velctrl_only)">
-    <param name="controller_rate" type="int" value="30" />
-    <param name="total_mass" value="$(arg total_mass)" type="double" />
+  			if="$(eval use_velctrl_only)">
+    <param name="controller_rate"   type="int"    value="30" />
+    <param name="total_mass"        type="double" value="$(arg total_mass)" />
   </node>
 
   <!-- autopilot communication node -->
@@ -59,7 +64,10 @@
 
   <!-- apm/px4 needs a handler for translation, plus a mavros node -->
   <group if="$(eval autopilot=='arducopter')">
-    <node name="apm_handler" pkg="apm_handler" type="apm_handler_node" />
+    <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>

freyja_msgs/msg/vehicle_handler/FreyjaStatus.msg

@@ -0,0 +1,15 @@
+# Custom status msg that keeps track of system status
+## Autopilot handlers must fill these using their own
+## communication interfaces. Not all information may
+## be available to a handler. Fill -1 if so.
+
+# No internal component of Freyja should depend on
+# this msg, i.e., no state machine transitions are
+# effected through this. The primary purpose of this
+# msg is to ease log analysis. External tools/code
+# are, of course, free to consume it however.
+
+Header  header
+int8    is_connected    # comms to vehicle
+int8    is_armed        # motors on
+int8    is_compctrl     # mode == computer

state_manager/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
   freyja_msgs
   sensor_msgs
   nav_msgs
+  std_srvs
 )
 
 

state_manager/include/state_manager.h

@@ -12,9 +12,12 @@
 #include <ros/ros.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/Vector3.h>
 #include <sensor_msgs/NavSatFix.h>
 #include <nav_msgs/Odometry.h>
+#include <std_msgs/Float64.h>
 #include <tf/tf.h>
+#include <std_srvs/SetBool.h>
 
 #include <freyja_msgs/CurrentState.h>
 #include <freyja_msgs/AsctecData.h>
@@ -24,6 +27,8 @@
 typedef geometry_msgs::TransformStamped TFStamped;
 typedef geometry_msgs::TwistStamped TwStamped;
 typedef nav_msgs::Odometry CameraOdom;
+typedef std_srvs::SetBool::Request BoolServReq;
+typedef std_srvs::SetBool::Response BoolServRsp;
 
 #define DEG2RAD(D) ((D)*3.1415326/180.0)
 #define F_PI 3.14153
@@ -69,13 +74,21 @@ class StateManager
   FreyjaFilters pose_filter_;
   FreyjaFilters rate_filter_;
 
+  /* containers for handling gps data */
   double home_lat_, home_lon_;
   bool have_location_fix_;
+  double compass_yaw_;
+  
+  bool have_arming_origin_;
+  double map_rtk_pn_, map_rtk_pe_, map_rtk_pd_;
+  double arming_gps_pn_, arming_gps_pe_, arming_gps_pd_;
+  double gps_odom_pn_, gps_odom_pe_, gps_odom_pd_;
+  double rtk_baseoffset_pn_, rtk_baseoffset_pe_, rtk_baseoffset_pd_;
 
   public:
     StateManager();
     /* launch-time parameter specifies which one to pick */
-    void initApmManager();
+    void initPixhawkManager();
     void initAsctecManager();
     void initViconManager();
     void initCameraManager();
@@ -87,17 +100,40 @@ class StateManager
     /* Callback handler for asctec_onboard_data */
     ros::Subscriber asctec_data_sub_;
     void asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr & );
-
-    /* Callback handlers for mavros data */
-    ros::Subscriber mavros_gps_sub_;
-    ros::Subscriber mavros_vel_sub_;
-    void mavrosGpsCallback( const sensor_msgs::NavSatFix::ConstPtr & );
-    void mavrosGpsVelCallback( const TwStamped::ConstPtr & );
 
     /* Callback handler for camera updates */
     ros::Subscriber camera_estimate_sub_;
     void cameraUpdatesCallback( const CameraOdom::ConstPtr & );
 
+    /* Callback handlers for mavros data */
+    ros::Subscriber mavros_gpsraw_sub_;
+    ros::Subscriber mavros_vel_sub_;
+    ros::Subscriber compass_sub_;
+    ros::Subscriber mavros_gpsodom_sub_;
+    ros::Subscriber mavros_rtk_sub_;
+    void mavrosGpsOdomCallback( const nav_msgs::Odometry::ConstPtr & );
+    void mavrosCompassCallback( const std_msgs::Float64::ConstPtr & );
+    void mavrosRtkBaselineCallback( const geometry_msgs::Vector3::ConstPtr & );
+    
+    void mavrosGpsRawCallback( const sensor_msgs::NavSatFix::ConstPtr & );
+    
+    /* handlers for locking map frame origins */
+    inline void lockArmingGps( bool _lock = true )
+    {
+      arming_gps_pn_ = _lock? gps_odom_pn_ : 0.0;
+      arming_gps_pe_ = _lock? gps_odom_pe_ : 0.0;
+      arming_gps_pd_ = _lock? gps_odom_pd_ : 0.0;
+    }
+    inline void lockMapRTK( bool _lock = true )
+    {
+      map_rtk_pn_ = _lock? rtk_baseoffset_pn_ : 0.0;
+      map_rtk_pe_ = _lock? rtk_baseoffset_pe_ : 0.0;
+      map_rtk_pd_ = _lock? rtk_baseoffset_pd_ : 0.0;
+    }
+    
+    ros::ServiceServer maplock_srv_;
+    bool maplockArmingHandler( BoolServReq&, BoolServRsp& );
+
 
     /* Publisher for state information */
     ros::Publisher state_pub_;

state_manager/package.xml

@@ -15,6 +15,7 @@
   <build_depend>freyja_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
@@ -23,5 +24,6 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>nav_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
 
 </package>

state_manager/src/callback_implementations.cpp

@@ -74,7 +74,7 @@ void StateManager::viconCallback( const TFStamped::ConstPtr &msg )
   last_yaw_ = yaw;
 
   /* Copy over and publish right away */
-  freyja_msgs::CurrentState state_msg;
+  static freyja_msgs::CurrentState state_msg;
   state_msg.header.stamp = ros::Time::now();
   for( uint8_t idx = 0; idx < STATE_VECTOR_LEN; idx++ )
     state_msg.state_vector[idx] = state_vector_[idx];
@@ -152,11 +152,12 @@ void StateManager::asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr &
   state_pub_.publish( state_msg );
 }
 
-void StateManager::mavrosGpsCallback( const sensor_msgs::NavSatFix::ConstPtr& msg )
+void StateManager::mavrosGpsRawCallback( const sensor_msgs::NavSatFix::ConstPtr& msg )
 {
+  /*
   if( msg -> status.status >= 0 && !have_location_fix_ )
   {
-    /* first location fix */
+    // first location fix
     home_lat_ = msg -> latitude;
     home_lon_ = msg -> longitude;
     have_location_fix_ = true;
@@ -167,30 +168,88 @@ void StateManager::mavrosGpsCallback( const sensor_msgs::NavSatFix::ConstPtr& ms
 
   state_vector_[0] = ( (msg->latitude)/10000000.0 - home_lat_ )*111050.51;
   state_vector_[1] = ( (msg->longitude)/10000000.0 - home_lon_ )*84356.28;
+  */
+}
+
 
+
+void StateManager::mavrosCompassCallback( const std_msgs::Float64::ConstPtr &msg )
+{
+  compass_yaw_ = msg -> data;
 }
 
-void StateManager::mavrosGpsVelCallback( const TwStamped::ConstPtr &msg )
+void StateManager::mavrosGpsOdomCallback( const nav_msgs::Odometry::ConstPtr &msg )
 {
-  if( !have_location_fix_ )
+  /* Callback for odometry direct from the Pixhawk.
+  Note that this is Pixhawk's best estimate of where it is in the world,
+  there is no need to filter it, only structure it to our format.
+  Likely topics:
+        /mavros/global_position/local --> does not zero at arming.
+        /mavros/local_position/local  --> zeros at arming, has IMU frame quirk.
+  */
+  
+  static double pn, pe, pd, vn, ve, vd;
+  
+  // update containers anyway (needed for capturing arming location)
+  gps_odom_pn_ = msg -> pose.pose.position.y;
+  gps_odom_pe_ = msg -> pose.pose.position.x;
+  gps_odom_pd_ = -( msg -> pose.pose.position.z );
+  
+  if( !have_arming_origin_ )
+  {
+    ROS_INFO_THROTTLE( 5, "StateManager:: Waiting for arming .." );
     return;
+  }
 
-  double time_since = (ros::Time::now() - lastUpdateTime_).toSec();
+  // armed at this point
+  pn = gps_odom_pn_ + map_rtk_pn_ - arming_gps_pn_;
+  pe = gps_odom_pe_ + map_rtk_pe_ - arming_gps_pe_;
+  pd = gps_odom_pd_ + map_rtk_pd_ - arming_gps_pd_;
 
-  state_vector_[3] = msg -> twist.linear.x;
-  state_vector_[4] = msg -> twist.linear.y;
-  state_vector_[5] = msg -> twist.linear.z;
+  vn = msg -> twist.twist.linear.y;
+  ve = msg -> twist.twist.linear.x;
+  vd = -( msg -> twist.twist.linear.z );
 
-  state_vector_[12] = time_since;
+  static freyja_msgs::CurrentState state_msg;
+  state_msg.state_vector[0] = pn;
+  state_msg.state_vector[1] = pe;
+  state_msg.state_vector[2] = pd;
+  state_msg.state_vector[3] = vn;
+  state_msg.state_vector[4] = ve;
+  state_msg.state_vector[5] = vd;
+  state_msg.state_vector[8] = DEG2RAD( compass_yaw_ );
 
-  lastUpdateTime_ = ros::Time::now();
-
-  freyja_msgs::CurrentState state_msg;
   state_msg.header.stamp = ros::Time::now();
-  for( uint8_t idx = 0; idx < STATE_VECTOR_LEN; idx++ )
-    state_msg.state_vector[idx] = state_vector_[idx];
   state_pub_.publish( state_msg );
+}
+
+
+void StateManager::mavrosRtkBaselineCallback( const geometry_msgs::Vector3::ConstPtr &msg )
+{
+  rtk_baseoffset_pn_ = msg -> x;
+  rtk_baseoffset_pe_ = msg -> y;
+  rtk_baseoffset_pd_ = msg -> z;
+}
 
+bool StateManager::maplockArmingHandler( BoolServReq& rq, BoolServRsp& rp )
+{
+  /*  Service handler for when the vehicle is armed or disarmed.
+     This must lock in/release all the map offsets needed by manager.
+  */
+  if( !have_arming_origin_ )
+  {
+    lockArmingGps();  // set this locatin as origin
+    lockMapRTK();     // capture current offset from rtk base (zero if no rtk)
+    have_arming_origin_ = true;
+    ROS_WARN( "StateManager::Origin set, locking RTK map-frame!" );
+  }
+  else
+  {
+    lockArmingGps( false );
+    lockMapRTK( false );
+    have_arming_origin_ = false;
+    ROS_WARN( "StateManager::Origin cleared, unlocking RTK map-frame!" );
+  }
 }
 
 void StateManager::cameraUpdatesCallback( const CameraOdom::ConstPtr &msg )

state_manager/src/state_manager.cpp

@@ -23,7 +23,7 @@ StateManager::StateManager() : nh_(), priv_nh_("~")
   else if( state_source_ == "asctec" )
     initAsctecManager();
   else if (state_source_ == "apm" )
-    initApmManager();
+    initPixhawkManager();
   else if( state_source_ == "onboard_camera" )
     initCameraManager();
 
@@ -98,12 +98,21 @@ void StateManager::initAsctecManager()
                                 &StateManager::asctecDataCallback, this );
 }
 
-void StateManager::initApmManager()
+void StateManager::initPixhawkManager()
 {
-  mavros_gps_sub_ = nh_.subscribe( "/mavros/global_position/global", 1,
-                                &StateManager::mavrosGpsCallback, this );
-  mavros_vel_sub_ = nh_.subscribe( "/mavros/global_position/gp_vel", 1,
-                                &StateManager::mavrosGpsVelCallback, this );
+  have_arming_origin_ = false;
+  mavros_gpsraw_sub_ = nh_.subscribe( "/mavros/global_position/global", 1,
+                                &StateManager::mavrosGpsRawCallback, this );
+  mavros_gpsodom_sub_ = nh_.subscribe( "/mavros/global_position/local", 1,
+                                &StateManager::mavrosGpsOdomCallback, this,
+                                ros::TransportHints().tcpNoDelay() );
+  mavros_rtk_sub_ = nh_.subscribe( "/ublox_f9p_rtkbaseline", 1,
+                                &StateManager::mavrosRtkBaselineCallback, this );
+  compass_sub_ = nh_.subscribe( "/mavros/global_position/compass_hdg", 1, 
+				                &StateManager::mavrosCompassCallback, this );
+				                
+  maplock_srv_ = nh_.advertiseService( "/lock_arming_mapframe", 
+                        &StateManager::maplockArmingHandler, this );
 }
 
 void StateManager::initCameraManager()