From dcd707fb34e960ed2875a5edb3e64d044328d503 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 15 May 2023 10:15:31 +0200 Subject: [PATCH] add arm and disarm service that return boolean for status and use this in test controller --- src/drone_services/srv/ArmDrone.srv | 2 ++ src/drone_services/srv/DisarmDrone.srv | 2 ++ src/px4_connection/src/px4_controller.cpp | 25 +++++++++++++------ .../test_controls/test_controller.py | 8 +++--- 4 files changed, 25 insertions(+), 12 deletions(-) create mode 100644 src/drone_services/srv/ArmDrone.srv create mode 100644 src/drone_services/srv/DisarmDrone.srv diff --git a/src/drone_services/srv/ArmDrone.srv b/src/drone_services/srv/ArmDrone.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/src/drone_services/srv/ArmDrone.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/src/drone_services/srv/DisarmDrone.srv b/src/drone_services/srv/DisarmDrone.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/src/drone_services/srv/DisarmDrone.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 61c4db95..101b8408 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include @@ -49,8 +51,8 @@ public: set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - disarm_service_ = this->create_service("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + disarm_service_ = this->create_service("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_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)); @@ -183,8 +185,8 @@ private: */ void handle_disarm_request( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { RCLCPP_INFO(this->get_logger(), "Got disarm request..."); @@ -193,6 +195,12 @@ private: armed = false; flying = false; publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + response->success = true; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Got disarm request but drone was not armed!"); + response->success = false; } } @@ -205,8 +213,8 @@ private: */ void handle_arm_request( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { RCLCPP_INFO(this->get_logger(), "Got arm request..."); @@ -220,10 +228,12 @@ private: RCLCPP_INFO(this->get_logger(), "Arm command sent"); this->last_thrust = -0.1; // spin motors at 10% thrust armed = true; + response->success = true; } else { RCLCPP_ERROR(this->get_logger(), "Arm request received but the drone is already armed!"); + response->success = false; } } @@ -307,7 +317,7 @@ private: send_attitude_setpoint(); */ - + RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); if (!flying) { @@ -332,7 +342,6 @@ private: send_position_setpoint(); } } - } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index b2b53fca..484026ff 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,7 +7,7 @@ import asyncio from drone_services.srv import SetAttitude from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl -from std_msgs.msg import Empty +from drone_services.srv import ArmDrone class TestController(Node): @@ -27,7 +27,7 @@ class TestController(Node): while not self.traj_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set trajectory service not available, waiting again...') self.get_logger().info('successfully connected to set trajectory service') - self.arm_cli = self.create_client(Empty, '/drone/arm') + self.arm_cli = self.create_client(ArmDrone, '/drone/arm') while not self.arm_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('arm service not available, waiting again...') self.get_logger().info('successfully connected to arm service') @@ -38,6 +38,7 @@ class TestController(Node): self.attitude_req = SetAttitude.Request() self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() + self.arm_req = ArmDrone.Request() self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit") @@ -47,8 +48,7 @@ class TestController(Node): rclpy.spin_once(self, timeout_sec=0.1) def send_arm(self): - arm_req = Empty() - self.future = self.arm_cli.call_async(arm_req) + self.future = self.arm_cli.call_async(self.arm_req) rclpy.spin_until_future_complete(self, self.future) self.get_logger().info('publishing message arm msg on service') return self.future.result()