add changing control mode of heartbeat through service

src/drone_services/srv/SetVehicleControl.srv

@@ -1,3 +1,11 @@
+# bitmask for control:
+# bit 0: actuator
+# bit 1: body rate
+# bit 2: attitude
+# bit 3: acceleration
+# bit 4: velocity
+# bit 5: position
+
 int32 control # control bitmask
 ---
 int8 status # status of operation

src/px4_connection/src/heartbeat.cpp

@@ -1,13 +1,20 @@
 /**
  * @file heartbeat.cpp
  * @author Sem van der Hoeven (sem.hoeven@gmail.com)
- * @brief Heartbeat node that keeps the connection between the flight computer 
+ * @brief Heartbeat node that keeps the connection between the flight computer
  * and PX4 flight controller alive by sending OffboardControl messages
  */
 #include <chrono>
 #include "rclcpp/rclcpp.hpp"
 #include <px4_msgs/msg/offboard_control_mode.hpp>
 
+#define CONTROL_ACTUATOR_POS 0
+#define CONTROL_BODY_RATE_POS 1
+#define CONTROL_ATTITUDE_POS 2
+#define CONTROL_ACCELERATION_POS 3
+#define CONTROL_VELOCITY_POS 4
+#define CONTROL_POSITION_POS 5
+
 using namespace std::chrono_literals;
 
 class HeartBeat : public rclcpp::Node
@@ -15,8 +22,10 @@ class HeartBeat : public rclcpp::Node
 public:
     HeartBeat() : Node("heartbeat")
     {
-        // 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);
+        // 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())
+                                        // 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
         timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this));
         start_time = this->get_clock()->now().seconds();
@@ -30,7 +39,10 @@ private:
     rclcpp::TimerBase::SharedPtr timer_;
     // 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.
@@ -38,19 +50,35 @@ private:
      */
     void send_heartbeat()
     {
-        // set message to enable attitude
+        // set message to enable attitude based on control mode variable
         auto msg = px4_msgs::msg::OffboardControlMode();
-        msg.position = false;
-        msg.velocity = false;
-        msg.acceleration = false;
-        msg.attitude = true;
-        msg.body_rate = false;
-        msg.actuator = false;
+        msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false;
+        msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false;
+        msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false;
+        msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false;
+        msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false;
+        msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false;
 
         // get timestamp and publish message
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
         offboard_control_mode_publisher_->publish(msg);
     }
+
+    void handle_vehicle_control_change(
+        const std::shared_ptr<rmw_request_id> request_header,
+        const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
+        const std::shared_ptr<drone_services::srv::SetVehicleControl::Response> response)
+    {
+        if (request->control < 0 || request->control > CONTROL_POSITION_POS)
+        {
+            response->status = 1;
+        } else {
+            this->control_mode = request->control
+            RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode)
+            response->status = 0;
+        }
+
+    }
 };
 
 int main(int argc, char *argv[])