Add back ability to publish orientation transform directly

CMakeLists.txt

@@ -15,6 +15,8 @@ find_package(catkin REQUIRED COMPONENTS
   ros_utils
   message_generation
   serial
+  tf2
+  tf2_ros
   iarc7_safety
   sensor_msgs
 )

include/iarc7_fc_comms/CommonFcComms.hpp

@@ -14,6 +14,7 @@
 
 #include <ros/ros.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 #include "iarc7_safety/SafetyClient.hpp"
 #include "iarc7_msgs/Arm.h"
@@ -89,6 +90,7 @@ namespace FcComms{
         void publishFcStatus();
 
         // Send out the orientation data for the quad
+        // Can also send transform based on flag
         void sendOrientation(double (&attitude)[3]);
 
         // Send out the accelerations from the FC
@@ -127,6 +129,9 @@ namespace FcComms{
         // Just use the default constructor
         T flightControlImpl_;
 
+        // Broadcaster to send transforms with
+        tf2_ros::TransformBroadcaster transform_broadcaster_;
+
         // Subscriber for uav_angle values
         ros::Subscriber uav_angle_subscriber;
 
@@ -165,6 +170,8 @@ namespace FcComms{
 
         bool calibrate_accelerometer_ = false;
 
+        bool publish_orientation_transform_ = false;
+
         double initial_heading_offset_ = std::nan("");
 
     };
@@ -182,6 +189,7 @@ status_publisher(),
 imu_publisher(),
 orientation_pub_(),
 flightControlImpl_(),
+transform_broadcaster_(),
 uav_angle_subscriber(),
 landing_detected_subscriber(),
 uav_arm_service(),
@@ -203,6 +211,10 @@ orientation_timestamp_offset_()
                             ros_utils::ParamUtils::getParam<bool>(
                             private_nh_, "calibrate_accelerometer");
 
+    publish_orientation_transform_ = 
+                            ros_utils::ParamUtils::getParam<bool>(
+                            private_nh_, "publish_orientation_transform");
+
     valid_landing_detected_message_delay_ = ros::Duration(
                         ros_utils::ParamUtils::getParam<double>(
                         private_nh_, "valid_landing_detected_message_delay"));
@@ -681,6 +693,24 @@ void CommonFcComms<T>::sendOrientation(double (&attitude)[3])
     orientation_msg.data.yaw = attitude[2];
 
     orientation_pub_.publish(orientation_msg);
+
+    if (publish_orientation_transform_) {
+        geometry_msgs::TransformStamped transformStamped;
+
+        transformStamped.header.stamp = ros::Time::now();
+        transformStamped.header.frame_id = CommonConf::kTfParentName;
+        transformStamped.child_frame_id = CommonConf::kTfChildName;
+
+        tf2::Quaternion q;
+       // This assumes the values are returned in the form roll pitch yaw in radians
+        q.setRPY(attitude[0], attitude[1], attitude[2]);
+        transformStamped.transform.rotation.x = q.x();
+        transformStamped.transform.rotation.y = q.y();
+        transformStamped.transform.rotation.z = q.z();
+        transformStamped.transform.rotation.w = q.w();
+
+        transform_broadcaster_.sendTransform(transformStamped);
+    }
 }
 
 // Send out the accelerations from the quad FC

package.xml

@@ -49,10 +49,14 @@
   <build_depend>message_generation</build_depend>
   <build_depend>serial</build_depend>
   <build_depend>ros_utils</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
   <build_depend>iarc7_safety</build_depend>
   <run_depend>iarc7_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>message_runtime</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_ros</run_depend>
   <run_depend>ros_utils</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->

params/fc_comms.yaml

@@ -13,3 +13,7 @@ valid_landing_detected_message_delay: 0.2
 
 # Offset for timestamping orientation messages due to filter delay on the FC
 imu_orientation_timestamp_offset: -0.00
+
+# Publish transform
+publish_orientation_transform: false
+

params/fc_comms_1.5.yaml

@@ -0,0 +1,19 @@
+# Publish the battery as read from the FC
+publish_fc_battery: false
+
+# Use the initial heading as the zero point for all future headings
+initial_heading_as_offset: true
+
+# Calibrate the accelerometer every time the foot switches say
+# the drone is on the ground
+calibrate_accelerometer: true
+
+# Timeout for determining validity of contact switch messages
+valid_landing_detected_message_delay: 0.2
+
+# Offset for timestamping orientation messages due to filter delay on the FC
+imu_orientation_timestamp_offset: -0.00
+
+# Publish transform
+publish_orientation_transform: false
+