From a670aa8c1ac24ca0a1865459a415c06a3578fbed Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:30:00 +0200 Subject: [PATCH] add ability to arm --- src/test_controls/test_controls/test_controller.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index bb2b4162..4ee0ce0e 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -7,6 +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 class TestController(Node): @@ -28,7 +29,9 @@ class TestController(Node): self.vehicle_control_req = SetVehicleControl.Request() self.traj_req = SetTrajectory.Request() - self.get_logger().info("Controls:1 - Attitude control\n2 - Velocity control\n3 - Position control\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") + self.arm_publisher = self.create_publisher(Empty, '/drone/arm', 10) + + self.get_logger().info("Controls:1 - 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") def spin(self): while rclpy.ok(): @@ -222,6 +225,9 @@ class TestController(Node): if key == '1': self.get_logger().info('position control') self.control_mode = 3 + if key == '/': + self.get_logger().info('arming') + self.arm_publisher.publish(Empty()) except Exception as e: self.get_logger().error(str(e))