Feat/controller mavros

.vscode/settings.json

@@ -0,0 +1,2 @@
+{
+}

src/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "remote.autoForwardPortsSource": "output",
+    "python.languageServer": "Pylance",
+    "python.analysis.completeFunctionParens": true,
+    "python.analysis.diagnosticMode": "workspace",
+}

src/controller/controller/angle_data.csv

@@ -0,0 +1,9 @@
+Angle,Value
+0, 220
+10, 260
+20, 315
+30, 370
+40, 480
+50, 610
+60, 770
+70, 1150

src/controller/controller/button_manager.py

@@ -0,0 +1,178 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32
+from mavros_msgs.srv import CommandBool, SetMode, CommandLong
+from mavros_msgs.msg import State
+from geometry_msgs.msg import Twist, Vector3
+from controller.helpers.button import Button
+from time import sleep
+from interfaces.srv import ToggleStepper, SetFloat
+
+
+
+VALID_MODES = [
+    'GUIDED',
+    'LAND',
+    'STABILIZE',
+    'LOITER'
+]
+
+TAKEOFF_HEIGHT = 1.0
+MAV_CMD_NAV_TAKEOFF = 22 # From Mavlink MAV_CMD: https://mavlink.io/en/messages/common.html#mav_commands)
+MAX_VELOCITY = 0.10
+
+
+class ButtonManagerNode(Node):
+    def __init__(self):
+        super().__init__('button_manager')
+
+        self.declare_parameter('hand', 'right')
+
+        self.hand = self.get_parameter('hand').get_parameter_value().string_value
+
+        if self.hand not in ['right', 'left']:
+            self.get_logger().error("Invalid hand parameter passed")
+            return
+
+        if self.hand == 'right':
+            self.button1 = Button(topic='controller/right/but1', short_callback=self.set_guided_callback)
+            self.button2 = Button(topic='controller/right/but2', short_callback=self.arm_callback)
+            self.button3 = Button(topic='controller/right/but3', short_callback=self.takeoff_callback)
+            self.button4 = Button(topic='controller/right/but4', short_callback=self.land_callback)
+
+            self.get_logger().info('Right hand initialized')
+
+        else:
+            self.button1 = Button(topic='controller/left/but1', press_callback=self.vertical_up_movement_callback)
+            self.button2 = Button(topic='controller/left/but2', press_callback=self.vertical_down_movement_callback)
+            self.button3 = Button(topic='controller/left/but3', short_callback=self.bag_in_callback)
+            self.button4 = Button(topic='controller/left/but4', short_callback=self.bag_out_callback)
+
+            self.get_logger().info('Left hand initialized')
+
+
+        # RIGHT CONTROLLER BUTTONS
+        self.but1_subscriber = self.create_subscription(Int32, 'controller/but1', self.button1.data_callback, 10)
+        self.but2_subscriber = self.create_subscription(Int32, 'controller/but2', self.button2.data_callback, 10)
+        self.but3_subscriber = self.create_subscription(Int32, 'controller/but3', self.button3.data_callback, 10)
+        self.but4_subscriber = self.create_subscription(Int32, 'controller/but4', self.button4.data_callback, 10)
+
+        self.mavros_clients = {
+            'mode': self.create_client(SetMode, '/mavros/set_mode'),
+            'arm': self.create_client(CommandBool, '/mavros/cmd/arming'),
+            'takeoff': self.create_client(CommandLong, '/mavros/cmd/command'),
+            'bag': self.create_client(ToggleStepper, '/rotate_stepper'),
+            'move_vert': self.create_client(SetFloat, '/vert_vel')
+        }
+
+        for service_name, client in self.mavros_clients.items():
+            while not client.wait_for_service(timeout_sec=1.0):
+                self.get_logger().warn(f'Waiting for {service_name} service')
+
+        self.get_logger().info('All services online')
+
+    def arm_takeoff_callback(self):
+        self.get_logger().info("arm and takeoff")
+        self.arm_drone()
+
+    def land_callback(self):
+        self._change_mode('LAND')
+
+    def set_guided_callback(self):
+        self._change_mode('GUIDED')
+
+    def is_armed_callback(self, msg: State):
+        self.is_armed = msg.armed
+
+    def arm_callback(self):
+        self.arm_drone()
+
+    def takeoff_callback(self):
+        self.takeoff()
+
+    def vertical_up_movement_callback(self):
+        float_data_req = SetFloat.Request()
+        float_data_req.data = MAX_VELOCITY
+        future = self.mavros_clients['move_vert'].call_async(float_data_req)
+        self.get_logger().info("Positive vertical movement")
+
+    def vertical_down_movement_callback(self):        
+        float_data_req = SetFloat.Request()
+        float_data_req.data = -MAX_VELOCITY
+        future = self.mavros_clients['move_vert'].call_async(float_data_req)
+        self.get_logger().info("Negative vertical movement")
+
+
+    def bag_in_callback(self):
+        bag_in_req = ToggleStepper.Request()
+        bag_in_req.stepper_id = 1
+        bag_in_req.speed = float(1)
+        future = self.mavros_clients['bag'].call_async(bag_in_req)
+        try:
+            self.get_logger().info(f"bag_in result: {future.result()}")
+        except Exception as e:
+            self.get_logger().error('bag_in: Exception {e} occured')
+
+    def bag_out_callback(self):
+        bag_out_req = ToggleStepper.Request()
+        bag_out_req.stepper_id = 1
+        bag_out_req.speed = float(-1)
+        future = self.mavros_clients['bag'].call_async(bag_out_req)
+        try:
+            self.get_logger().info(f"bag_in result: {future.result()}")
+        except Exception as e:
+            self.get_logger().error('bag_in: Exception {e} occured')
+
+    def _change_mode(self, mode: str):
+        mode_upper = mode.upper()
+        if mode_upper not in VALID_MODES:
+            self.get_logger().error(f"{mode.upper()} is not a valid mode")
+            return
+        mode_req = SetMode.Request(custom_mode=mode_upper)
+        future = self.mavros_clients['mode'].call_async(mode_req)
+        self.get_logger().info(f"Change mode to {mode_upper} result: {future.result()}")
+
+    def arm_drone(self):
+        arm_req = CommandBool.Request(value=True)
+        future = self.mavros_clients['arm'].call_async(arm_req)
+        self.get_logger().info(f"Arm drone result: {future.result()}")
+
+        # # rclpy.spin_until_future_complete(self, future)
+        # if future.exception():
+        #     print('exception raised')
+        # elif future.result() is None:
+        #     print('result returned none')
+        # else:
+        #     print(f"{future.result()}")
+
+        # self.get_logger().info(f"{future.done()}")
+        # return future.done()
+
+    def takeoff(self):
+        takeoff_req = CommandLong.Request(
+            broadcast=False, 
+            command=MAV_CMD_NAV_TAKEOFF, 
+            confirmation=0, 
+            param1=0.0, 
+            param2=0.0, 
+            param3=0.0, 
+            param4=0.0, 
+            param5=0.0, 
+            param6=0.0, 
+            param7=TAKEOFF_HEIGHT
+            )
+        future = self.mavros_clients['takeoff'].call_async(takeoff_req)
+        self.get_logger().info(f"Takeoff command result: {future.result()}")
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    button_manager = ButtonManagerNode()
+    rclpy.spin(button_manager)
+    button_manager.destroy_node()
+
+if __name__ == '__main__':
+    main()
+
+

src/controller/controller/controller_pub.py

@@ -0,0 +1,114 @@
+import time
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Int32  # Import Int32 message type
+from controller.helpers.serial_helper import SerialHelper
+
+VALID_MODES = [
+    'GUIDED',
+    'LAND',
+    'STABILIZE',
+    'LOITER'
+]
+
+
+
+class ControllerPubNode(Node):
+    def __init__(self):
+        super().__init__('controller_pub')
+
+        # Declare parameters for serial port and baud rate with default values
+        self.declare_parameter('serial_port', '/dev/ttyUSB0')
+        self.declare_parameter('baud_rate', 115200)
+        self.declare_parameter('hand', 'right')  # Declare 'hand' parameter
+
+
+        # Get the parameters set at runtime (or use the defaults)
+        serial_port = self.get_parameter('serial_port').get_parameter_value().string_value
+        baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
+        self.hand = self.get_parameter('hand').get_parameter_value().string_value
+
+        # Validate 'hand' parameter
+        if self.hand not in ["right", "left"]:
+            self.get_logger().warn("Invalid 'hand' parameter! Must be 'right' or 'left'. Defaulting to 'right'.")
+            self.hand = "right"
+        print("hello")
+        # Initialize the SerialHelper with the parameters
+        self.ser = SerialHelper(serial_port, baud_rate)
+
+
+        # Create publishers
+        self.imu_publisher = self.create_publisher(Imu, 'imu/' + self.hand + '/data', 10)
+        self.pot_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/pot', 10)
+        self.but1_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/but1', 10)
+        self.but2_publisher = self.create_publisher(Int32, 'controller/but2', 10)
+        self.but3_publisher = self.create_publisher(Int32, 'controller/but3', 10)
+        self.but4_publisher = self.create_publisher(Int32, 'controller/but4', 10)
+
+        # TODO: Subscribe to get state of drone for deconfliction
+
+        # Start a timer to update IMU data every 50ms (20 Hz)
+        self.timer = self.create_timer(0.05, self.update)
+
+    def update(self):
+        try:
+            # Read and process serial data
+            line = self.ser.read_from_serial()
+            # TODO: Send data from ESP in dict format and convert it from its string representation
+            # TODO: Send data from ESP in dict format and convert it from its string representation
+            if line:
+                # print(line)
+                split = line.split(',')
+                if len(split) != 11:
+                    return
+
+                # Convert data to float (accelerometer and gyroscope data)
+                accel_data = list(map(float, split[:3]))  # accelx, accely, accelz
+                gyro_data = list(map(lambda x: float(x) * (3.141592653589793 / 180), split[3:6]))  # Convert to radians/sec
+                pot_value = int(split[6])  # Potentiometer value (7th number)
+                but1_value = int(split[7])  # Button state (8th number)
+                but2_value = int(split[8])
+                but3_value = int(split[9])
+                but4_value = int(split[10])
+
+                # Create and populate IMU message
+                imu_msg = Imu()
+                imu_msg.linear_acceleration.x = accel_data[0]
+                imu_msg.linear_acceleration.y = accel_data[1]
+                imu_msg.linear_acceleration.z = accel_data[2]
+                imu_msg.angular_velocity.x = gyro_data[0]
+                imu_msg.angular_velocity.y = gyro_data[1]
+                imu_msg.angular_velocity.z = gyro_data[2]
+                self.imu_publisher.publish(imu_msg)
+
+                # Create and publish Potentiometer message
+                pot_msg = Int32()
+                pot_msg.data = pot_value
+                self.pot_publisher.publish(pot_msg)
+
+                # Create and publish Button message
+                self.but1_publisher.publish(Int32(data=but1_value))
+                self.but2_publisher.publish(Int32(data=but2_value))
+                self.but3_publisher.publish(Int32(data=but3_value))
+                self.but4_publisher.publish(Int32(data=but4_value))
+
+
+        except Exception as e:
+            self.get_logger().error(f"Error in update: {e}")
+
+
+
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    controller_pub = ControllerPubNode()
+    rclpy.spin(controller_pub)
+    controller_pub.destroy_node()
+    # rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()