From 6b7481da842060075d13ac2ca9fc8a129d1bd813 Mon Sep 17 00:00:00 2001
From: ubuntu <ubuntu@ubuntu-20-drone.myguest.virtualbox.org>
Date: Fri, 12 May 2023 15:40:54 +0200
Subject: [PATCH] fix some errors

---
 call_attitude.sh                     |  1 +
 src/px4_connection/CMakeLists.txt    |  1 +
 src/px4_connection/src/heartbeat.cpp | 26 ++++++++++++++------------
 src/px4_msgs                         |  2 +-
 4 files changed, 17 insertions(+), 13 deletions(-)
 create mode 100755 call_attitude.sh

diff --git a/call_attitude.sh b/call_attitude.sh
new file mode 100755
index 00000000..89ddf4e7
--- /dev/null
+++ b/call_attitude.sh
@@ -0,0 +1 @@
+ros2 service call /drone/set_attitude drone_services/srv/SetAttitude "{pitch: $1, yaw: $2, roll: $3, thrust: $4}"
diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt
index 638d46f7..265fe1e0 100644
--- a/src/px4_connection/CMakeLists.txt
+++ b/src/px4_connection/CMakeLists.txt
@@ -28,6 +28,7 @@ add_executable(heartbeat src/heartbeat.cpp)
 ament_target_dependencies(
     heartbeat
     rclcpp
+    drone_services
     px4_ros_com
     px4_msgs
 )
diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp
index 3f1c5119..1563dfc8 100644
--- a/src/px4_connection/src/heartbeat.cpp
+++ b/src/px4_connection/src/heartbeat.cpp
@@ -1,3 +1,4 @@
+
 /**
  * @file heartbeat.cpp
  * @author Sem van der Hoeven (sem.hoeven@gmail.com)
@@ -7,6 +8,7 @@
 #include <chrono>
 #include "rclcpp/rclcpp.hpp"
 #include <px4_msgs/msg/offboard_control_mode.hpp>
+#include <drone_services/srv/set_vehicle_control.hpp>
 
 #define CONTROL_ACTUATOR_POS 0
 #define CONTROL_BODY_RATE_POS 1
@@ -23,9 +25,9 @@ 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())
-                                        // 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);
+        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));
+        // 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();
@@ -52,12 +54,12 @@ private:
     {
         // set message to enable attitude based on control mode variable
         auto msg = px4_msgs::msg::OffboardControlMode();
-        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;
+        msg.position = false;
+        msg.velocity = false;
+        msg.acceleration = false;
+        msg.attitude = true;
+        msg.body_rate = false;
+        msg.actuator = false;
 
         // get timestamp and publish message
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
@@ -65,7 +67,7 @@ private:
     }
 
     void handle_vehicle_control_change(
-        const std::shared_ptr<rmw_request_id> request_header,
+        const std::shared_ptr<rmw_request_id_t> request_header,
         const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
         const std::shared_ptr<drone_services::srv::SetVehicleControl::Response> response)
     {
@@ -73,8 +75,8 @@ private:
         {
             response->status = 1;
         } else {
-            this->control_mode = request->control
-            RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode)
+            this->control_mode = request->control;
+            RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode);
             response->status = 0;
         }
 
diff --git a/src/px4_msgs b/src/px4_msgs
index b64ef047..ffc3a4cd 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit b64ef0475c1d44605688f4770899fe453d532be4
+Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266