From ff9b01cc79435f95211d77ce3bca8b9a1a219fd2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 12 May 2023 17:25:02 +0200 Subject: [PATCH] add changing control modes in test controller --- .../test_controls/test_controller.py | 215 ++++++++++++++---- 1 file changed, 166 insertions(+), 49 deletions(-) diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 8a6d7bda..bb2b4162 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -5,6 +5,7 @@ from sshkeyboard import listen_keyboard_manual import asyncio from drone_services.srv import SetAttitude +from drone_services.srv import SetTrajectory from drone_services.srv import SetVehicleControl @@ -15,23 +16,59 @@ class TestController(Node): self.cli = self.create_client(SetAttitude, 'drone/set_attitude') while not self.cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('set attitude service not available, waiting again...') - self.vehicle_control_cli = self.create_client(SetVehicleControl, 'drone/set_vehicle_control') - self.req = SetAttitude.Request() + self.vehicle_control_cli = self.create_client( + SetVehicleControl, '/drone/set_vehicle_control') + while not self.vehicle_control_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('set vehicle control service not available, waiting again...') + self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') + while not self.traj_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('set trajectory service not available, waiting again...') + + self.attitude_req = SetAttitude.Request() + self.vehicle_control_req = SetVehicleControl.Request() + self.traj_req = SetTrajectory.Request() - self.get_logger().info("Controls:\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.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") def spin(self): while rclpy.ok(): asyncio.run(listen_keyboard_manual(on_press=self.on_press)) rclpy.spin_once(self, timeout_sec=0.1) + + def send_control_mode(self): + self.vehicle_control_req.control = self.control_mode + self.future = self.vehicle_control_cli.call_async(self.vehicle_control_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message vehicle control msg on service') + return self.future.result() - def send_request(self, yaw, pitch, roll, thrust): - self.req.yaw = yaw - self.req.pitch = pitch - self.req.roll = roll - self.req.thrust = thrust + def send_attitude_request(self, yaw, pitch, roll, thrust): + self.attitude_req.yaw = yaw + self.attitude_req.pitch = pitch + self.attitude_req.roll = roll + self.attitude_req.thrust = thrust self.get_logger().info('set request to %f %f %f %f' % (yaw, pitch, roll, thrust)) - self.future = self.cli.call_async(self.req) + self.future = self.cli.call_async(self.attitude_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + + def send_velocity_request(self, x, y, z, angle): + self.traj_req.control_mode = 2 + self.traj_req.yaw = angle + self.traj_req.values = [x, y, z] + self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) + self.future = self.traj_cli.call_async(self.traj_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + + def send_position_request(self, x, y, z, angle): + self.traj_req.control_mode = 3 + self.traj_req.yaw = angle + self.traj_req.values = [x, y, z] + self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) + self.future = self.traj_cli.call_async(self.traj_req) rclpy.spin_until_future_complete(self, self.future) self.get_logger().info('publishing message on service') return self.future.result() @@ -40,71 +77,151 @@ class TestController(Node): # self.get_logger().info('released ' + str(key)) pass + def move_up(self): + pass + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.05) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=1.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) + + def move_right(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=1.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0) + + def move_down(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.05) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + + def move_left(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=-1.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0) + + def rotate_right(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=1.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0) + + def rotate_left(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=-1.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0) + + def move_forward(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=-1.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0) + + def move_backward(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=1.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0) + + def stop(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0) + + def move_up_little(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.01) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=0.5, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) + + def move_down_little(self): + if (self.control_mode == 1): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.01) + elif (self.control_mode == 2): + self.send_velocity_request(x=0.0, y=0.0, z=-0.5, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + def on_press(self, key): try: # self.get_logger().info('pressed ' + char) if key == 'w': self.get_logger().info('forward') - self.send_request(pitch=-1.0, yaw=0.0, - roll=0.0, thrust=0.0) + self.move_forward() if key == 's': self.get_logger().info('backward') - self.send_request(pitch=1.0, yaw=0.0, - roll=0.0, thrust=0.0) + self.move_backward() if key == 'a': self.get_logger().info('left') - self.send_request(pitch=0.0, yaw=0.0, - roll=-1.0, thrust=0.0) + self.move_left() if key == 'd': self.get_logger().info('right') - self.send_request(pitch=0.0, yaw=0.0, - roll=1.0, thrust=0.0) + self.move_right() if key == 'q': self.get_logger().info('rotate left') - self.send_request(pitch=0.0, yaw=-1.0, - roll=0.0, thrust=0.0) + self.rotate_left() if key == 'e': self.get_logger().info('rotate right') - self.send_request(pitch=0.0, yaw=1.0, - roll=0.0, thrust=0.0) + self.rotate_right() if key == 'z': self.get_logger().info('down') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.05) + self.move_down() if key == 'space': self.get_logger().info('up') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=0.05) + self.move_up() if key == 'v': self.get_logger().info('down a little') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.01) + self.move_down_little() if key == 'f': self.get_logger().info('up a little') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=0.01) + self.move_up_little() 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 - # name = key.name - # vk = key.value.vk - # except AttributeError: - # # unknown keys like headphones skip song button - # name = 'UNKNOWN' - # vk = key.vk - # # self.get_logger().info('pressed {} ({})'.format(name, vk)) - # if vk == 32: - # self.get_logger().info('up') - # self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) - # if vk == 65505: - # self.get_logger().info('down') - # self.send_request(pitch=0.0, yaw=0.0, - # roll=0.0, thrust=-0.05) + self.stop() + if key == '1': + self.get_logger().info('attitude control') + self.control_mode = 1 + if key == '2': + self.get_logger().info('velocity control') + self.control_mode = 2 + if key == '1': + self.get_logger().info('position control') + self.control_mode = 3 except Exception as e: self.get_logger().error(str(e))