Science help me

src/Bringup/launch/science.launch.py

@@ -0,0 +1,82 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    RegisterEventHandler,
+    IncludeLaunchDescription,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, Command
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+from launch.event_handlers import OnProcessStart
+
+
+def generate_launch_description():
+    joystick_control_dir = get_package_share_directory("joystick_control")
+    joy_parameters_file = os.path.join(joystick_control_dir, "pxn.yaml")
+
+    science_drill_control = Node(
+        package="joystick_control",
+        executable="drill",
+        name="drill_teleop_node",
+        parameters=[joy_parameters_file],
+        remappings=[("/joy", "/arm/joy")],
+    )
+    talon_container = ComposableNodeContainer(
+        name="PhoenixContainerScienceTeleop",
+        namespace="",
+        package="ros_phoenix",
+        executable="phoenix_container",
+        parameters=[{"interface": "can0"}],
+        composable_node_descriptions=[
+            ComposableNode(
+                package="ros_phoenix",
+                plugin="ros_phoenix::TalonSRX",
+                name="elevator",
+                parameters=[
+                    {"id": 20},
+                    {"max_voltage": 24.0},
+                    {"brake_mode": True},
+                    {"watchdog_ms": 500},
+                ],
+            ),
+            ComposableNode(
+                package="ros_phoenix",
+                plugin="ros_phoenix::TalonSRX",
+                name="drill",
+                parameters=[
+                    {"id": 21},
+                    {"max_voltage": 24.0},
+                    {"brake_mode": True},
+                    {"watchdog_ms": 500},
+                ],
+            ),
+        ],
+        output="screen",
+    )
+
+    esp_serial_bridge = Node(
+        package="science_sensors",
+        executable="esp_serial_bridge",
+        name="esp_serial_bridge",
+        parameters=[{"port": "/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0"}],
+    )
+
+    panoramic = Node(
+        package="science_sensors",
+        executable="panoramic",
+        name="panoramic",
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(science_drill_control)
+    ld.add_action(talon_container)
+    ld.add_action(esp_serial_bridge)
+    ld.add_action(panoramic)
+    return ld

src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py

@@ -18,9 +18,11 @@ def __init__(self):
             115200,
         )
         self.pub = self.create_publisher(Distance, "eef_distance", 10)
-        self.us_sub = self.create_subscription(Float32, "/mast_angle", self.mast_callback, 3)
+        self.us_sub = self.create_subscription(
+            Float32, "/mast_angle", self.mast_callback, 3
+        )
 
-        self.declare_parameter("min", 350.0)
+        self.declare_parameter("min", 500.0)
         self.declare_parameter("max", 2500.0)
         self.declare_parameter("rom", 6.2832)
 
@@ -35,6 +37,7 @@ def mast_callback(self, msg: Float32):
         us = convert_from_radians(
             msg.data, self.servo_min, self.servo_max, self.servo_rom
         )
+        self.get_logger().debug(f"Received angle: {msg.data:.3f} rad -> PWM: {us} us")
         data = bytes("M" + str(us), "utf-8")
         self.ser.write(data)
 

src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py

@@ -19,6 +19,9 @@ class EspSerialBridge(Node):
     _PWM_STRUCT = struct.Struct("<BHHHH")
     _SENSOR_STRUCT = struct.Struct("<HHHff")
 
+    _MAGIC0 = 0xAA
+    _MAGIC1 = 0x55
+
     @staticmethod
     def _normalize_port(port_value: str) -> str:
         port = (port_value or "").strip()
@@ -48,11 +51,18 @@ def _resolve_port_path(port_value: str) -> str:
                     return candidate
         return port_value
 
+    @staticmethod
+    def _checksum(data: bytes) -> int:
+        checksum = 0
+        for b in data:
+            checksum ^= b
+        return checksum & 0xFF
+
     def __init__(self):
         super().__init__("esp_serial_bridge")
 
         self.declare_parameter(
-            "port",
+            "esp_port",
             "/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0",
         )
         self.declare_parameter("baudrate", 115200)
@@ -61,7 +71,7 @@ def __init__(self):
         self.declare_parameter("read_poll_hz", 100.0)
         self.declare_parameter("reconnect_period_s", 2.0)
 
-        configured_port = self.get_parameter("port").get_parameter_value().string_value
+        configured_port = self.get_parameter("esp_port").get_parameter_value().string_value
         self._port_config = self._normalize_port(configured_port)
         self._port = self._resolve_port_path(self._port_config)
         if configured_port != self._port_config:
@@ -162,8 +172,23 @@ def _on_pwm_command(self, msg: PwmCommand):
             int(msg.frequency) & 0xFFFF,
             int(msg.ramp) & 0xFFFF,
         )
+
+        checksum = self._checksum(payload)
+
+        packet = (
+            bytes(
+                [
+                    self._MAGIC0,
+                    self._MAGIC1,
+                ]
+            )
+            + payload
+            + bytes([checksum])
+        )
+
         try:
-            self._serial.write(payload)
+            self._serial.write(packet)
+
         except (SerialException, OSError) as exc:
             self.get_logger().warn(f"Serial write failed: {exc}")
             self._close_serial()
@@ -180,16 +205,34 @@ def _poll_serial(self):
             self.get_logger().warn(f"Serial read failed: {exc}")
             self._close_serial()
             return
+        # header(2) + payload + checksum(1)
+        packet_size = 2 + self._SENSOR_STRUCT.size + 1
 
-        packet_size = self._SENSOR_STRUCT.size
         while len(self._rx_buffer) >= packet_size:
+            if self._rx_buffer[0] != self._MAGIC0 or self._rx_buffer[1] != self._MAGIC1:
+                del self._rx_buffer[0]
+                continue
+
+            payload_start = 2
+            payload_end = payload_start + self._SENSOR_STRUCT.size
+
+            payload = bytes(self._rx_buffer[payload_start:payload_end])
+
+            received_checksum = self._rx_buffer[payload_end]
+            expected_checksum = self._checksum(payload)
+
+            if received_checksum != expected_checksum:
+                del self._rx_buffer[0]
+                continue
+
             (
                 methane,
                 co2,
                 polarimeter,
                 temperature,
                 moisture,
-            ) = self._SENSOR_STRUCT.unpack_from(self._rx_buffer, 0)
+            ) = self._SENSOR_STRUCT.unpack(payload)
+
             del self._rx_buffer[:packet_size]
 
             msg = EspSensorReadings()

src/HW-Devices/science_sensors/science_sensors/panoramic.py

@@ -6,7 +6,8 @@
 from rclpy.node import Node
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
-from interfaces.srv import VideoCapture
+from interfaces.srv import VideoCapture, VideoOut
+from interfaces.msg import VideoSource
 from threading import Event
 from std_msgs.msg import Float32
 
@@ -18,10 +19,11 @@ def __init__(self):
         self.load_params()
         self.callback_group = MutuallyExclusiveCallbackGroup()
         self.video_callback_group = MutuallyExclusiveCallbackGroup()
-        self.servo_pan_pub = self.create_publisher(Float32, "/mast_servo", 10)
+        self.servo_pan_pub = self.create_publisher(Float32, "/mast_angle", 10)
         self.video_cli = self.create_client(
             VideoCapture, "/capture_frame", callback_group=self.video_callback_group
         )
+        self.set_cam_cli = self.create_client(VideoOut, "/start_video", callback_group=self.video_callback_group)
         self.pan_srv = self.create_service(
             VideoCapture,
             "/capture_panoramic",
@@ -101,9 +103,38 @@ def construct_panoramic(self, images):
 
     def start(self, request, response):
         self.get_logger().info("Panoramic capture started")
+        if not self.set_cam_cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().error("Could not set camera, exiting...")
+            response.success = False
+            return response
+        video_start_req = VideoOut.Request()
+        source = VideoSource()
+        source.name = "Mast"
+        source.width = 100
+        source.height = 100
+        video_start_req.sources = [source]
+
+        # Use an Event to wait for the async call to complete
+        # Using ros2 executor blocking calls creates deadlock after first call
+        event = Event()
+
+        def done_callback(future):
+            nonlocal event
+            event.set()
+
+        future = self.set_cam_cli.call_async(video_start_req)
+        future.add_done_callback(done_callback)
+        event.wait(10)  # wait for max 10 seconds
+        if not future.done():
+            self.get_logger().error("Could not set camera")
+            response.success = False
+            return response
+
+        result = future.result()
         images = []
         for i in range(self.num_images):
-            pan_angle = int(i * (360 / self.num_images))
+            pan_angle = i * (6.28 / self.num_images)
+            self.get_logger().info(f"Moving servo to {pan_angle} radians")
             self.move_servo(self.servo_pan_pub, pan_angle)
             time.sleep(self.sleep_time)
             image = self.capture_image()