change service response to success boolean

.vscode/c_cpp_properties.json

@@ -12,7 +12,8 @@
                 "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/rtls_driver/include/**",
                 "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/serial_communication/include/**",
                 "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**",
-                "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**"
+                "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**",
+                "/home/ubuntu/ros2_ws/src/px4_connection/include/**"
             ],
             "name": "ROS",
             "intelliSenseMode": "gcc-x64",

src/drone_services/srv/SetAttitude.srv

@@ -6,4 +6,4 @@ float32 roll
 #thrust between -1 and 1
 float32 thrust
 ---
-int8 status
+bool success

src/drone_services/srv/SetVehicleControl.srv

@@ -12,4 +12,4 @@
 # 32: position
 int32 control # control bitmask
 ---
-int8 status # status of operation
+bool success # if operation succeeded

src/drone_services/srv/SetVelocity.srv

@@ -6,4 +6,4 @@ float32 z_speed
 #angle is in degrees
 float32 angle
 ---
-int8 status
+bool success

src/px4_connection/src/heartbeat.cpp

@@ -109,14 +109,14 @@ private:
     {
         if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS))
         {
-            response->status = 1;
+            response->success = false;
         }
         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;
+            response->success = true;
         }
     }
 };

src/px4_connection/src/px4_controller.cpp

@@ -17,6 +17,7 @@
 #include <px4_msgs/msg/vehicle_attitude.hpp>
 
 #include <drone_services/srv/set_attitude.hpp>
+#include <drone_services/msg/DroneControlMode.hpp>
 
 #include <std_srvs/srv/empty.hpp>
 
@@ -42,6 +43,7 @@ public:
         // offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
 
         vehicle_attitude_subscription_ = this->create_subscription<px4_msgs::msg::VehicleAttitude>("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1));
+        control_mode_subscription_ = this->create_subscription<drone_services::msg::DroneControlMode>("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1));
         
         set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
         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));
@@ -58,10 +60,13 @@ private:
     rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher;
     rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
     rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr vehicle_attitude_subscription_;
+    rclcpp::Subscription<drone_services::msg::DroneControlMode>::SharedPtr control_mode_subscription_;
 
     rclcpp::Service<drone_services::srv::SetAttitude>::SharedPtr set_attitude_service_;
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr disarm_service_;
 
+
+
     // rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
 
     rclcpp::TimerBase::SharedPtr timer_;
@@ -83,6 +88,8 @@ private:
     float base_q[4] = {0, 0, 0, 0};
     int base_q_amount = 0;
 
+    char current_control_mode = CONTROL_MODE_ATTITUDE;
+
     // result quaternion
     std::array<float, 4> q = {0, 0, 0, 0};
 
@@ -117,7 +124,7 @@ private:
             RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);
             new_setpoint = true;
 
-            response->status = 0;
+            response->success = true;
         }
         else
         {
@@ -125,7 +132,7 @@ private:
             last_setpoint[0] = 0;
             last_setpoint[1] = 0;
             last_setpoint[2] = 0;
-            response->status = 1;
+            response->success = false;
         }
     }
 
@@ -286,6 +293,11 @@ private:
         }
     }
 
+    void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg)
+    {
+        
+    }
+
     /**
      * @brief Publish vehicle commands
      * @param command   Command code (matches VehicleCommand and MAVLink MAV_CMD codes)