From b93a616f58cab778d7329b694b9d392e44b87ea5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 4 May 2023 14:55:39 +0200 Subject: [PATCH] add emergency stop --- src/px4_connection/src/px4_controller.cpp | 1 + src/test_controls/test_controls/test_controller.py | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 12a6f645..8c8e1d1f 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -102,6 +102,7 @@ private: last_setpoint[1] = degrees_to_radians(request->pitch); last_setpoint[2] = degrees_to_radians(request->roll); last_thrust = request->thrust; + RCLCPP_INFO(this->get_logger(), "STOPPING MOTORS"); } else { diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 89cd5432..9ed15c26 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -16,7 +16,7 @@ class TestController(Node): self.get_logger().info('service not available, waiting again...') self.req = SetAttitude.Request() - self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nEsc - exit") + self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nN - emergency stop\nEsc - exit") def spin(self): while rclpy.ok(): @@ -73,6 +73,10 @@ class TestController(Node): self.get_logger().info('up') self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) + if key == 'n': + self.get_logger().info('stop') + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.0) # else: # try: # # known keys like spacebar, ctrl