From 73295bcb28315a14d81879aee9c85da46482cf37 Mon Sep 17 00:00:00 2001 From: Eugene Date: Mon, 22 Dec 2025 03:35:48 +0300 Subject: [PATCH] Initial commit --- Dockerfile | 34 +++++ docker-compose.yml | 23 ++++ ultrasonic_sensor/package.xml | 22 ++++ ultrasonic_sensor/resource/ultrasonic_sensor | 0 ultrasonic_sensor/setup.cfg | 5 + ultrasonic_sensor/setup.py | 27 ++++ .../ultrasonic_sensor/__init__.py | 0 .../ultrasonic_sensor/ultrasonic_node.py | 122 ++++++++++++++++++ 8 files changed, 233 insertions(+) create mode 100644 Dockerfile create mode 100644 docker-compose.yml create mode 100644 ultrasonic_sensor/package.xml create mode 100644 ultrasonic_sensor/resource/ultrasonic_sensor create mode 100644 ultrasonic_sensor/setup.cfg create mode 100644 ultrasonic_sensor/setup.py create mode 100644 ultrasonic_sensor/ultrasonic_sensor/__init__.py create mode 100644 ultrasonic_sensor/ultrasonic_sensor/ultrasonic_node.py diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..b17b4ea --- /dev/null +++ b/Dockerfile @@ -0,0 +1,34 @@ +FROM ros:jazzy-ros-base-noble + +# Install pyserial +RUN apt-get update && apt-get install -y \ + python3-pip \ + && rm -rf /var/lib/apt/lists/* + +RUN pip3 install --break-system-packages pyserial + +# Create workspace +WORKDIR /ros2_ws + +# Copy the package +COPY ultrasonic_sensor /ros2_ws/src/ultrasonic_sensor + +# Build the package +RUN . /opt/ros/jazzy/setup.sh && \ + colcon build --packages-select ultrasonic_sensor + +# Source setup in bashrc for interactive use +RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ + echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc + +# Create entrypoint script +RUN echo '#!/bin/bash\n\ +set -e\n\ +source /opt/ros/jazzy/setup.bash\n\ +source /ros2_ws/install/setup.bash\n\ +exec "$@"' > /ros2_entrypoint.sh && \ + chmod +x /ros2_entrypoint.sh + +ENTRYPOINT ["/ros2_entrypoint.sh"] +CMD ["ros2", "run", "ultrasonic_sensor", "ultrasonic_node"] + diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 0000000..398cc11 --- /dev/null +++ b/docker-compose.yml @@ -0,0 +1,23 @@ +services: + ultrasonic_sensor: + build: + context: . + dockerfile: Dockerfile + container_name: ultrasonic_sensor_node + devices: + - /dev/ttyUSB0:/dev/ttyUSB0 + # Alternative: use privileged mode if device passthrough doesn't work + # privileged: true + environment: + - ROS_DOMAIN_ID=0 + # Override parameters if needed + # command: > + # ros2 run ultrasonic_sensor ultrasonic_node + # --ros-args + # -p serial_port:=/dev/ttyUSB0 + # -p baudrate:=38400 + restart: unless-stopped + # For debugging, enable interactive mode + # stdin_open: true + # tty: true + diff --git a/ultrasonic_sensor/package.xml b/ultrasonic_sensor/package.xml new file mode 100644 index 0000000..8a31293 --- /dev/null +++ b/ultrasonic_sensor/package.xml @@ -0,0 +1,22 @@ + + + + ultrasonic_sensor + 1.0.0 + ROS2 node for reading and publishing ultrasonic sensor data via serial port + Evgenii Rybalkin + MIT + + rclpy + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + + diff --git a/ultrasonic_sensor/resource/ultrasonic_sensor b/ultrasonic_sensor/resource/ultrasonic_sensor new file mode 100644 index 0000000..e69de29 diff --git a/ultrasonic_sensor/setup.cfg b/ultrasonic_sensor/setup.cfg new file mode 100644 index 0000000..23a76ea --- /dev/null +++ b/ultrasonic_sensor/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/ultrasonic_sensor +[install] +install_scripts=$base/lib/ultrasonic_sensor + diff --git a/ultrasonic_sensor/setup.py b/ultrasonic_sensor/setup.py new file mode 100644 index 0000000..6b878a0 --- /dev/null +++ b/ultrasonic_sensor/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'ultrasonic_sensor' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='User', + maintainer_email='user@example.com', + description='ROS2 node for reading and publishing ultrasonic sensor data via serial port', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'ultrasonic_node = ultrasonic_sensor.ultrasonic_node:main', + ], + }, +) + diff --git a/ultrasonic_sensor/ultrasonic_sensor/__init__.py b/ultrasonic_sensor/ultrasonic_sensor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ultrasonic_sensor/ultrasonic_sensor/ultrasonic_node.py b/ultrasonic_sensor/ultrasonic_sensor/ultrasonic_node.py new file mode 100644 index 0000000..22f946b --- /dev/null +++ b/ultrasonic_sensor/ultrasonic_sensor/ultrasonic_node.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +"""ROS2 node for reading ultrasonic sensor data from serial port and publishing.""" + +import rclpy +from rclpy.node import Node +from std_msgs.msg import UInt8MultiArray + +import serial +import serial.tools.list_ports + + +class UltrasonicSensorNode(Node): + """ROS2 node that reads serial data from ultrasonic sensor and publishes it.""" + + def __init__(self): + super().__init__('ultrasonic_sensor_node') + + # Declare parameters + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('baudrate', 38400) + + # Get parameters + self.serial_port = self.get_parameter('serial_port').get_parameter_value().string_value + self.baudrate = self.get_parameter('baudrate').get_parameter_value().integer_value + + # Serial connection + self.ser = None + self.is_connected = False + + # Publisher + self.publisher = self.create_publisher( + UInt8MultiArray, + '/ultrasonic/raw_data', + 10 + ) + + # Statistics + self.byte_count = 0 + + # Connect to serial port + self.connect_serial() + + # Timer for reading serial data (10ms interval) + self.timer = self.create_timer(0.01, self.read_serial_data) + + self.get_logger().info( + f'Ultrasonic sensor node started. ' + f'Port: {self.serial_port}, Baudrate: {self.baudrate}' + ) + + def connect_serial(self): + """Connect to the serial port.""" + try: + self.ser = serial.Serial( + port=self.serial_port, + baudrate=self.baudrate, + bytesize=serial.EIGHTBITS, + parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_TWO, + timeout=1 + ) + self.is_connected = True + self.get_logger().info(f'Connected to {self.serial_port}') + except serial.SerialException as e: + self.is_connected = False + self.get_logger().error(f'Failed to connect to {self.serial_port}: {e}') + self.get_logger().info('Available ports:') + for port in serial.tools.list_ports.comports(): + self.get_logger().info(f' - {port.device}: {port.description}') + + def read_serial_data(self): + """Read data from serial port and publish.""" + if not self.is_connected or self.ser is None: + return + + try: + if self.ser.in_waiting > 0: + # Read all available data + data = self.ser.read(self.ser.in_waiting) + self.byte_count += len(data) + + if len(data) > 0: + # Create and publish message + msg = UInt8MultiArray() + msg.data = list(data) + self.publisher.publish(msg) + + # Log hex representation for debugging + hex_data = data.hex(' ', 1) + self.get_logger().debug( + f'Published {len(data)} bytes: {hex_data}' + ) + + except serial.SerialException as e: + self.get_logger().error(f'Serial read error: {e}') + self.is_connected = False + + def destroy_node(self): + """Clean up resources.""" + if self.ser and self.ser.is_open: + self.ser.close() + self.get_logger().info('Serial connection closed') + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + + node = UltrasonicSensorNode() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() +