Have robot interface publish TF

ros_ws/src/robot/autonomy/controls/mavros_interface/src/mavros_interface.cpp

@@ -35,8 +35,8 @@ class MAVROSInterface : public robot_interface::RobotInterface {
     mavros_msgs::msg::State current_state_;
 
     // data from the flight control unit (FCU)
-    bool is_fcu_yaw_received_ = false;
-    float fcu_yaw_ = 0.0;
+    bool is_yaw_received_ = false;
+    float yaw_ = 0.0;
 
     // Services
 
@@ -53,12 +53,13 @@ class MAVROSInterface : public robot_interface::RobotInterface {
     // subscriber for state messages
     rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
 
-    // subscriber for pixhawk pose messages
-    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr fcu_pose_sub_;
+    // subscriber for odometry messages
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mavros_odometry_sub_;
 
    public:
     MAVROSInterface()
         : RobotInterface("mavros_interface"),
+          // gets the parent namespace of the node to prepend to other topics
           parent_namespace_(std::filesystem::path(this->get_namespace()).parent_path().string()),
           // services
           set_mode_client_(this->create_client<mavros_msgs::srv::SetMode>(parent_namespace_ +
@@ -74,9 +75,9 @@ class MAVROSInterface : public robot_interface::RobotInterface {
           state_sub_(this->create_subscription<mavros_msgs::msg::State>(
               parent_namespace_ + "/mavros/state", 1,
               std::bind(&MAVROSInterface::state_callback, this, std::placeholders::_1))),
-          fcu_pose_sub_(this->create_subscription<geometry_msgs::msg::PoseStamped>(
-              parent_namespace_ + "/mavros/local_position/pose", 1,
-              std::bind(&MAVROSInterface::fcu_pose_callback, this, std::placeholders::_1))) {
+          mavros_odometry_sub_(this->create_subscription<nav_msgs::msg::Odometry>(
+              parent_namespace_ + "/mavros/local_position/odom", rclcpp::SensorDataQoS(), 
+              std::bind(&MAVROSInterface::mavros_odometry_callback, this, std::placeholders::_1))) {
         RCLCPP_INFO_STREAM(this->get_logger(), "my namespace is " << parent_namespace_);
     }
 
@@ -88,7 +89,7 @@ class MAVROSInterface : public robot_interface::RobotInterface {
 
     // Attitude Controls
 
-    void attitude_thrust_callback(
+    void cmd_attitude_thrust_callback(
         const mav_msgs::msg::AttitudeThrust::SharedPtr desired_cmd) override {
         mavros_msgs::msg::AttitudeTarget mavros_cmd;
         mavros_cmd.header.stamp = this->get_clock()->now();  //.to_msg();
@@ -102,9 +103,9 @@ class MAVROSInterface : public robot_interface::RobotInterface {
         attitude_target_pub_->publish(mavros_cmd);
     }
 
-    void roll_pitch_yawrate_thrust_callback(
+    void cmd_roll_pitch_yawrate_thrust_callback(
         const mav_msgs::msg::RollPitchYawrateThrust::SharedPtr desired_cmd) override {
-        if (!this->is_fcu_yaw_received_) {
+        if (!this->is_yaw_received_) {
             RCLCPP_WARN_STREAM_ONCE(this->get_logger(),
                                     "roll_pitch_yawrate_thrust command called but haven't yet "
                                     "received drone current yaw");
@@ -116,7 +117,7 @@ class MAVROSInterface : public robot_interface::RobotInterface {
         mavros_cmd.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_ROLL_RATE |
                                mavros_msgs::msg::AttitudeTarget::IGNORE_PITCH_RATE;
         tf2::Matrix3x3 m;
-        m.setRPY(desired_cmd->roll, desired_cmd->pitch, this->fcu_yaw_);
+        m.setRPY(desired_cmd->roll, desired_cmd->pitch, this->yaw_);
         tf2::Quaternion q;
         m.getRotation(q);
         mavros_cmd.body_rate.z = desired_cmd->yaw_rate;
@@ -132,7 +133,8 @@ class MAVROSInterface : public robot_interface::RobotInterface {
 
     // Position Controls
 
-    void position_callback(const geometry_msgs::msg::PoseStamped::SharedPtr desired_cmd) override {
+    void cmd_position_callback(
+        const geometry_msgs::msg::PoseStamped::SharedPtr desired_cmd) override {
         RCLCPP_DEBUG(this->get_logger(), "received pose desired_cmd: pose_callback");
 
         geometry_msgs::msg::PoseStamped desired_cmd_copy = *desired_cmd;
@@ -195,20 +197,31 @@ class MAVROSInterface : public robot_interface::RobotInterface {
     // Subscriber Callbacks
 
     /**
-     * @brief Get the current Yaw from the Pixhawk. This is used to provide the
-     * current yaw to the PitchRollYawrateThrust command, so that the drone
-     * doesn't try to set its yaw to zero.
-     *
-     * @param pose
+     * @brief Forwards the odometry message from mavros to the odometry publisher
+     * 
+     * @param msg 
      */
-    void fcu_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
-        tf2::Quaternion q(pose->pose.orientation.x, pose->pose.orientation.y,
-                          pose->pose.orientation.z, pose->pose.orientation.w);
+    void mavros_odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+        // update receiving the yaw
+        tf2::Quaternion q(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z,
+                          msg->pose.pose.orientation.w);
         double roll, pitch, yaw;
         tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
 
-        this->is_fcu_yaw_received_ = true;
-        this->fcu_yaw_ = yaw;
+        this->is_yaw_received_ = true;
+        this->yaw_ = yaw;
+
+        // update the TF and odometry publisher
+        geometry_msgs::msg::TransformStamped t;
+        t.header = msg->header;
+        t.child_frame_id = "base_link";
+        t.transform.translation.x = msg->pose.pose.position.x;
+        t.transform.translation.y = msg->pose.pose.position.y;
+        t.transform.translation.z = msg->pose.pose.position.z;
+        t.transform.rotation = msg->pose.pose.orientation;
+        // Send the transformation
+        tf_broadcaster_->sendTransform(t);
+        odometry_pub_->publish(*msg);
     }
 };
 

ros_ws/src/robot/autonomy/controls/robot_interface/CMakeLists.txt

@@ -11,6 +11,8 @@ find_package(rclcpp REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(mav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 add_library(robot_interface src/robot_interface.cpp)
 target_include_directories(robot_interface PUBLIC
@@ -19,13 +21,16 @@ target_include_directories(robot_interface PUBLIC
 target_compile_features(robot_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
 ament_target_dependencies(
   robot_interface
-  rclcpp
-  mav_msgs
+  # Required dependencies
   geometry_msgs
+  mav_msgs
   pluginlib
+  rclcpp
+  tf2
+  tf2_ros
 )
 ament_export_targets(robot_interface HAS_LIBRARY_TARGET)
-ament_export_dependencies(rclcpp mav_msgs geometry_msgs)
+ament_export_dependencies(rclcpp mav_msgs geometry_msgs tf2 tf2_ros)
 
 install(TARGETS robot_interface
   DESTINATION lib/${PROJECT_NAME})
@@ -34,7 +39,7 @@ add_executable(robot_interface_node src/robot_interface_node.cpp)
 target_include_directories(robot_interface_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-ament_target_dependencies(robot_interface_node rclcpp pluginlib mav_msgs)
+ament_target_dependencies(robot_interface_node rclcpp pluginlib mav_msgs tf2 tf2_ros)
 install(TARGETS robot_interface_node
   DESTINATION lib/${PROJECT_NAME})
 

ros_ws/src/robot/autonomy/controls/robot_interface/include/robot_interface/robot_interface.hpp

@@ -15,6 +15,10 @@
 
 #pragma once
 
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <nav_msgs/msg/odometry.hpp>
+
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <mav_msgs/msg/attitude_thrust.hpp>
@@ -34,61 +38,76 @@ namespace robot_interface {
  * control logic.
  */
 class RobotInterface : public rclcpp::Node {
-    rclcpp::Subscription<mav_msgs::msg::AttitudeThrust>::SharedPtr attitude_thrust_sub_;
-    rclcpp::Subscription<mav_msgs::msg::RateThrust>::SharedPtr rate_thrust_sub_;
+    // Command subscribers
+    rclcpp::Subscription<mav_msgs::msg::AttitudeThrust>::SharedPtr cmd_attitude_thrust_sub_;
+    rclcpp::Subscription<mav_msgs::msg::RateThrust>::SharedPtr cmd_rate_thrust_sub_;
     rclcpp::Subscription<mav_msgs::msg::RollPitchYawrateThrust>::SharedPtr
-        roll_pitch_yawrate_thrust_sub_;
-    rclcpp::Subscription<mav_msgs::msg::TorqueThrust>::SharedPtr torque_thrust_sub_;
-    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_sub_;
-    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr position_sub_;
+        cmd_roll_pitch_yawrate_thrust_sub_;
+    rclcpp::Subscription<mav_msgs::msg::TorqueThrust>::SharedPtr cmd_torque_thrust_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_velocity_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr cmd_position_sub_;
 
    protected:
+    // TF broadcaster
+    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    // odometry publisher
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_pub_;
+
     RobotInterface(std::string interface_name)
         : Node(interface_name),
+          // TODO: should these be ROS actions instead?
           // Subscribers. These will bind the subclass' overriden version of the method
-          attitude_thrust_sub_(this->create_subscription<mav_msgs::msg::AttitudeThrust>(
-              "attitude_thrust", 10,
-              std::bind(&RobotInterface::attitude_thrust_callback, this, std::placeholders::_1))),
-          rate_thrust_sub_(this->create_subscription<mav_msgs::msg::RateThrust>(
-              "rate_thrust", 10,
-              std::bind(&RobotInterface::rate_thrust_callback, this, std::placeholders::_1))),
-          roll_pitch_yawrate_thrust_sub_(
+          cmd_attitude_thrust_sub_(this->create_subscription<mav_msgs::msg::AttitudeThrust>(
+              "cmd_attitude_thrust", 10,
+              std::bind(&RobotInterface::cmd_attitude_thrust_callback, this,
+                        std::placeholders::_1))),
+          cmd_rate_thrust_sub_(this->create_subscription<mav_msgs::msg::RateThrust>(
+              "cmd_rate_thrust", 10,
+              std::bind(&RobotInterface::cmd_rate_thrust_callback, this, std::placeholders::_1))),
+          cmd_roll_pitch_yawrate_thrust_sub_(
               this->create_subscription<mav_msgs::msg::RollPitchYawrateThrust>(
-                  "roll_pitch_yawrate_thrust", 10,
-                  std::bind(&RobotInterface::roll_pitch_yawrate_thrust_callback, this,
+                  "cmd_roll_pitch_yawrate_thrust", 10,
+                  std::bind(&RobotInterface::cmd_roll_pitch_yawrate_thrust_callback, this,
                             std::placeholders::_1))),
-          torque_thrust_sub_(this->create_subscription<mav_msgs::msg::TorqueThrust>(
-              "torque_thrust", 10,
-              std::bind(&RobotInterface::torque_thrust_callback, this, std::placeholders::_1))),
-          velocity_sub_(this->create_subscription<geometry_msgs::msg::TwistStamped>(
-              "velocity", 10,
-              std::bind(&RobotInterface::velocity_callback, this, std::placeholders::_1))),
-          position_sub_(this->create_subscription<geometry_msgs::msg::PoseStamped>(
-              "position", 10,
-              std::bind(&RobotInterface::position_callback, this, std::placeholders::_1))) {}
+          cmd_torque_thrust_sub_(this->create_subscription<mav_msgs::msg::TorqueThrust>(
+              "cmd_torque_thrust", 10,
+              std::bind(&RobotInterface::cmd_torque_thrust_callback, this, std::placeholders::_1))),
+          cmd_velocity_sub_(this->create_subscription<geometry_msgs::msg::TwistStamped>(
+              "cmd_velocity", 10,
+              std::bind(&RobotInterface::cmd_velocity_callback, this, std::placeholders::_1))),
+          cmd_position_sub_(this->create_subscription<geometry_msgs::msg::PoseStamped>(
+              "cmd_position", 10,
+              std::bind(&RobotInterface::cmd_position_callback, this, std::placeholders::_1))),
+          // broadcasters
+          tf_broadcaster_(std::make_unique<tf2_ros::TransformBroadcaster>(this)),
+          odometry_pub_(this->create_publisher<nav_msgs::msg::Odometry>("odometry", 10))
+           {}
 
    public:
     // TODO add low thrust mode
 
     // Control callbacks.
-    virtual void attitude_thrust_callback(
+    virtual void cmd_attitude_thrust_callback(
         const mav_msgs::msg::AttitudeThrust::SharedPtr desired_cmd) {
         RCLCPP_WARN_ONCE(this->get_logger(), "Attitude thrust callback not implemented.");
     }
-    virtual void rate_thrust_callback(const mav_msgs::msg::RateThrust::SharedPtr desired_cmd) {
+    virtual void cmd_rate_thrust_callback(const mav_msgs::msg::RateThrust::SharedPtr desired_cmd) {
         RCLCPP_WARN_ONCE(this->get_logger(), "Rate thrust callback not implemented.");
     }
-    virtual void roll_pitch_yawrate_thrust_callback(
+    virtual void cmd_roll_pitch_yawrate_thrust_callback(
         const mav_msgs::msg::RollPitchYawrateThrust::SharedPtr desired_cmd) {
         RCLCPP_WARN_ONCE(this->get_logger(), "Roll pitch yawrate thrust callback not implemented.");
     }
-    virtual void torque_thrust_callback(const mav_msgs::msg::TorqueThrust::SharedPtr desired_cmd) {
+    virtual void cmd_torque_thrust_callback(
+        const mav_msgs::msg::TorqueThrust::SharedPtr desired_cmd) {
         RCLCPP_WARN_ONCE(this->get_logger(), "Torque thrust callback not implemented.");
     }
-    virtual void velocity_callback(const geometry_msgs::msg::TwistStamped::SharedPtr desired_cmd) {
+    virtual void cmd_velocity_callback(
+        const geometry_msgs::msg::TwistStamped::SharedPtr desired_cmd) {
         RCLCPP_WARN_ONCE(this->get_logger(), "Velocity callback not implemented.");
     }
-    virtual void position_callback(const geometry_msgs::msg::PoseStamped::SharedPtr desired_cmd) {
+    virtual void cmd_position_callback(
+        const geometry_msgs::msg::PoseStamped::SharedPtr desired_cmd) {
         RCLCPP_WARN_ONCE(this->get_logger(), "Pose callback not implemented.");
     }
 
@@ -130,6 +149,7 @@ class RobotInterface : public rclcpp::Node {
      */
     virtual bool has_control() = 0;
 
+
     virtual ~RobotInterface() {}
 };
 }  // namespace robot_interface

ros_ws/src/robot/autonomy/controls/robot_interface/package.xml

@@ -9,6 +9,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_tools</depend>
   <depend>rclcpp</depend>
   <depend>pluginlib</depend>
   <depend>mav_msgs</depend>

ros_ws/src/robot/robot_bringup/rviz/robot.rviz

@@ -6,8 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-        - /PointCloud21
-        - /LaserScan1
+        - /TF1
+        - /TF1/Frames1
       Splitter Ratio: 0.5
     Tree Height: 522
   - Class: rviz_common/Selection
@@ -44,11 +44,42 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: true
+        base_link_frd:
+          Value: false
+        map:
+          Value: true
+        map_ned:
+          Value: false
+        odom:
+          Value: false
+        odom_ned:
+          Value: false
+      Marker Scale: 2
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          base_link:
+            base_link_frd:
+              {}
+          map_ned:
+            {}
+      Update Interval: 0
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 8.646108627319336
-        Min Value: -2.548776388168335
+        Max Value: 8.698741912841797
+        Min Value: -10.431985855102539
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -143,7 +174,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: sim_lidar
+    Fixed Frame: map
     Frame Rate: 30
   Name: root
   Tools:
@@ -186,25 +217,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 36.675174713134766
+      Distance: 18.189199447631836
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 4.668973922729492
-        Y: -2.4260337352752686
-        Z: -3.6555962562561035
+        X: 3.4855470657348633
+        Y: -3.23576021194458
+        Z: -2.5088095664978027
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7403979301452637
+      Pitch: 0.6803981065750122
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.2585909366607666
+      Yaw: 2.4135921001434326
     Saved: ~
 Window Geometry:
   Depth: