sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add checking for up or down with sshkeyboard¨v

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-04 11:51:59 +02:00
parent 7133a11e92
commit 2485647c72
1 changed files with 20 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/test_controls/test_controls/test_controller.py
View File

@@ -12,16 +12,16 @@ class TestController(Node):
def __init__(self): def __init__(self):
super().__init__('test_controller') super().__init__('test_controller')
self.cli = self.create_client(SetAttitude, 'drone/set_attitude') self.cli = self.create_client(SetAttitude, 'drone/set_attitude')
# while not self.cli.wait_for_service(timeout_sec=1.0): while not self.cli.wait_for_service(timeout_sec=1.0):
# self.get_logger().info('service not available, waiting again...') self.get_logger().info('service not available, waiting again...')
self.req = SetAttitude.Request() 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\nEsc - exit")
def spin(self): def spin(self):
while rclpy.ok(): while rclpy.ok():
asyncio.run(listen_keyboard_manual(on_press=self.on_press)) asyncio.run(listen_keyboard_manual(on_press=self.on_press))
rclpy.spin_once(self, timeout_sec=0.1) rclpy.spin_once(self, timeout_sec=0.1)
def send_request(self, yaw, pitch, roll, thrust): def send_request(self, yaw, pitch, roll, thrust):
self.req.yaw = yaw self.req.yaw = yaw
@@ -44,27 +44,35 @@ class TestController(Node):
if key == 'w': if key == 'w':
self.get_logger().info('forward') self.get_logger().info('forward')
self.send_request(pitch=-10.0, yaw=0.0, self.send_request(pitch=-10.0, yaw=0.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
if key == 's': if key == 's':
self.get_logger().info('backward') self.get_logger().info('backward')
self.send_request(pitch=10.0, yaw=0.0, self.send_request(pitch=10.0, yaw=0.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
if key == 'a': if key == 'a':
self.get_logger().info('left') self.get_logger().info('left')
self.send_request(pitch=0.0, yaw=0.0, self.send_request(pitch=0.0, yaw=0.0,
roll=-10.0, thrust=0.0) roll=-10.0, thrust=0.0)
if key == 'd': if key == 'd':
self.get_logger().info('right') self.get_logger().info('right')
self.send_request(pitch=0.0, yaw=0.0, self.send_request(pitch=0.0, yaw=0.0,
roll=10.0, thrust=0.0) roll=10.0, thrust=0.0)
if key == 'q': if key == 'q':
self.get_logger().info('rotate left') self.get_logger().info('rotate left')
self.send_request(pitch=0.0, yaw=-10.0, self.send_request(pitch=0.0, yaw=-10.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
if key == 'e': if key == 'e':
self.get_logger().info('rotate right') self.get_logger().info('rotate right')
self.send_request(pitch=0.0, yaw=10.0, self.send_request(pitch=0.0, yaw=10.0,
roll=0.0, thrust=0.0) roll=0.0, thrust=0.0)
if key == 'z':
self.get_logger().info('down')
self.send_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=-0.05)
if key == 'space':
self.get_logger().info('down')
self.send_request(pitch=0.0, yaw=0.0,
roll=0.0, thrust=-0.05)
# else: # else:
# try: # try:
# # known keys like spacebar, ctrl # # known keys like spacebar, ctrl