diff --git a/Documentation/2025/Images/MODBUS.png b/Documentation/2025/Images/MODBUS.png new file mode 100644 index 0000000..bc8ad83 Binary files /dev/null and b/Documentation/2025/Images/MODBUS.png differ diff --git a/Documentation/2025/Images/Probe Motor Implementation Overview.png b/Documentation/2025/Images/Probe Motor Implementation Overview.png new file mode 100644 index 0000000..520f4ec Binary files /dev/null and b/Documentation/2025/Images/Probe Motor Implementation Overview.png differ diff --git a/Documentation/2025/Images/TCP-ADU.png b/Documentation/2025/Images/TCP-ADU.png new file mode 100644 index 0000000..5cd3d7a Binary files /dev/null and b/Documentation/2025/Images/TCP-ADU.png differ diff --git a/Documentation/2025/probe-motor.md b/Documentation/2025/probe-motor.md new file mode 100644 index 0000000..6a6441b --- /dev/null +++ b/Documentation/2025/probe-motor.md @@ -0,0 +1,331 @@ +# Rover Probe Motor Control System + +# Overview +This documentation provides setup guide and serves as a resource to understand the implementation of the LMDCE421(probe-motor) communication. + +This document will give the reader sufficient knowledge to modify/rebuild the probe-motor codebase. + +With these goals in mind I have indicated (opt) next to topics or sections that I deem to be irrelevant to goal 1. + +# Table Of Content +Probe-Motor + - [LMDCE421](#lmdce421) + - [Function Support](#function-support) + + +Communication Medium + - [MODBUS/TCP](#modbus/tcp) + - [ROS2 Network](#ros2-network) + - [pymodbus](#pymodbus) + +Setup + - [Static IP Setup](#static-ip-setup) + +--- + +# Probe-Motor +The probe-motor's task is to provide linear motion to a temperature probe, allowing the probe to take readings closer to the surface of the hotspot. The chosen motor for this task is the **LMDCE421**, a stepper with high precision and high load capacity. + +## LMDCE421 +image + +[LMDCE421](https://www.novantaims.com/liberty-mdrives/lm42-nema-17-ethernet-tcpip-ip20-lmoe42/) is an integrated Lexium MDrive (LMD) motor combining a stepper motor, drive electronics, and a controller into a single package. + +* **Operating Principle:** It uses microstepping to achieve high resolution. For our application, the motor translates rotary motion into linear motion via a lead screw. +* **Power Requirements:** Typically operates on **12–48 VDC**. +* **Networking:** The "E" in the model name signifies **Ethernet** connectivity, supporting industrial protocols like MODBUS/TCP. +* **Registers (opt):** Internally, the motor operates on a set of parameters (e.g., Velocity, Acceleration, Target Position) mapped to specific memory addresses. + + + +## Function Support +To achieve the goals of the probe-motor assembly, the codebase supports the following primary functions: + +1. **Relative/Absolute Movement:** Moving the probe to specific depths (steps or mm) relative to the current or home position. +2. **Status Monitoring:** Reading motor registers to check if the device is "Ready," "Moving," or in an "Error" state. +3. **Emergency Stop:** Immediate deceleration and torque-off command for safety. + +--- + +# Communication Medium +The LMDCE421 offers an ethernet and serial interface. We have used the ethernet interface communication because there is extensive library support for encoding/decoding data easily over this interface and communication can easily be reproduced from any computer with an ethernet port. **MODBUS/TCP** is the chosen communication protocol for this interface. + +## MODBUS/TCP +[MODBUS/TCP](https://www.novantaims.com/application-note/modbustcp-vs-mcodetcp-overview/) is a variant of the Modbus family of vendor-neutral communication protocols intended for supervision and control of automation equipment. + +image + + +* **Structure:** It wraps a standard Modbus frame inside a TCP/IP packet (Port 502). +* **Client/Server Model:** In this setup, our control script acts as the **Client** (Master), and the LMDCE421 acts as the **Server** (Slave). +* **Protocol Data Unit (PDU):** This is the core of the message containing the instruction. + +image + + +### Protocol Data Unit Examples +The following table illustrates how the request parameters are structured within the PDU for common motor operations: + +| Operation | Function Code | Data Address | Data Quantity/Value | Observation | +| :--- | :--- | :--- | :--- | :--- | +| **Read Position** | `03` (Read Holding) | `00 64` (100) | `00 02` | Reads two registers to get a 32-bit position value. | +| **Check Status** | `04` (Read Input) | `00 0A` (10) | `00 01` | Checks a single register for the "Ready" flag. | +| **Set Velocity** | `06` (Write Single) | `01 2C` (300) | `03 E8` | Writes a fixed speed (e.g., 1000) to the velocity register. | +| **Multi-Move** | `10` (Write Multiple) | `02 58` (600) | `00 04` | Updates acceleration/deceleration, and target at once. | + + + +## pymodbus +[pymodbus](https://pymodbus.readthedocs.io/en/latest/) is a full Python implementation of the Modbus protocol. It abstracts the low-level socket handling into readable Python methods. + +### 1. read_holding_registers +Used to retrieve current settings or state data from the motor. Returns 2 registers in Little-Endian format: registers[0] = low, registers[1] = high. +```python +# Read 2 registers starting at address 100 +response = client.read_holding_registers(address=100, count=2) +if not response.isError(): + print(response.registers) +``` + +### 2. write_register +Used to write a single 16-bit value to the motor. +```python +# Write the value 1 to register address 50 +client.write_register(address=50, value=1) +``` + +### 3. write_registers +Used for 32-bit values or simultaneous parameter updates. Uses Little-Endian format[low,high] at the register level. +```python +# Write a list of values to registers starting at address 200 +values = [1000, 500] +client.write_registers(address=200, values=values) +``` + +### 4. read_input_registers +Used to read hardware-level data that is typically read-only. +```python +# Read the current position from the input register +pos_data = client.read_input_registers(address=300, count=1) +``` +--- + +This is a hardware-abstracted ROS2 interface for controlling a linear probe motor (LMDCE421 Stepper). It features a **Factory Pattern** that allows for seamless switching between real hardware and simulated environments via a JSON configuration. + + +## ROS2 Network +![Text](Images/Probe%20Motor%20Implementation%20Overview.png) + +The system is divided into three layers: + +1. **Hardware/Simulation Layer**: Handles Modbus TCP communication or simulated physics. +2. **Abstraction Layer**: The `StepperFactory` uses `sensors.json` to decide which driver to instantiate. +3. **ROS2 Layer**: The `probeMotor` node acts as the server, while `nav` acts as the mission controller. + +--- + +### 1. Requirements + +#### Hardware + +* **Motor**: LMDCE421 Stepper Motor. +* **Communication**: Ethernet/Modbus TCP (Default IP: `192.168.33.1`). + +#### Software + +* **OS**: Ubuntu 22.04 LTS with ROS2 Humble. +* **Ethernet:** [static IP](rpi-ip-setup-for-motor.md) +* **Python Dependencies**: +* [`pymodbus`](https://pymodbus.readthedocs.io/en/latest/index.html): For Modbus TCP communication. + + + +--- + +### 2. Configuration (`sensors.json`) + +All hardware parameters are stored in `config/sensors.json`. This allows for calibration without changing the source code. + +| Parameter | Description | +| --- | --- | +| `mode` | Set to `"real"` for hardware or `"sim"` for simulation. | +| `steps_per_distance` | Conversion factor (Steps per cm). Default: `51200`. | +| `motion_range` | Maximum vertical displacement allowed (cm). | +| `max_velocity` | Speed limit in steps/second. | + +--- + +### 3. ROS2 Interface + +#### Subscribed Topics + +* **`/move_probe_motor`** (`msg_interface/MoveProbeMotor`): Receives target position commands (cm). +* **`/probe_motor_feedback`** (`msg_interface/ProbeMotorFeedback`): Receives position updates and movement confirmation. + +#### Message Definitions + +* **MoveProbeMotor**: `int32 move_position` (Target depth in cm). +* **ProbeMotorFeedback**: `bool has_moved`, `int32 position` (Current depth in cm). + +--- + +### 4. Setup and Execution + +#### Building the Package + +```bash +cd ~/ros2_ws +colcon build --packages-select embr msg_interface +source install/setup.bash + +``` + +#### Running the System + +To launch all nodes (Temperature, Cube, Radio, and Motor) with the default configuration: + +```bash +ros2 launch embr embr_launch.py + +``` + +To specify a custom configuration file: + +```bash +ros2 launch embr embr_launch.py config_file:=/path/to/your_config.json + +``` + +--- + +### 5. Logic Flow: Navigation & Survey + +The `nav` node executes an autonomous survey mission: + +1. **Search Phase**: Randomly waits for a "hotspot" detection. +2. **Approach Phase**: Moves the rover toward the target. +3. **Deployment**: +* Publishes to `/move_probe_motor`. +* Uses a `threading.Event` to block until `/probe_motor_feedback` confirms the move. + + +4. **Retrieval**: Returns the probe to position `0` before continuing the search. + +--- + +### 6. Troubleshooting + +* **Connection Errors**: Ensure the LMDCE421 IP (`192.168.33.1`) is reachable from your host machine (`ping 192.168.33.1`) [see](rpi-ip-setup-for-motor.md). + +--- + +# Setup + +## Static IP Setup + +This guide explains how to configure your Raspberry Pi to talk directly to an industrial LMD/Lexium drive using a static IPv4 address using the built-in Netplan. + +--- + +### Prerequisite: +- [ ] rpi is connected to the LMD Drive via ethernet + +LMD Drive is powered with sufficient power: +- [ ] Voltage: 12VDC - 48VDC +- [ ] Current: 0.0Amp - 2.0 Amp + +### Step 1: Identify your Ethernet Interface +Before editing configurations, you need the system name for your Ethernet port. + +```bash +ip link show + +``` + +Usually, this is `eth0`, but on some Ubuntu builds it may appear as `enp1s0` or similar. + +--- + +### Step 2: Locate the Netplan Configuration + +Netplan stores its settings in `.yaml` files. List them to find the one your system is using: + +```bash +ls /etc/netplan/ + +``` + +Common names: `01-netcfg.yaml`, `50-cloud-init.yaml`, or `00-installer-config.yaml`. + +--- + +### Step 3: Edit the Configuration + +Open the file with `nano`. **Note:** YAML files are strictly sensitive to spaces. Do not use tabs. + +```bash +sudo nano /etc/netplan/01-netcfg.yaml + +``` + +Replace the content with the following block. Adjust `eth0` to your interface name and `192.168.33.10` to your desired Pi IP. + +```yaml +network: + version: 2 + renderer: networkd + ethernets: + eth0: + dhcp4: no + addresses: + - 192.168.33.10/24 + +``` + +--- + +### Step 4: Validate and Apply + +Netplan has a built-in safety feature to prevent you from locking yourself out of the system. + +1. **Test the configuration:** + +```bash +sudo netplan try + +``` + +If you don't press 'Enter' within 120 seconds, it will roll back the changes. + +2. **Apply the changes permanently:** + +```bash +sudo netplan apply + +``` + +--- + +### Step 5: Verify the Link + +1. **Check that the IP is assigned:** + +```bash +ip addr show eth0 + +``` + +2. **Ping the LMD Drive:** +Replace `192.168.33.1` with the actual IP address of your drive. + +```bash +ping 192.168.33.1 + +``` + +--- + +### Troubleshooting Tips + +* **No Link:** If `ip link` shows `NO-CARRIER`, check your physical Ethernet cable. +* **YAML Errors:** If `netplan apply` throws an error, double-check your indentation. Every nested line must be indented by exactly two spaces. diff --git a/Tools/Setup-Scripts/install-dependencies b/Tools/Setup-Scripts/install-dependencies index 41c7813..d2921d0 100644 --- a/Tools/Setup-Scripts/install-dependencies +++ b/Tools/Setup-Scripts/install-dependencies @@ -14,6 +14,9 @@ sudo apt install -y python3-pip echo "Installing libuvc-dev..." sudo apt install -y libuvc-dev +echo "Installing pymodbus..." +sudo apt install -y pymodbus + echo "=========================================" echo "Dependencies installed successfully!" echo "=========================================" diff --git a/ros2_ws/src/embr/config/sensors.json b/ros2_ws/src/embr/config/sensors.json index d6b26d2..a0203c7 100644 --- a/ros2_ws/src/embr/config/sensors.json +++ b/ros2_ws/src/embr/config/sensors.json @@ -2,16 +2,30 @@ "temperature": { "mode": "real", "device": "/dev/ttyACM0", - "baud": 9600 + "baud": 9600, + "params": "" }, "cube": { "mode": "real", "device": "/dev/ttyAMA0", - "baud": 57600 + "baud": 57600, + "params": "" }, "radio": { "mode": "real", "device": "/dev/ttyAMA1", - "baud": 57600 + "baud": 57600, + "params": "" + }, + "probeMotor": { + "mode": "real", + "device": "", + "baud": 0, + "params": { + "max_velocity": 768000, + "acceleration": 1000000, + "steps_per_distance": 51200, + "motion_range": 10 + } } } diff --git a/ros2_ws/src/embr/config/sensors_mixed.json b/ros2_ws/src/embr/config/sensors_mixed.json index 4e47b00..bfda6cb 100644 --- a/ros2_ws/src/embr/config/sensors_mixed.json +++ b/ros2_ws/src/embr/config/sensors_mixed.json @@ -4,7 +4,8 @@ "temperature": { "mode": "real", "device": "/dev/ttyACM0", - "baud": 9600 + "baud": 9600, + "params": "" }, "cube": { @@ -19,6 +20,7 @@ }, "mavlink": { - "mode": "sim" + "mode": "sim", + "params": "" } } diff --git a/ros2_ws/src/embr/config/sensors_sim.json b/ros2_ws/src/embr/config/sensors_sim.json index b1cf562..08793ef 100644 --- a/ros2_ws/src/embr/config/sensors_sim.json +++ b/ros2_ws/src/embr/config/sensors_sim.json @@ -22,6 +22,19 @@ }, "radio": { - "mode": "sim" + "mode": "sim", + "params": "" + }, + + "probeMotor": { + "mode": "sim", + "device": "", + "baud": 0, + "params": { + "max_velocity": 768000, + "acceleration": 1000000, + "steps_per_distance": 51200, + "motion_range": 10 + } } } diff --git a/ros2_ws/src/embr/embr/ProbeMotor.py b/ros2_ws/src/embr/embr/ProbeMotor.py new file mode 100644 index 0000000..c455fcd --- /dev/null +++ b/ros2_ws/src/embr/embr/ProbeMotor.py @@ -0,0 +1,121 @@ +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +from typing import Optional +from std_msgs.msg import Int32 +from msg_interface.msg import ProbeMotorFeedback +from embr.sensors import create_sensor +from rcl_interfaces.msg import SetParametersResult + +class ProbeMotor(Node): + + def __init__(self): + super().__init__("probe_motor_server") + + self._log = self.get_logger() + + self.callback_group = ReentrantCallbackGroup() + + # Declare parameter for config file path + self.declare_parameter('config_file', '') + config_path = self.get_parameter('config_file').value + + # Create probeMotor interface + try: + self.probeMotor = create_sensor('probeMotor', config_path) + mode_type = 'simulated' if isinstance(self.probeMotor.__class__.__name__, str) and 'Sim' in self.probeMotor.__class__.__name__ else 'real' + self._log.info(f'probe motor initialized in {mode_type} mode (using {mode_type} probe motor)') + self.probeMotor.start() + except Exception as e: + self._log.error(f'Failed to initialize probe motor: {e}') + + config = self.probeMotor.CONFIG + + self.declare_parameter('motion_range', config["motion_range"]['value']) + self.declare_parameter('max_velocity', config['max_velocity']['value']) + self.declare_parameter('acceleration', config['acceleration']['value']) + + self.add_on_set_parameters_callback(self.parameter_callback) + + # move probe topic + self.moveTopic = self.create_subscription(Int32, 'move_probe', self.move_callback ,10, callback_group=self.callback_group) + + # post-move probe feedback topic + self.feedbackTopic = self.create_publisher(ProbeMotorFeedback, 'probe_motor_feedback',10) + + def parameter_callback(self, params): + + for param in params: + try: + self.probeMotor.setConfigSetting(param.name, param.value) + except Exception as e: + return SetParametersResult(successful=False, reason=str(e)) + + return SetParametersResult(successful=True) + + async def move_callback(self, received_msg : Optional[Int32] = None) -> None: + """ + Callback that handles move probe publishes. Commands the probe motor to move via its interface. + Publishes the feedback of probe motor after a command. + + Args: + received_msg(Int32): The position to move the probe to in cm. + """ + response_msg = ProbeMotorFeedback() + + move_position = received_msg.data + + try: + originalPosition = self.probeMotor.readPosition() + + if originalPosition == move_position: + self._log.info(f"probe already at position {originalPosition}") + response_msg.has_moved = False + response_msg.position = originalPosition + + else: + await self.probeMotor.moveAbsolute(move_position) + newPosition = self.probeMotor.readPosition() + + response_msg.position = newPosition + response_msg.has_moved = newPosition != originalPosition + + self._log.info(f"probe moved {abs((originalPosition - newPosition) / (move_position - originalPosition) * 100)}%, position: {newPosition}cm") + + except (Exception, ValueError) as e: + self._log.error(f"Communication failed with probe motor: {e}") + + except RuntimeError as e: + self._log.error(e) + + self.feedbackTopic.publish(response_msg) + + def shutdown(self) -> None: + """ + Terminate the connection and interface with the probe motor. + """ + self.probeMotor.stop() + + + +def main(args=None): + rclpy.init(args=args) + probeMotor_node = ProbeMotor() + + executor = MultiThreadedExecutor() + executor.add_node(probeMotor_node) + try: + executor.spin() + + except KeyboardInterrupt as e: + pass + + finally: + probeMotor_node.shutdown() # close connection to probe motor before destroying node + probeMotor_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/embr/embr/getCube.py b/ros2_ws/src/embr/embr/getCube.py index ec562e8..56bc2a3 100755 --- a/ros2_ws/src/embr/embr/getCube.py +++ b/ros2_ws/src/embr/embr/getCube.py @@ -7,7 +7,7 @@ from rclpy.node import Node import time from msg_interface.msg import Gps -from embr.sensors import create_sensor, SensorConfig, SensorFactory +from embr.sensors import create_sensor, SensorConfig class AttitudePublisher(Node): @@ -16,33 +16,15 @@ def __init__(self): # Declare parameter for config file path self.declare_parameter('config_file', '') - config_file = self.get_parameter('config_file').value - - # Use default config path if not provided - if not config_file: - config_file = 'src/embr/config/sensors.json' - - # Try to load from config file - try: - configs = SensorFactory.load_config(config_file) - config = configs.get('cube') - - if config is None: - self.get_logger().error(f'Cube sensor not found in config file: {config_file}') - raise ValueError(f'Cube sensor configuration not found in {config_file}') - - self.get_logger().info(f'Loaded cube config from {config_file}') - except Exception as e: - self.get_logger().error(f'Failed to load config file: {e}') - raise + config_path = self.get_parameter('config_file').value # Create sensor try: - self.sensor = create_sensor('cube', config) + self.sensor = create_sensor('cube', config_path) self.sensor.start() - sensor_type = 'simulated' if 'Sim' in self.sensor.__class__.__name__ else 'real' - self.get_logger().info(f'Cube sensor initialized in {config.mode} mode (using {sensor_type} sensor)') + mode_type = 'simulated' if 'Sim' in self.sensor.__class__.__name__ else 'real' + self.get_logger().info(f'Cube sensor initialized in {mode_type} mode (using {mode_type} sensor)') except Exception as e: self.get_logger().error(f'Failed to initialize sensor: {e}') raise diff --git a/ros2_ws/src/embr/embr/getTemp.py b/ros2_ws/src/embr/embr/getTemp.py index c7ca4d2..09fffc0 100644 --- a/ros2_ws/src/embr/embr/getTemp.py +++ b/ros2_ws/src/embr/embr/getTemp.py @@ -15,33 +15,15 @@ def __init__(self): # Declare parameter for config file path self.declare_parameter('config_file', '') - config_file = self.get_parameter('config_file').value - - # Use default config path if not provided - if not config_file: - config_file = 'src/embr/config/sensors.json' - - # Try to load from config file - try: - configs = SensorFactory.load_config(config_file) - config = configs.get('temperature') - - if config is None: - self.get_logger().error(f'Temperature sensor not found in config file: {config_file}') - raise ValueError(f'Temperature sensor configuration not found in {config_file}') - - self.get_logger().info(f'Loaded temperature config from {config_file}') - except Exception as e: - self.get_logger().error(f'Failed to load config file: {e}') - raise - + config_path = self.get_parameter('config_file').value + # Create sensor try: - self.sensor = create_sensor('temperature', config) + self.sensor = create_sensor('temperature', config_path) self.sensor.start() - sensor_type = 'simulated' if isinstance(self.sensor.__class__.__name__, str) and 'Sim' in self.sensor.__class__.__name__ else 'real' - self.get_logger().info(f'Temperature sensor initialized in {config.mode} mode (using {sensor_type} sensor)') + mode_type = 'simulated' if isinstance(self.sensor.__class__.__name__, str) and 'Sim' in self.sensor.__class__.__name__ else 'real' + self.get_logger().info(f'Temperature sensor initialized in {mode_type} mode (using {mode_type} sensor)') except Exception as e: self.get_logger().error(f'Failed to initialize sensor: {e}') raise diff --git a/ros2_ws/src/embr/embr/nav.py b/ros2_ws/src/embr/embr/nav.py new file mode 100644 index 0000000..1f90457 --- /dev/null +++ b/ros2_ws/src/embr/embr/nav.py @@ -0,0 +1,81 @@ +import time +import threading +import random + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Int32 +from msg_interface.msg import ProbeMotorFeedback + +class Nav(Node): + + def __init__(self): + super().__init__('nav') + self.moveProbeMotorTopic = self.create_publisher(Int32, 'move_probe', 10) + self.probeMotorFeedbackTopic = self.create_subscription(ProbeMotorFeedback, 'probe_motor_feedback', self.probeMotorFeedback_callback, 10) + self.probe_motor_feedback_event = threading.Event() + + self.survey_thread = threading.Thread(target=self.startSurvey) + self.survey_thread.start() + + def probeMotorFeedback_callback(self, feedback): + self.probe_motor_feedback_event.has_moved = feedback.has_moved + self.probe_motor_feedback_event.position = feedback.position + self.probe_motor_feedback_event.set() + + + + + def startSurvey(self): + + # to do: initiate motion and related components + + # trigger probe to move + # below is an example where the probe is randomly triggered to move + while(True): + self.get_logger().info("searching for hotspot") + + time.sleep(random.randint(10,30)) + self.get_logger().info("found a hotspot") + self.get_logger().info("moving towards hotspot") + time.sleep(random.randint(5,10)) + self.get_logger().info("reached hotspot") + self.get_logger().info("extending probe") + if self.moveProbe(10): # displace probe to a position 5cm vertically downwards + self.get_logger().info(f"extended probe to {self.probe_motor_feedback_event.position}") + self.get_logger().info("reading temp values") + time.sleep(4) + self.get_logger().info("retracting probe") + if not self.moveProbe(0): + # to do: halt operation to avoid potential harm to probe + self.get_logger().error("Failed to retract probe") + self.get_logger().info("retracted probe") + + def moveProbe(self, toPosition) -> bool: + msg = Int32() + msg.data = toPosition + self.moveProbeMotorTopic.publish(msg) + self.probe_motor_feedback_event.wait() + self.probe_motor_feedback_event.clear() + if not self.probe_motor_feedback_event.has_moved and ( self.probe_motor_feedback_event.position != toPosition ): + self.get_logger().error(f"Probe failed to move position from {self.probe_motor_feedback_event.position} to {toPosition}") + return False + return True + + +def main(args=None): + rclpy.init(args=args) + navNode = Nav() + try: + rclpy.spin(navNode) + + except KeyboardInterrupt: + pass + + finally: + navNode.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/embr/embr/radio.py b/ros2_ws/src/embr/embr/radio.py index 2e9c31e..f54ccbb 100644 --- a/ros2_ws/src/embr/embr/radio.py +++ b/ros2_ws/src/embr/embr/radio.py @@ -15,7 +15,7 @@ from sensor_msgs.msg import PointCloud2 import sensor_msgs_py.point_cloud2 as pc2 -from embr.sensors import create_sensor, SensorConfig, SensorFactory +from embr.sensors import create_sensor, SensorConfig class CommSubscriber(Node): @@ -28,31 +28,13 @@ def __init__(self): self.declare_parameter('config_file', '') config_file = self.get_parameter('config_file').value - # Use default config path if not provided - if not config_file: - config_file = 'src/embr/config/sensors.json' - - # Try to load from config file - try: - configs = SensorFactory.load_config(config_file) - config = configs.get('radio') - - if config is None: - self.get_logger().error(f'Radio config not found in config file: {config_file}') - raise ValueError(f'Radio configuration not found in {config_file}') - - self.get_logger().info(f'Loaded Radio config from {config_file}') - except Exception as e: - self.get_logger().error(f'Failed to load config file: {e}') - raise - # Create Radio connection try: - self.radio_connection = create_sensor('radio', config) + self.radio_connection = create_sensor('radio', config_file) self.radio_connection.start() - conn_type = 'simulated' if 'Sim' in self.radio_connection.__class__.__name__ else 'real' - self.get_logger().info(f'Radio connection initialized in {config.mode} mode (using {conn_type} connection)') + mode_type = 'simulated' if 'Sim' in self.radio_connection.__class__.__name__ else 'real' + self.get_logger().info(f'Radio connection initialized in {mode_type} mode (using {mode_type} connection)') except Exception as e: self.get_logger().error(f'Failed to initialize Radio: {e}') raise @@ -68,6 +50,10 @@ def __init__(self): ) self.get_logger().info('Radio Subscriber node initialized') + + self.radio_connection.read() + self.get_logger().info('done reading') + def temperature_callback(self, msg): """Handle temperature messages.""" diff --git a/ros2_ws/src/embr/embr/sensors/__init__.py b/ros2_ws/src/embr/embr/sensors/__init__.py index 59f4d9f..333606d 100644 --- a/ros2_ws/src/embr/embr/sensors/__init__.py +++ b/ros2_ws/src/embr/embr/sensors/__init__.py @@ -8,6 +8,7 @@ from .temperature import TemperatureSensor, RealTemperatureSensor, SimTemperatureSensor from .cube import CubeSensor, RealCubeSensor, SimCubeSensor from .radio import RadioConnection, RealRadioConnection, SimRadioConnection +from .probeMotor import RealProbeMotor, SimProbeMotor __all__ = [ 'Sensor', @@ -23,4 +24,6 @@ 'RadioConnection', 'RealRadioConnection', 'SimRadioConnection', + 'RealProbeMotor', + 'SimProbeMotor' ] diff --git a/ros2_ws/src/embr/embr/sensors/base.py b/ros2_ws/src/embr/embr/sensors/base.py index 682510b..15e3fd1 100644 --- a/ros2_ws/src/embr/embr/sensors/base.py +++ b/ros2_ws/src/embr/embr/sensors/base.py @@ -8,12 +8,11 @@ @dataclass class SensorConfig: """Configuration for a sensor instance.""" - mode: str = "real" # real, sim - device: Optional[str] = None - baud: int = 9600 - params: Dict[str, Any] = field(default_factory=dict) - - + mode: str # real, sim + device: Optional[str] + baud: int + params: Dict[str, Any] + class Sensor(ABC): """Abstract base class for all sensors.""" diff --git a/ros2_ws/src/embr/embr/sensors/cube.py b/ros2_ws/src/embr/embr/sensors/cube.py index c67d709..020b2d2 100644 --- a/ros2_ws/src/embr/embr/sensors/cube.py +++ b/ros2_ws/src/embr/embr/sensors/cube.py @@ -70,7 +70,7 @@ class SimCubeSensor(CubeSensor): def __init__(self, config: Optional[SensorConfig] = None): super().__init__(config) - params = config.params if config else {} + params = config["params"] if config else {} # Starting position (default: somewhere in US) self.start_lat = params.get('start_lat', 37.7749) self.start_lon = params.get('start_lon', -122.4194) diff --git a/ros2_ws/src/embr/embr/sensors/factory.py b/ros2_ws/src/embr/embr/sensors/factory.py index b477cfb..0a0c904 100644 --- a/ros2_ws/src/embr/embr/sensors/factory.py +++ b/ros2_ws/src/embr/embr/sensors/factory.py @@ -9,7 +9,7 @@ from .temperature import RealTemperatureSensor, SimTemperatureSensor from .cube import RealCubeSensor, SimCubeSensor from .radio import RealRadioConnection, SimRadioConnection - +from .probeMotor import ProbeMotorBase, RealProbeMotor, SimProbeMotor class SensorFactory: """Factory for creating sensor instances.""" @@ -28,15 +28,19 @@ class SensorFactory: 'real': RealRadioConnection, 'sim': SimRadioConnection, }, + 'probeMotor' : { + 'real' : RealProbeMotor, + 'sim': SimProbeMotor, + } } @classmethod - def create(cls, sensor_type: str, config: Optional[SensorConfig] = None) -> Sensor: + def create(cls, sensor_type: str, config_path: Optional[str] = None): """ Create a sensor instance. Args: - sensor_type: Type of sensor ('temperature', 'cube', 'radio') + sensor_type: Type of sensor ('temperature', 'cube', 'radio', 'probeMotor) config: Sensor configuration Returns: @@ -48,8 +52,9 @@ def create(cls, sensor_type: str, config: Optional[SensorConfig] = None) -> Sens """ if sensor_type not in cls.SENSOR_MAP: raise ValueError(f"Unknown sensor type: {sensor_type}") + - config = config or SensorConfig() + config = cls.load_config(sensor_type, config_path) # Determine mode - default to 'real' unless specified otherwise mode = cls._determine_mode(sensor_type, config) @@ -57,7 +62,7 @@ def create(cls, sensor_type: str, config: Optional[SensorConfig] = None) -> Sens # Get the appropriate class sensor_classes = cls.SENSOR_MAP[sensor_type] sensor_class = sensor_classes.get(mode) - + if not sensor_class: raise ValueError(f"Unknown mode '{mode}' for sensor '{sensor_type}'") @@ -87,7 +92,7 @@ def _determine_mode(cls, sensor_type: str, config: SensorConfig) -> str: return 'real' @classmethod - def load_config(cls, config_path: Optional[str] = None) -> Dict[str, SensorConfig]: + def load_config(cls, device_type: str ,config_path: Optional[str] = None) -> SensorConfig: """ Load sensor configuration from JSON file. @@ -97,10 +102,11 @@ def load_config(cls, config_path: Optional[str] = None) -> Dict[str, SensorConfi Returns: Dictionary mapping sensor names to SensorConfig objects """ - if config_path is None: + + if not config_path: # Look for config in common locations within workspace search_paths = [ - 'config/sensors.json', + 'src/embr/config/sensors.json', 'sensors.json', os.path.expanduser('~/.embr/sensors.json'), '/etc/embr/sensors.json', @@ -110,31 +116,14 @@ def load_config(cls, config_path: Optional[str] = None) -> Dict[str, SensorConfi if os.path.exists(path): config_path = path break - + if config_path is None or not os.path.exists(config_path): - return {} + raise RuntimeError(f"Failed to find configuration file: cannot find configuration file from {config_path} or default config paths") try: with open(config_path, 'r') as f: data = json.load(f) - - configs = {} - for sensor_name, sensor_data in data.items(): - # Skip non-sensor entries (like comments or metadata) - if not isinstance(sensor_data, dict): - continue - - # Get mode from config, default to 'real' - mode = sensor_data.get('mode', 'real') - - configs[sensor_name] = SensorConfig( - mode=mode, - device=sensor_data.get('device'), - baud=sensor_data.get('baud', 9600), - params=sensor_data.get('params', {}) - ) - - return configs + return SensorConfig(**data[device_type]) except Exception as e: raise RuntimeError(f"Failed to load config from {config_path}: {e}") @@ -152,3 +141,4 @@ def create_sensor(sensor_type: str, config: Optional[SensorConfig] = None) -> Se Sensor instance """ return SensorFactory.create(sensor_type, config) + \ No newline at end of file diff --git a/ros2_ws/src/embr/embr/sensors/probeMotor.py b/ros2_ws/src/embr/embr/sensors/probeMotor.py new file mode 100644 index 0000000..79eb9ef --- /dev/null +++ b/ros2_ws/src/embr/embr/sensors/probeMotor.py @@ -0,0 +1,302 @@ +### to do : handle case where physical connection to motor is lost during operation + +from abc import ABC, abstractmethod +from typing import Any, Dict, Optional +from dataclasses import dataclass, field + +import time # for time.sleep() in SimProbeMotor::move() +import random + +from pymodbus.client import ModbusTcpClient + +from embr.sensors import SensorConfig +from embr.sensors import Sensor + +class ProbeMotorBase(Sensor): + + CONFIG = { # settings : {value,range} + "steps_per_distance" : { # unit in step/centi meter, translation of + "value" : 51200, # rotation(steps) to vertical displacement + "range" : [0,51200] + }, + "motion_range" : { # unit in centi meter + "value" : 10, + "range" : [0,10] + }, + "max_velocity" : { # unit in step/second + "value" : 768000, + "range" : [0, 768000] + }, + "acceleration" : { # unit in step/second^2 + "value" : 1000000, + "range" : [0,1000000] + } + } + + def __init__(self, config: Optional[SensorConfig] = None): + super().__init__(config) + + # start to update the running state + self.start() + + # retrieve all the parameters from the supplied config + params = config.params + + # update the respective config settings using the parameters + if params: + for setting, value in params.items(): + if params[setting]: + self.setConfigSetting(setting, value) + else: + raise ValueError(f"Invalid configuration setting") + else: + raise ValueError(f"Probe motor configuration is empty") + + @abstractmethod + async def moveAbsolute(self, distance : int) -> None: + """ + Spin the motor in steps equivalent to the translated vertical distance of arg-distance. + + Args: + distance (int): distance in cm. + """ + + pass + + @abstractmethod + def setConfigSetting(self, setting : str, value : int) -> None: + """ + Sets the config setting value to arg-value and update the probe motor's setting if required. + + Args: + setting (string): Setting label + value (int): Setting value + """ + pass + + @property + def is_running(self) -> bool: + """Check if probe motor is running.""" + # to do: override this RealProbeMotor to also check self.client.connected + return self._running + + def set_running(self) -> None: + self._running = True + + def stop_running(self) -> None: + self._running = False + + def getConfig(): + return self.config + + + +class RealProbeMotor(ProbeMotorBase): + + DRIVE_IP = '192.168.33.1' # default drive IP + DRIVE_PORT = 502 # Standard MODBUS TCP port + + REGISTERS = { #length = number of registers + "move_absolute" : {"address" : 0x0043, "length" : 2}, + "position_counter" : {"address" : 0x0057, "length" : 2}, + "max_velocity" : {"address" : 0x008B, "length" : 2}, + "acceleration" : {"address" : 0x0000, "length" : 2}, + "moving" : {"address" : 0x004A, "length" : 1}, + } + + def __init__(self, config: Optional[SensorConfig] = None): + super().__init__(config) + + def _write_registers(self, register, value: int) -> None: + """ + Directly communicate with the probe motor via ethernet by manipulating the probe motor + registers to command it, change configuration, or mode. + + Args: + register (dict[str, [str: int, str: int]]): Register dictionary containing + the register address and number + of relavent registers + value (int): The value to write to the respective register(s). + """ + + if register["length"] == 1: + response = self.client.write_registers(register["address"], [value]) + + + elif register["length"] == 2: + # encode int_32 into two int_16 values(one lower 16-bits, one upper 16-bits) + high = (value >> 16) & 0xFFFF + low = value & 0xFFFF + response = self.client.write_registers(register["address"], [low,high]) + + if response.isError(): + raise RuntimeError(f"Error writing {value} to register {register}") + + + def _read_registers(self, register): + if register["length"] == 1: + response = self.client.read_holding_registers(register["address"], count = 1) + + if not response.isError(): + return response, response.registers[0] + + else: + raise RuntimeError(f"Error reading from register {register}") + + elif register["length"] == 2: + response = self.client.read_holding_registers(register["address"], count = 2) + + if not response.isError(): + # decode two int_16 values(one lower 16-bits, one upper 16-bits) into one int_32 value + value = response.registers[0] | (response.registers[1] << 16) + return response, value + + else: + raise RuntimeError(f"Error reading from register {register}") + + + def start(self) -> None: + #Establish connection to the probe motor + self.client = ModbusTcpClient(self.DRIVE_IP, port=self.DRIVE_PORT) + self.client.connect() + + if not self.client.connected: + raise RuntimeError("Failed to establish connection with probe motor") + + self.set_running() + + + async def _waitTillMoveFinish(self) -> None: + """ + Meant to be used by self.moveAbsolute(distance). + Wait till probe motor has finished moving to specified position. + """ + + # Assume (reasonably) probe motor is moving after a successful move command + is_moving = True + + # check every half a second for the move status of the probe motor + while(is_moving): + time.sleep(0.5) + response, value = self._read_registers(self.REGISTERS["moving"]) + is_moving = value + + + async def moveAbsolute(self, position) -> None: + if not self.is_running: + raise Exception("probe motor is not running") + + motion_range = self.CONFIG["motion_range"]["value"] + if not (0 <= position <= motion_range): + raise ValueError(f"Position {position}cm is out of motion range [0, {motion_range}]") + + positionInSteps = position * self.CONFIG["steps_per_distance"]["value"] + + # spin the motor by number of steps + self._write_registers(self.REGISTERS["move_absolute"], positionInSteps) + + await self._waitTillMoveFinish() + return True + + + def readPosition(self) -> int: + if not self.is_running: + raise Exception("probe motor is not running") + + response, positionInSteps = self._read_registers(self.REGISTERS["position_counter"]) + + positionInDistance = positionInSteps / self.CONFIG["steps_per_distance"]["value"] + + return int(positionInDistance) + + def read(self) -> int: + return readPosition() + + + def setConfigSetting(self, setting : str, value : int) -> None: + if setting in self.REGISTERS: + lowerbound = self.CONFIG[setting]['range'][0] + upperbound = self.CONFIG[setting]['range'][1] + + if not (lowerbound <= value <= upperbound): + raise ValueError(f'{setting} value {value} is not in range [{lowerbound}, {upperbound}]') + + self._write_registers(self.REGISTERS[setting], value) + self.CONFIG[setting]["value"] = value + + else: + if self.CONFIG[setting]["value"]: + self.CONFIG[setting]["value"] = value + + else: + raise ValueError(f"No such {setting} setting in config") + + + def stop(self) -> bool: + if not self.is_running: + raise Exception("No active connection to probe motor") + + # disconnect from the drive + self.client.close() + + self.stop_running() + + +class SimProbeMotor(ProbeMotorBase): + TIME_PER_DISTANCE = 0.5 # in second/distance + POSITION = 0 # in steps + + def __init__(self, config: Optional[SensorConfig] = None): + super().__init__(config) + + def start(self) -> None: + if not self.is_running: + self.set_running() + + + async def moveAbsolute(self, position) -> bool: + if not self.is_running: + raise Exception("No active connection to probe motor") + + if position > self.POSITION: + # apply a 90% probability that the probe doesn't fully extend to the provided position + if random.random() > 0.9: + position = random.randint(0,position) + + # sleep to mimic the time a motor would take to complete spin + time.sleep(abs((position + - (self.POSITION/self.CONFIG["steps_per_distance"]["value"] if self.POSITION else 0)) + * self.TIME_PER_DISTANCE)) # /100 to convert mili sec to sec + + # update the position by converting received position from cm to steps + self.POSITION = position * self.CONFIG["steps_per_distance"]["value"] + + + def readPosition(self) -> int: + return int(self.POSITION / self.CONFIG["steps_per_distance"]["value"]) + + def read(self) -> int: + return readPosition() + + + def setConfigSetting(self, setting : str, value : int) -> None: + + if not self.CONFIG[setting]: + raise ValueError(f"No such {setting} setting in config") + + else: + lowerbound = self.CONFIG[setting]['range'][0] + upperbound = self.CONFIG[setting]['range'][1] + + if not (lowerbound <= value <= upperbound): + raise Exception(f'{setting} value {value} is not in range [{lowerbound}, {upperbound}]') + + self.CONFIG[setting]["value"] = value + + + def stop(self) -> None: + if not self.is_running: + raise Exception("No active connection to probe motor") + + self.stop_running() + \ No newline at end of file diff --git a/ros2_ws/src/embr/embr/sensors/temperature.py b/ros2_ws/src/embr/embr/sensors/temperature.py index 5b654a6..7d8522e 100644 --- a/ros2_ws/src/embr/embr/sensors/temperature.py +++ b/ros2_ws/src/embr/embr/sensors/temperature.py @@ -17,8 +17,8 @@ class RealTemperatureSensor(TemperatureSensor): def __init__(self, config: Optional[SensorConfig] = None): super().__init__(config) self.ser = None - self.device = config.device if config else '/dev/ttyACM0' - self.baud = config.baud if config else 9600 + self.device = config["device"] if config else '/dev/ttyACM0' + self.baud = config["baud"] if config else 9600 def start(self) -> None: """Open serial connection to temperature sensor.""" diff --git a/ros2_ws/src/embr/package.xml b/ros2_ws/src/embr/package.xml index 0e3aa1f..dd4fb88 100644 --- a/ros2_ws/src/embr/package.xml +++ b/ros2_ws/src/embr/package.xml @@ -6,6 +6,9 @@ TODO: Package description ubuntu Apache-2.0 + + pymodbus + rclpy msg_interface sensor_msgs diff --git a/ros2_ws/src/embr/setup.py b/ros2_ws/src/embr/setup.py index ea861cd..8902758 100644 --- a/ros2_ws/src/embr/setup.py +++ b/ros2_ws/src/embr/setup.py @@ -25,7 +25,9 @@ 'console_scripts': [ 'getCube = embr.getCube:main', 'getTemp = embr.getTemp:main', - 'radio = embr.radio:main' + 'radio = embr.radio:main', + 'probeMotor = embr.ProbeMotor:main', + 'nav = embr.nav:main' ], }, ) diff --git a/ros2_ws/src/msg_interface/CMakeLists.txt b/ros2_ws/src/msg_interface/CMakeLists.txt index 7d22076..852f5ef 100644 --- a/ros2_ws/src/msg_interface/CMakeLists.txt +++ b/ros2_ws/src/msg_interface/CMakeLists.txt @@ -11,7 +11,9 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Gps.msg" + "msg/ProbeMotorFeedback.msg" ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/ros2_ws/src/msg_interface/msg/ProbeMotorFeedback.msg b/ros2_ws/src/msg_interface/msg/ProbeMotorFeedback.msg new file mode 100644 index 0000000..442d041 --- /dev/null +++ b/ros2_ws/src/msg_interface/msg/ProbeMotorFeedback.msg @@ -0,0 +1,2 @@ +bool has_moved +int32 position