From 5684c27f47d4de768955f0788d747e342f71d2dc Mon Sep 17 00:00:00 2001 From: tomaswilliston Date: Wed, 13 May 2026 23:17:02 +0000 Subject: [PATCH 1/4] First draft on talon drill control --- .../science_esp_code/science_esp.ino | 8 +-- .../joystick_control/CMakeLists.txt | 2 +- .../joystick_control/config/pxn.yaml | 14 +---- .../joystick_control/include/drill.hpp | 6 +- .../launch/rover_science.launch.py | 59 +++++++++++++------ .../joystick_control/src/drill.cpp | 24 ++++---- 6 files changed, 64 insertions(+), 49 deletions(-) diff --git a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino index 2f6d565b..f3a87244 100644 --- a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino +++ b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino @@ -29,10 +29,10 @@ static const uint32_t PWM_TICK_MS = 10; static const uint32_t SENSOR_PERIOD_MS = 100; -static const int METHANE_PIN = 4; -static const int CO2_PIN = 12; -static const int POLARIMETER_PIN = 14; -static const int DHT_PIN = 2; +static const int METHANE_PIN = 12; +static const int CO2_PIN = 33; +static const int POLARIMETER_PIN = 4; +static const int DHT_PIN = 14; #define DHT_TYPE DHT22 diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 19169965..5b45b34a 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -67,7 +67,7 @@ ament_target_dependencies(arm ament_target_dependencies(drill rclcpp sensor_msgs - std_msgs + ros_phoenix ) install(TARGETS diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 239e2987..89810ff3 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -35,16 +35,4 @@ drill_teleop_node: max_drill_duty: 1.0 max_elevator_duty: 1.0 drill_cmd_topic: "/drill/set" - elevator_cmd_topic: "/elevator/set" - -elevator: - roboclaw_elevator_node: - ros__parameters: - port: "/dev/ttyACM0" - baudrate: 115200 - -drill: - roboclaw_drill_node: - ros__parameters: - port: "/dev/ttyACM1" - baudrate: 115200 \ No newline at end of file + elevator_cmd_topic: "/elevator/set" \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/drill.hpp b/src/Teleop-Control/joystick_control/include/drill.hpp index 4ffb1b6e..a313b998 100644 --- a/src/Teleop-Control/joystick_control/include/drill.hpp +++ b/src/Teleop-Control/joystick_control/include/drill.hpp @@ -2,8 +2,8 @@ #define DRILL_HPP #include "rclcpp/rclcpp.hpp" +#include "ros_phoenix/msg/motor_control.hpp" #include "sensor_msgs/msg/joy.hpp" -#include "std_msgs/msg/float32.hpp" class drill : public rclcpp::Node { public: @@ -15,8 +15,8 @@ class drill : public rclcpp::Node { void load_parameters(); rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr drill_pub_; - rclcpp::Publisher::SharedPtr elevator_pub_; + rclcpp::Publisher::SharedPtr drill_pub_; + rclcpp::Publisher::SharedPtr elevator_pub_; rclcpp::TimerBase::SharedPtr joy_watchdog_timer_; void on_joy_watchdog(); diff --git a/src/Teleop-Control/joystick_control/launch/rover_science.launch.py b/src/Teleop-Control/joystick_control/launch/rover_science.launch.py index 3adca420..b6ad5e0f 100644 --- a/src/Teleop-Control/joystick_control/launch/rover_science.launch.py +++ b/src/Teleop-Control/joystick_control/launch/rover_science.launch.py @@ -1,4 +1,4 @@ -"""Science stack: RoboClaw elevator + drill, Thrustmaster joy, drill teleop.""" +"""Science stack: Phoenix drill/elevator, Thrustmaster joy, drill teleop.""" from __future__ import annotations @@ -12,7 +12,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode # Ordered: first match wins. Thrustmaster sticks often expose *-event-joystick in by-id. DEFAULT_JOY_GLOBS: Tuple[str, ...] = ( @@ -95,6 +96,10 @@ def _science_setup(context, *args, **kwargs): pkg = get_package_share_directory("joystick_control") parameters_file = os.path.join(pkg, "pxn.yaml") + can_iface = LaunchConfiguration("phoenix_can_interface").perform(context) + drill_id = int(LaunchConfiguration("drill_motor_id").perform(context)) + elev_id = int(LaunchConfiguration("elevator_motor_id").perform(context)) + joy_arg = LaunchConfiguration("joy_dev").perform(context).strip() env_dev = os.environ.get("SCIENCE_JOY_DEV", "").strip() @@ -126,21 +131,35 @@ def _science_setup(context, *args, **kwargs): print(f"[rover_science] Using joystick device: {joy_dev}", flush=True) return [ - Node( - package="ros_roboclaw", - executable="roboclaw_node", - name="roboclaw_elevator_node", - namespace="elevator", - parameters=[parameters_file], - remappings=[("robo_duty_cycle", "/elevator/set")], - ), - Node( - package="ros_roboclaw", - executable="roboclaw_node", - name="roboclaw_drill_node", - namespace="drill", - parameters=[parameters_file], - remappings=[("robo_duty_cycle", "/drill/set")], + ComposableNodeContainer( + name="PhoenixContainerScienceTeleop", + namespace="", + package="ros_phoenix", + executable="phoenix_container", + parameters=[{"interface": can_iface}], + composable_node_descriptions=[ + ComposableNode( + package="ros_phoenix", + plugin="ros_phoenix::TalonSRX", + name="elevator", + parameters=[ + {"id": elev_id}, + {"max_voltage": 24.0}, + {"brake_mode": True}, + ], + ), + ComposableNode( + package="ros_phoenix", + plugin="ros_phoenix::TalonSRX", + name="drill", + parameters=[ + {"id": drill_id}, + {"max_voltage": 24.0}, + {"brake_mode": True}, + ], + ), + ], + output="screen", ), Node( package="joy_linux", @@ -173,6 +192,12 @@ def generate_launch_description(): description="Absolute path to joystick (e.g. /dev/input/js0). " "If empty, auto-detect Thrustmaster via /dev/input/by-id.", ), + DeclareLaunchArgument( + "phoenix_can_interface", + default_value="can0", + ), + DeclareLaunchArgument("drill_motor_id", default_value="20"), + DeclareLaunchArgument("elevator_motor_id", default_value="21"), OpaqueFunction(function=_science_setup), ] ) diff --git a/src/Teleop-Control/joystick_control/src/drill.cpp b/src/Teleop-Control/joystick_control/src/drill.cpp index 919fc20a..53635df1 100644 --- a/src/Teleop-Control/joystick_control/src/drill.cpp +++ b/src/Teleop-Control/joystick_control/src/drill.cpp @@ -7,9 +7,10 @@ drill::drill() : Node("drill_teleop_node") { declare_parameters(); load_parameters(); - drill_pub_ = create_publisher(k_drill_cmd_topic_, 10); - elevator_pub_ = - create_publisher(k_elevator_cmd_topic_, 10); + drill_pub_ = + create_publisher(k_drill_cmd_topic_, 10); + elevator_pub_ = create_publisher( + k_elevator_cmd_topic_, 10); joy_sub_ = create_subscription( "/joy", 10, std::bind(&drill::drill_control, this, std::placeholders::_1)); @@ -64,17 +65,18 @@ void drill::drill_control(std::shared_ptr joystick_msg) { const double power_axis = joystick_msg->axes[k_drill_power_axis_]; const double elev_axis = joystick_msg->axes[k_drill_elevation_axis_]; - // Stick [-1, 1] -> drill throttle [0, 1]; elevator uses raw axis [-1, 1]. const double drill_t = 0.5 * (power_axis + 1.0); - const double drill_duty = std::clamp(drill_t * k_max_drill_duty_, 0.0, 1.0); - - const double elev_duty = + const double drill_value = std::clamp(drill_t * k_max_drill_duty_, 0.0, 1.0); + const double elev_value = std::clamp(elev_axis * k_max_elevator_duty_, -1.0, 1.0); - auto drill_msg = std_msgs::msg::Float32(); - drill_msg.data = static_cast(drill_duty); - auto elev_msg = std_msgs::msg::Float32(); - elev_msg.data = static_cast(elev_duty); + auto drill_msg = ros_phoenix::msg::MotorControl(); + drill_msg.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; + drill_msg.value = drill_value; + + auto elev_msg = ros_phoenix::msg::MotorControl(); + elev_msg.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; + elev_msg.value = elev_value; drill_pub_->publish(drill_msg); elevator_pub_->publish(elev_msg); From bc9ee037a122ae05332e659da67584290b797d04 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Thu, 14 May 2026 01:23:18 +0000 Subject: [PATCH 2/4] feat: add headers to esp32 code --- .../science_sensors/esp_serial_bridge.py | 46 +++++- .../science_esp_code/science_esp.ino | 132 +++++++++++++++--- 2 files changed, 153 insertions(+), 25 deletions(-) diff --git a/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py b/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py index 2bd80cea..9c91f0b2 100644 --- a/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py +++ b/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py @@ -19,6 +19,9 @@ class EspSerialBridge(Node): _PWM_STRUCT = struct.Struct(" str: port = (port_value or "").strip() @@ -48,6 +51,13 @@ 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") @@ -162,8 +172,17 @@ 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 +199,37 @@ 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() diff --git a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino index f3a87244..46c0d8e8 100644 --- a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino +++ b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino @@ -22,17 +22,20 @@ struct SensorReadings { }; #pragma pack(pop) -static const uint8_t PWM_RES_BITS = 10; +static const uint8_t MAGIC0 = 0xAA; +static const uint8_t MAGIC1 = 0x55; + +static const uint8_t PWM_RES_BITS = 10; static const uint16_t PWM_MAX_DUTY = (1u << PWM_RES_BITS) - 1; -static const uint8_t PWM_MAX_ACTIVE = 8; -static const uint32_t PWM_TICK_MS = 10; +static const uint8_t PWM_MAX_ACTIVE = 8; +static const uint32_t PWM_TICK_MS = 10; static const uint32_t SENSOR_PERIOD_MS = 100; static const int METHANE_PIN = 12; static const int CO2_PIN = 33; static const int POLARIMETER_PIN = 4; -static const int DHT_PIN = 14; +static const int DHT_PIN = 14; #define DHT_TYPE DHT22 @@ -48,10 +51,87 @@ struct ActivePwm { uint32_t start_ms; uint32_t ramp_end_ms; uint32_t run_end_ms; + uint32_t start_duty; }; static ActivePwm active_pins[PWM_MAX_ACTIVE]; +static uint8_t calcChecksum(const uint8_t* data, size_t len) { + uint8_t c = 0; + for (size_t i = 0; i < len; i++) { + c ^= data[i]; + } + return c; +} + +static bool readFramedCommand(PwmCommand& cmd) { + static uint8_t state = 0; + static uint8_t payload[sizeof(PwmCommand)]; + static size_t payload_index = 0; + + while (Serial.available() > 0) { + uint8_t b = Serial.read(); + + switch (state) { + case 0: + if (b == MAGIC0) { + state = 1; + } + break; + + case 1: + if (b == MAGIC1) { + payload_index = 0; + state = 2; + } else if (b == MAGIC0) { + state = 1; + } else { + state = 0; + } + break; + + case 2: + payload[payload_index++] = b; + if (payload_index >= sizeof(PwmCommand)) { + state = 3; + } + break; + + case 3: { + uint8_t expected = calcChecksum(payload, sizeof(PwmCommand)); + uint8_t received = b; + + state = 0; + payload_index = 0; + + if (received == expected) { + memcpy(&cmd, payload, sizeof(PwmCommand)); + return true; + } + + break; + } + + default: + state = 0; + payload_index = 0; + break; + } + } + + return false; +} + +static void writeFramedSensorReadings(const SensorReadings& pkt) { + const uint8_t* payload = reinterpret_cast(&pkt); + uint8_t checksum = calcChecksum(payload, sizeof(SensorReadings)); + + Serial.write(MAGIC0); + Serial.write(MAGIC1); + Serial.write(payload, sizeof(SensorReadings)); + Serial.write(checksum); +} + static int findPWMIndex(uint8_t pin) { for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { if (active_pins[i].active && active_pins[i].pin == pin) return i; @@ -81,29 +161,34 @@ static void runCommand(const PwmCommand& cmd) { int i = findPWMIndex(cmd.pin); if (i < 0) i = findFreeIndex(); if (i < 0) return; - + if (!active_pins[i].active) { if (!ledcAttach(cmd.pin, cmd.frequency, PWM_RES_BITS)) { return; } - } else if(cmd.frequency != active_pins[i].frequency) return; + } else if (cmd.frequency != active_pins[i].frequency) { + releaseIndex(i); + + if (!ledcAttach(cmd.pin, cmd.frequency, PWM_RES_BITS)) { + return; + } + } uint32_t now = millis(); uint32_t rampMs = (uint32_t)cmd.ramp * 100u; uint32_t holdMs = (uint32_t)cmd.duration * 100u; active_pins[i].active = true; + active_pins[i].start_duty = active_pins[i].duty_cycle; active_pins[i].pin = cmd.pin; - active_pins[i].duty_cycle = duty; + active_pins[i].duty_cycle = duty; active_pins[i].start_ms = now; active_pins[i].ramp_end_ms = now + rampMs; active_pins[i].run_end_ms = active_pins[i].ramp_end_ms + holdMs; - active_pins[i].frequency = cmd.frequency; + active_pins[i].frequency = cmd.frequency; if (rampMs == 0) { ledcWrite(cmd.pin, duty); - } else { - ledcWrite(cmd.pin, 0); } } @@ -125,7 +210,8 @@ static void updateActivePwm() { uint32_t elapsed = now - a.start_ms; uint32_t rampSpan = a.ramp_end_ms - a.start_ms; - uint32_t scaled = (uint32_t)a.duty_cycle * elapsed / rampSpan; + uint32_t rampDelta = (uint32_t)a.duty_cycle - a.start_duty; + uint32_t scaled = rampDelta * elapsed / rampSpan + a.start_duty; if (scaled > a.duty_cycle) scaled = a.duty_cycle; ledcWrite(a.pin, scaled); } @@ -143,11 +229,11 @@ static void pwmTask(void* /*arg*/) { } } -static uint16_t readMethane() { return (uint16_t)analogRead(METHANE_PIN); } -static uint16_t readCo2() { return (uint16_t)analogRead(CO2_PIN); } -static uint16_t readPolarimeter() { return (uint16_t)analogRead(POLARIMETER_PIN); } -static float readTemperatureC() { return dht.readTemperature(); } -static float readMoisture() { return dht.readHumidity(); } +static uint16_t readMethane() { return (uint16_t)analogRead(METHANE_PIN); } +static uint16_t readCo2() { return (uint16_t)analogRead(CO2_PIN); } +static uint16_t readPolarimeter() { return (uint16_t)analogRead(POLARIMETER_PIN); } +static float readTemperatureC() { return dht.readTemperature(); } +static float readMoisture() { return dht.readHumidity(); } static void sensorTask(void* /*arg*/) { TickType_t lastWake = xTaskGetTickCount(); @@ -159,7 +245,7 @@ static void sensorTask(void* /*arg*/) { pkt.temperature = readTemperatureC(); pkt.moisture = readMoisture(); - Serial.write(reinterpret_cast(&pkt), sizeof(pkt)); + writeFramedSensorReadings(pkt); vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(SENSOR_PERIOD_MS)); } @@ -169,7 +255,9 @@ void setup() { Serial.begin(115200); dht.begin(); - for (int i = 0; i < PWM_MAX_ACTIVE; ++i) active_pins[i].active = false; + for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { + active_pins[i].active = false; + } g_pwmCmdQueue = xQueueCreate(8, sizeof(PwmCommand)); @@ -178,10 +266,10 @@ void setup() { } void loop() { - if (Serial.available() >= (int)sizeof(PwmCommand)) { - PwmCommand cmd; - Serial.readBytes(reinterpret_cast(&cmd), sizeof(cmd)); - xQueueSend(g_pwmCmdQueue, &cmd, 0); + PwmCommand cmd; + + while (readFramedCommand(cmd)) { + xQueueSend(g_pwmCmdQueue, &cmd, pdMS_TO_TICKS(20)); } vTaskDelay(pdMS_TO_TICKS(1)); } From 39d72fa4c7f25e0b5ea1a3c11e743889b588ca90 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 May 2026 00:28:00 +0000 Subject: [PATCH 3/4] fix: switch to DHTesp lib to avoid crash --- .../science_esp_code/science_esp.ino | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino index 46c0d8e8..490a7bea 100644 --- a/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino +++ b/src/HW-Devices/science_sensors/science_sensors/science_esp_code/science_esp.ino @@ -2,7 +2,7 @@ #include #include #include -#include "DHT.h" +#include #pragma pack(push, 1) struct PwmCommand { @@ -31,15 +31,14 @@ static const uint8_t PWM_MAX_ACTIVE = 8; static const uint32_t PWM_TICK_MS = 10; static const uint32_t SENSOR_PERIOD_MS = 100; +static const uint32_t DHT_PERIOD_MS = 2000; static const int METHANE_PIN = 12; static const int CO2_PIN = 33; static const int POLARIMETER_PIN = 4; static const int DHT_PIN = 14; -#define DHT_TYPE DHT22 - -DHT dht(DHT_PIN, DHT_TYPE); +DHTesp dht; static QueueHandle_t g_pwmCmdQueue = nullptr; @@ -232,19 +231,26 @@ static void pwmTask(void* /*arg*/) { static uint16_t readMethane() { return (uint16_t)analogRead(METHANE_PIN); } static uint16_t readCo2() { return (uint16_t)analogRead(CO2_PIN); } static uint16_t readPolarimeter() { return (uint16_t)analogRead(POLARIMETER_PIN); } -static float readTemperatureC() { return dht.readTemperature(); } -static float readMoisture() { return dht.readHumidity(); } static void sensorTask(void* /*arg*/) { + static const int DHT_INTERVAL = DHT_PERIOD_MS / SENSOR_PERIOD_MS; + static int dht_counter = 0; + static float lastTemperature = 0; + static float lastMoisture = 0; TickType_t lastWake = xTaskGetTickCount(); for (;;) { SensorReadings pkt; pkt.methane = readMethane(); pkt.co2 = readCo2(); pkt.polarimeter = readPolarimeter(); - pkt.temperature = readTemperatureC(); - pkt.moisture = readMoisture(); - + if (++dht_counter >= DHT_INTERVAL) { + dht_counter = 0; + TempAndHumidity data = dht.getTempAndHumidity(); + lastTemperature = data.temperature; + lastMoisture = data.humidity; + } + pkt.temperature = lastTemperature; + pkt.moisture = lastMoisture; writeFramedSensorReadings(pkt); vTaskDelayUntil(&lastWake, pdMS_TO_TICKS(SENSOR_PERIOD_MS)); @@ -253,7 +259,7 @@ static void sensorTask(void* /*arg*/) { void setup() { Serial.begin(115200); - dht.begin(); + dht.setup(DHT_PIN, DHTesp::DHT22); for (int i = 0; i < PWM_MAX_ACTIVE; ++i) { active_pins[i].active = false; @@ -262,7 +268,7 @@ void setup() { g_pwmCmdQueue = xQueueCreate(8, sizeof(PwmCommand)); xTaskCreatePinnedToCore(pwmTask, "pwmTask", 4096, nullptr, 2, nullptr, 1); - xTaskCreatePinnedToCore(sensorTask, "sensorTask", 4096, nullptr, 1, nullptr, 1); + xTaskCreatePinnedToCore(sensorTask, "sensorTask", 4096, nullptr, 1, nullptr, 0); } void loop() { From 018be3223dda709ce440a27124e9ab9bfb6d7a4d Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 16 May 2026 11:33:31 +0000 Subject: [PATCH 4/4] Final science cleanup --- src/Bringup/launch/science.launch.py | 82 +++++++++++++++++++ .../gpio_controller/mast_esp.py | 7 +- .../science_sensors/esp_serial_bridge.py | 23 +++--- .../science_sensors/panoramic.py | 37 ++++++++- .../joystick_control/config/pxn.yaml | 2 +- .../joystick_control/include/drill.hpp | 7 -- .../joystick_control/src/drill.cpp | 33 -------- 7 files changed, 135 insertions(+), 56 deletions(-) create mode 100644 src/Bringup/launch/science.launch.py diff --git a/src/Bringup/launch/science.launch.py b/src/Bringup/launch/science.launch.py new file mode 100644 index 00000000..b261b015 --- /dev/null +++ b/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 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 9d1f47cf..474aebd3 100644 --- a/src/HW-Devices/gpio_controller/gpio_controller/mast_esp.py +++ b/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) diff --git a/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py b/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py index 9c91f0b2..19e0a477 100644 --- a/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py +++ b/src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py @@ -62,7 +62,7 @@ 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) @@ -71,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: @@ -175,10 +175,16 @@ def _on_pwm_command(self, msg: PwmCommand): checksum = self._checksum(payload) - packet = bytes([ - self._MAGIC0, - self._MAGIC1, - ]) + payload + bytes([checksum]) + packet = ( + bytes( + [ + self._MAGIC0, + self._MAGIC1, + ] + ) + + payload + + bytes([checksum]) + ) try: self._serial.write(packet) @@ -203,10 +209,7 @@ def _poll_serial(self): packet_size = 2 + self._SENSOR_STRUCT.size + 1 while len(self._rx_buffer) >= packet_size: - if ( - self._rx_buffer[0] != self._MAGIC0 or - self._rx_buffer[1] != self._MAGIC1 - ): + if self._rx_buffer[0] != self._MAGIC0 or self._rx_buffer[1] != self._MAGIC1: del self._rx_buffer[0] continue diff --git a/src/HW-Devices/science_sensors/science_sensors/panoramic.py b/src/HW-Devices/science_sensors/science_sensors/panoramic.py index 49470860..8ea93afb 100644 --- a/src/HW-Devices/science_sensors/science_sensors/panoramic.py +++ b/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() diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 89810ff3..1ed38376 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -29,7 +29,7 @@ arm_teleop_node: drill_teleop_node: ros__parameters: - drill_power_axis: 3 + drill_power_axis: 2 drill_elevation_axis: 1 joy_first_message_timeout_s: 10.0 max_drill_duty: 1.0 diff --git a/src/Teleop-Control/joystick_control/include/drill.hpp b/src/Teleop-Control/joystick_control/include/drill.hpp index a313b998..a38f32d8 100644 --- a/src/Teleop-Control/joystick_control/include/drill.hpp +++ b/src/Teleop-Control/joystick_control/include/drill.hpp @@ -17,20 +17,13 @@ class drill : public rclcpp::Node { rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr drill_pub_; rclcpp::Publisher::SharedPtr elevator_pub_; - rclcpp::TimerBase::SharedPtr joy_watchdog_timer_; - - void on_joy_watchdog(); - void mark_joy_received(); int k_drill_power_axis_; int k_drill_elevation_axis_; double k_max_drill_duty_; double k_max_elevator_duty_; - double k_joy_first_message_timeout_s_; std::string k_drill_cmd_topic_; std::string k_elevator_cmd_topic_; - - bool joy_received_{false}; }; #endif // DRILL_HPP diff --git a/src/Teleop-Control/joystick_control/src/drill.cpp b/src/Teleop-Control/joystick_control/src/drill.cpp index 53635df1..0def8e84 100644 --- a/src/Teleop-Control/joystick_control/src/drill.cpp +++ b/src/Teleop-Control/joystick_control/src/drill.cpp @@ -15,10 +15,6 @@ drill::drill() : Node("drill_teleop_node") { "/joy", 10, std::bind(&drill::drill_control, this, std::placeholders::_1)); - joy_watchdog_timer_ = create_wall_timer( - std::chrono::duration(k_joy_first_message_timeout_s_), - std::bind(&drill::on_joy_watchdog, this)); - RCLCPP_INFO( this->get_logger(), "Drill teleop started: drill axis=%d, elevation axis=%d -> '%s' / '%s'", @@ -26,33 +22,7 @@ drill::drill() : Node("drill_teleop_node") { k_elevator_cmd_topic_.c_str()); } -void drill::mark_joy_received() { - if (joy_received_) { - return; - } - joy_received_ = true; - if (joy_watchdog_timer_) { - joy_watchdog_timer_->cancel(); - } -} - -void drill::on_joy_watchdog() { - if (joy_watchdog_timer_) { - joy_watchdog_timer_->cancel(); - } - if (joy_received_) { - return; - } - RCLCPP_FATAL( - this->get_logger(), - "No /joy messages within %.1f s (check joy_linux_node, remappings, and " - "permissions). Shutting down.", - k_joy_first_message_timeout_s_); - rclcpp::shutdown(); -} - void drill::drill_control(std::shared_ptr joystick_msg) { - mark_joy_received(); const size_t n_axes = joystick_msg->axes.size(); if (static_cast(k_drill_power_axis_) >= n_axes || static_cast(k_drill_elevation_axis_) >= n_axes) { @@ -85,7 +55,6 @@ void drill::drill_control(std::shared_ptr joystick_msg) { void drill::declare_parameters() { declare_parameter("drill_power_axis", 3); declare_parameter("drill_elevation_axis", 1); - declare_parameter("joy_first_message_timeout_s", 10.0); declare_parameter("max_drill_duty", 1.0); declare_parameter("max_elevator_duty", 1.0); declare_parameter("drill_cmd_topic", "/drill/set"); @@ -95,8 +64,6 @@ void drill::declare_parameters() { void drill::load_parameters() { this->get_parameter("drill_power_axis", k_drill_power_axis_); this->get_parameter("drill_elevation_axis", k_drill_elevation_axis_); - this->get_parameter("joy_first_message_timeout_s", - k_joy_first_message_timeout_s_); this->get_parameter("max_drill_duty", k_max_drill_duty_); this->get_parameter("max_elevator_duty", k_max_elevator_duty_); this->get_parameter("drill_cmd_topic", k_drill_cmd_topic_);