diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt
index 96e592bc..638d46f7 100644
--- a/src/px4_connection/CMakeLists.txt
+++ b/src/px4_connection/CMakeLists.txt
@@ -21,6 +21,7 @@ find_package(rclcpp REQUIRED)
find_package(px4_ros_com REQUIRED)
find_package(px4_msgs REQUIRED)
find_package(drone_services REQUIRED)
+find_package(std_srvs REQUIRED)
include_directories(include)
add_executable(heartbeat src/heartbeat.cpp)
@@ -38,6 +39,7 @@ ament_target_dependencies(
px4_ros_com
px4_msgs
drone_services
+ std_srvs
)
target_include_directories(heartbeat PUBLIC
diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml
index 57fb022d..604e89b2 100644
--- a/src/px4_connection/package.xml
+++ b/src/px4_connection/package.xml
@@ -13,6 +13,7 @@
px4_ros_com
px4_msgs
drone_services
+ std_srvs
ament_lint_auto
ament_lint_common
diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 87d50d9d..40663c6a 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -15,12 +15,13 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
#include
#include
-// #include
#include
#include
#include
+#include
+
// #include
#define D_SPEED(x) -x - 9.81
@@ -38,7 +39,8 @@ public:
trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10);
// offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10);
- set_attitude_service_ = this->create_service("set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+ set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+ set_attitude_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
// create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this));
@@ -52,6 +54,7 @@ private:
rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher;
rclcpp::Publisher::SharedPtr vehicle_command_publisher_;
rclcpp::Service::SharedPtr set_attitude_service_;
+ rclcpp::Service::SharedPtr disarm_service_;
// rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_;
@@ -87,6 +90,21 @@ private:
response->status = 0;
}
+/**
+ * @brief disarms the drone when a call to the disarm service is made
+ *
+ * @param request_header the header for the service request
+ * @param request the request (empty)
+ * @param response the response (empty)
+ */
+ void handle_disarm_request(
+ const std::shared_ptr request_header,
+ const std::shared_ptr request,
+ const std::shared_ptr response)
+ {
+ publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
+ }
+
void send_trajectory_setpoint()
{