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() {