add changing control modes in test controller

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.vehicle_control_cli = self.create_client(
-        self.req = SetAttitude.Request()
+            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):
+    def send_attitude_request(self, yaw, pitch, roll, thrust):
-        self.req.yaw = yaw
+        self.attitude_req.yaw = yaw
-        self.req.pitch = pitch
+        self.attitude_req.pitch = pitch
-        self.req.roll = roll
+        self.attitude_req.roll = roll
-        self.req.thrust = thrust
+        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,
+                self.move_forward()
-                                  roll=0.0, thrust=0.0)
             if key == 's':
                 self.get_logger().info('backward')
-                self.send_request(pitch=1.0, yaw=0.0,
+                self.move_backward()
-                                  roll=0.0, thrust=0.0)
             if key == 'a':
                 self.get_logger().info('left')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.move_left()
-                                  roll=-1.0, thrust=0.0)
             if key == 'd':
                 self.get_logger().info('right')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.move_right()
-                                  roll=1.0, thrust=0.0)
             if key == 'q':
                 self.get_logger().info('rotate left')
-                self.send_request(pitch=0.0, yaw=-1.0,
+                self.rotate_left()
-                                  roll=0.0, thrust=0.0)
             if key == 'e':
                 self.get_logger().info('rotate right')
-                self.send_request(pitch=0.0, yaw=1.0,
+                self.rotate_right()
-                                  roll=0.0, thrust=0.0)
             if key == 'z':
                 self.get_logger().info('down')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.move_down()
-                                  roll=0.0, thrust=-0.05)
             if key == 'space':
                 self.get_logger().info('up')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.move_up()
-                                  roll=0.0, thrust=0.05)
             if key == 'v':
                 self.get_logger().info('down a little')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.move_down_little()
-                                  roll=0.0, thrust=-0.01)
             if key == 'f':
                 self.get_logger().info('up a little')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.move_up_little()
-                                  roll=0.0, thrust=0.01)
             if key == 'n':
                 self.get_logger().info('stop')
-                self.send_request(pitch=0.0, yaw=0.0,
+                self.stop()
-                                  roll=0.0, thrust=0.0)
+            if key == '1':
-            
+                self.get_logger().info('attitude control')
-            # else:
+                self.control_mode = 1
-            #     try:
+            if key == '2':
-            #         # known keys like spacebar, ctrl
+                self.get_logger().info('velocity control')
-            #         name = key.name
+                self.control_mode = 2
-            #         vk = key.value.vk
+            if key == '1':
-            #     except AttributeError:
+                self.get_logger().info('position control')
-            #         # unknown keys like headphones skip song button
+                self.control_mode = 3
-            #         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)
 
         except Exception as e:
             self.get_logger().error(str(e))