From 4c3e6a231db959123c09d32dba18420c49c9251a Mon Sep 17 00:00:00 2001 From: Nathaniel Hargrave Date: Fri, 15 May 2026 12:09:42 -0400 Subject: [PATCH 1/2] Update distance sensor node to add mast camera servo --- .../gpio_controller/esp_distance_sensor.py | 38 ---------- .../gpio_controller/mast_esp.py | 71 +++++++++++++++++++ .../launch/core_gpio_devices.launch.py | 4 +- src/HW-Devices/gpio_controller/setup.py | 2 +- 4 files changed, 74 insertions(+), 41 deletions(-) delete mode 100644 src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py create mode 100644 src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py diff --git a/src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py b/src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py deleted file mode 100644 index 85a26399..00000000 --- a/src/HW-Devices/gpio_controller/gpio_controller/esp_distance_sensor.py +++ /dev/null @@ -1,38 +0,0 @@ -import rclpy -from rclpy.node import Node -from interfaces.msg import Distance -import serial - -class TimeOfFlightSensor(Node): - def __init__(self): - super().__init__("time_of_flight_sensor") - self.ser = serial.Serial('/dev/serial/by-id/usb-Espressif_USB_JTAG_serial_debug_unit_20:6E:F1:69:EE:E0-if00', 115200) - self.pub = self.create_publisher(Distance, "eef_distance", 10) - self.create_timer(0.001, self.loop) - self.get_logger().info("Time of Flight Sensor node started") - - def loop(self): - if self.ser.in_waiting: - line = self.ser.readline().decode().strip() - try: - reading = int(line) - msg = Distance() - msg.header.stamp = self.get_clock().now().to_msg() - if (reading == -2): - mm = 0 - msg.status = Distance.STATUS_ERROR - elif (reading == -1): - mm = 0 - msg.status = Distance.STATUS_INVALID - else: - msg.status = Distance.STATUS_OK - mm = reading - msg.distance = mm / 1000.0 - self.get_logger().debug(f"Distance: {msg.distance:.3f} m") - self.pub.publish(msg) - except ValueError: - pass - -rclpy.init() -node = TimeOfFlightSensor() -rclpy.spin(node) \ No newline at end of file diff --git a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py new file mode 100644 index 00000000..de211af5 --- /dev/null +++ b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py @@ -0,0 +1,71 @@ +import rclpy +from rclpy.node import Node +from interfaces.msg import Distance +import serial + + +def convert_from_radians(angle, servo_min, servo_max, servo_rom): + total_range = servo_max - servo_min + return int(servo_min + (total_range * angle / servo_rom)) + + +class MastESP(Node): + def __init__(self): + super().__init__("mast_esp") + self.ser = serial.Serial( + "/dev/serial/by-id/usb-Espressif_USB_JTAG_serial_debug_unit_20:6E:F1:69:EE:E0-if00", + 115200, + ) + self.pub = self.create_publisher(Distance, "eef_distance", 10) + self.us_sub = self.create_subscription(Float32, "/mast_us", self.mast_callback) + + self.declare_parameter("min", 350) + self.declare_parameter("max", 2500) + self.declare_parameter("rom", 6.2832) + + self.servo_min = self.get_parameter("min").get_parameter_value().float_value + self.servo_max = self.get_parameter("max").get_parameter_value().float_value + self.servo_rom = self.get_parameter("rom").get_parameter_value().float_value + + self.create_timer(0.001, self.loop) + self.get_logger().info("Mast ESP node started") + + def mast_callback(self, msg: Float32): + us = convert_from_radians( + self.msg.data, self.servo_min, self.servo_max, self.servo_rom + ) + self.ser.write("M" + str(us)) + + def loop(self): + if self.ser.in_waiting: + line = self.ser.readline().decode().strip() + try: + reading = int(line) + msg = Distance() + msg.header.stamp = self.get_clock().now().to_msg() + if reading == -2: + mm = 0 + msg.status = Distance.STATUS_ERROR + elif reading == -1: + mm = 0 + msg.status = Distance.STATUS_INVALID + else: + msg.status = Distance.STATUS_OK + mm = reading + msg.distance = mm / 1000.0 + self.get_logger().debug(f"Distance: {msg.distance:.3f} m") + self.pub.publish(msg) + except ValueError: + pass + + +def main(args=None): + rclpy.init(args=args) + node = MastESP() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/HW-Devices/gpio_controller/launch/core_gpio_devices.launch.py b/src/HW-Devices/gpio_controller/launch/core_gpio_devices.launch.py index 94ce3e58..bf2430d6 100644 --- a/src/HW-Devices/gpio_controller/launch/core_gpio_devices.launch.py +++ b/src/HW-Devices/gpio_controller/launch/core_gpio_devices.launch.py @@ -13,8 +13,8 @@ def generate_launch_description(): ), launch_ros.actions.Node( package="gpio_controller", - executable="distance_sensor", - name="distance_sensor_node", + executable="mast_esp", + name="mast_esp_node", ), launch_ros.actions.Node( package="gpio_controller", diff --git a/src/HW-Devices/gpio_controller/setup.py b/src/HW-Devices/gpio_controller/setup.py index 05a4a447..d024129c 100644 --- a/src/HW-Devices/gpio_controller/setup.py +++ b/src/HW-Devices/gpio_controller/setup.py @@ -26,7 +26,7 @@ entry_points={ "console_scripts": [ "lights = gpio_controller.lights:main", - "distance_sensor = gpio_controller.esp_distance_sensor:main", + "mast_esp = gpio_controller.mast_esp:main", "pdb_rails = gpio_controller.pdb_rails:main", ], }, From a13d2862dc56716777e8c6e6e06b43722bb1d68d Mon Sep 17 00:00:00 2001 From: Jetson Date: Sat, 16 May 2026 05:25:57 +0000 Subject: [PATCH 2/2] fix: get mast servo working --- src/Bringup/launch/control.launch.py | 7 ------- .../gpio_controller/mast_esp.py | 18 ++++++++++-------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/src/Bringup/launch/control.launch.py b/src/Bringup/launch/control.launch.py index f6f01c3a..f63637e4 100644 --- a/src/Bringup/launch/control.launch.py +++ b/src/Bringup/launch/control.launch.py @@ -100,12 +100,6 @@ def generate_launch_description(): output="screen", condition=IfCondition(use_arm), ) - eef_distance = Node( - package="gpio_controller", - executable="distance_sensor", - output="screen", - condition=IfCondition(use_arm), - ) arm_controller_servo_spawner = Node( package="controller_manager", @@ -176,7 +170,6 @@ def generate_launch_description(): ld.add_action(delayed_spawners) ld.add_action(arm_launch) ld.add_action(eef_launch) - ld.add_action(eef_distance) ld.add_action(arm_control_node) ld.add_action(drive_control_node) return ld diff --git a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py index de211af5..9d1f47cf 100644 --- a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py +++ b/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py @@ -2,6 +2,7 @@ from rclpy.node import Node from interfaces.msg import Distance import serial +from std_msgs.msg import Float32 def convert_from_radians(angle, servo_min, servo_max, servo_rom): @@ -17,24 +18,25 @@ def __init__(self): 115200, ) self.pub = self.create_publisher(Distance, "eef_distance", 10) - self.us_sub = self.create_subscription(Float32, "/mast_us", self.mast_callback) + self.us_sub = self.create_subscription(Float32, "/mast_angle", self.mast_callback, 3) - self.declare_parameter("min", 350) - self.declare_parameter("max", 2500) + self.declare_parameter("min", 350.0) + self.declare_parameter("max", 2500.0) self.declare_parameter("rom", 6.2832) - self.servo_min = self.get_parameter("min").get_parameter_value().float_value - self.servo_max = self.get_parameter("max").get_parameter_value().float_value - self.servo_rom = self.get_parameter("rom").get_parameter_value().float_value + self.servo_min = self.get_parameter("min").get_parameter_value().double_value + self.servo_max = self.get_parameter("max").get_parameter_value().double_value + self.servo_rom = self.get_parameter("rom").get_parameter_value().double_value self.create_timer(0.001, self.loop) self.get_logger().info("Mast ESP node started") def mast_callback(self, msg: Float32): us = convert_from_radians( - self.msg.data, self.servo_min, self.servo_max, self.servo_rom + msg.data, self.servo_min, self.servo_max, self.servo_rom ) - self.ser.write("M" + str(us)) + data = bytes("M" + str(us), "utf-8") + self.ser.write(data) def loop(self): if self.ser.in_waiting: