From 9785136a4849fb2543f9018847adfea3f61c90ee Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Fri, 12 May 2023 12:16:14 +0200
Subject: [PATCH] defines and logging control mode

---
 src/drone_services/msg/DroneControlMode.msg |  6 ++-
 src/px4_connection/src/heartbeat.cpp        | 55 +++++++++++++++++++--
 2 files changed, 55 insertions(+), 6 deletions(-)

diff --git a/src/drone_services/msg/DroneControlMode.msg b/src/drone_services/msg/DroneControlMode.msg
index 68758456..92669b9e 100644
--- a/src/drone_services/msg/DroneControlMode.msg
+++ b/src/drone_services/msg/DroneControlMode.msg
@@ -1 +1,5 @@
-int32 mode
\ No newline at end of file
+# control mode of the drone
+# 1: Attitude
+# 2: Velocity
+# 3: Position
+int8 control_mode
\ No newline at end of file
diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp
index d989b02c..09718930 100644
--- a/src/px4_connection/src/heartbeat.cpp
+++ b/src/px4_connection/src/heartbeat.cpp
@@ -8,6 +8,11 @@
 #include "rclcpp/rclcpp.hpp"
 #include <px4_msgs/msg/offboard_control_mode.hpp>
 #include <drone_services/srv/set_vehicle_control.hpp>
+#include <drone_services/msg/drone_control_mode.hpp>
+
+#define CONTROL_MODE_ATTITUDE 1
+#define CONTROL_MODE_VELOCITY 2
+#define CONTROL_MODE_POSITION 3
 
 #define CONTROL_ACTUATOR_POS 0
 #define CONTROL_BODY_RATE_POS 1
@@ -23,9 +28,10 @@ class HeartBeat : public rclcpp::Node
 public:
     HeartBeat() : Node("heartbeat")
     {
-        // disarm_service_ = this->create_service<std_srvs::srv::Empty>("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
         vehicle_control_mode_service_ = this->create_service<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
         RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control");
+        // create a publisher on the drone control mode topic
+        drone_control_mode_publisher_ = this->create_publisher<drone_services::msg::DroneControlMode>("/drone/control_mode", 10);
         // create a publisher on the offboard control mode topic
         offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
         // create timer to send heartbeat messages (offboard control) every 100ms
@@ -37,14 +43,17 @@ public:
 private:
     // publisher for offboard control mode messages
     rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
+    // publisher for the control mode of the drone
+    rclcpp::Publisher<drone_services::msg::DroneControlMode>::SharedPtr drone_control_mode_publisher_;
     // timer for sending the heartbeat
     rclcpp::TimerBase::SharedPtr timer_;
+    // service to change vehicle_mode
+    rclcpp::Service<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_mode_service_;
+
     // start time of node in seconds
     double start_time;
     // which level of control is needed, only attitude control by default
     int control_mode = 1 << CONTROL_ATTITUDE_POS;
-    // service to change vehicle_mode
-    rclcpp::Service<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_mode_service_;
     /**
      * @brief Publish offboard control mode messages as a heartbeat.
      *        Only the attitude is enabled, because that is how the drone will be controlled.
@@ -66,6 +75,40 @@ private:
         offboard_control_mode_publisher_->publish(msg);
     }
 
+/**
+ * @brief Publish the current control mode of the drone onto the 'drone/control_mode' topic
+ * 
+ */
+    void publish_new_control_mode()
+    {
+        auto msg = drone_services::msg::DroneControlMode();
+        if ((this->control_mode >> CONTROL_ATTITUDE_POS) & 1)
+        {
+            msg.control_mode = CONTROL_MODE_ATTITUDE;
+            RCLCPP_INFO(this->get_logger(), "set control mode to attitude");
+        }
+        else if ((this->control_mode >> CONTROL_VELOCITY_POS) & 1)
+        {
+            msg.control_mode = CONTROL_MODE_VELOCITY;
+            RCLCPP_INFO(this->get_logger(), "set control mode to velocity");
+        }
+        else if ((this->control_mode >> CONTROL_POSITION_POS) & 1)
+        {
+            msg.control_mode = CONTROL_MODE_POSITION;
+            RLCPP_INFO(this->get_logger(), "set control mode to position");
+        }
+        RCLCPP_INFO(this->get_logger(), "publishing new control mode %d", msg.control_mode);
+        drone_control_mode_publisher_->publish(msg);
+    }
+
+    /**
+     * @brief Handle a request to change the vehicle control mode
+     *
+     * @param request_header the header of the service request
+     * @param request the request that was sent to the service
+     * @param response the reponse that the service sends back to the client.
+     */
+
     void handle_vehicle_control_change(
         const std::shared_ptr<rmw_request_id_t> request_header,
         const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
@@ -74,12 +117,14 @@ private:
         if (request->control < 0 || request->control > CONTROL_POSITION_POS)
         {
             response->status = 1;
-        } else {
+        }
+        else
+        {
             this->control_mode = request->control;
             RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode);
+            publish_new_control_mode();
             response->status = 0;
         }
-
     }
 };