diff --git a/Dockerfile b/Dockerfile index d2428c5..e949f8d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -2,20 +2,19 @@ ARG ROS_DISTRO=humble ARG PREFIX= +# =========================== package builder =============================== FROM husarnet/ros:$ROS_DISTRO-ros-base AS pkg-builder -SHELL ["/bin/bash", "-c"] - WORKDIR /ros2_ws -# install everything needed +# Setup workspace RUN git clone https://github.com/Slamtec/sllidar_ros2.git /ros2_ws/src/sllidar_ros2 && \ cp /ros2_ws/src/sllidar_ros2/launch/sllidar_a1_launch.py \ /ros2_ws/src/sllidar_ros2/launch/sllidar_launch.py && \ rosdep update --rosdistro $ROS_DISTRO && \ rosdep install --from-paths src --ignore-src -y -# Create health check package +# Create healthcheck package RUN cd src/ && \ source /opt/ros/$ROS_DISTRO/setup.bash && \ ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp sensor_msgs && \ @@ -25,34 +24,25 @@ RUN cd src/ && \ install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \ /ros2_ws/src/healthcheck_pkg/CMakeLists.txt -COPY ./healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ +COPY ./husarion_utils/healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ # Build RUN source /opt/ros/$ROS_DISTRO/setup.bash && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + echo $(cat /ros2_ws/src/sllidar_ros2/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') > /version.txt && \ rm -rf build log -# Second stage - Deploy the built packages -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core +# =========================== final stage =============================== +FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core AS final-stage ARG PREFIX -ENV PREFIX_ENV=$PREFIX - -SHELL ["/bin/bash", "-c"] COPY --from=pkg-builder /ros2_ws /ros2_ws +COPY --from=pkg-builder /version.txt /version.txt +COPY ./husarion_utils /husarion_utils -RUN echo $(cat /ros2_ws/src/sllidar_ros2/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') > /version.txt - -# Run healthcheck in background -RUN entrypoint_file=$(if [ -f "/ros_entrypoint.sh" ]; then echo "/ros_entrypoint.sh"; else echo "/vulcanexus_entrypoint.sh"; fi) && \ - sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \ - ros2 run healthcheck_pkg healthcheck_node &' \ - $entrypoint_file - -COPY ./healthcheck.sh / HEALTHCHECK --interval=2s --timeout=1s --start-period=20s --retries=1 \ - CMD ["/healthcheck.sh"] + CMD ["/husarion_utils/healthcheck.sh"] # Ensure LIDAR stops spinning on container shutdown STOPSIGNAL SIGINT diff --git a/README.md b/README.md index 5af1455..9f49b40 100644 --- a/README.md +++ b/README.md @@ -6,51 +6,70 @@ The repository includes a GitHub Actions workflow that automatically deploys bui [![ROS Docker Image](https://github.com/husarion/rplidar-docker/actions/workflows/ros-docker-image.yaml/badge.svg)](https://github.com/husarion/rplidar-docker/actions/workflows//ros-docker-image.yaml) - ## Prepare Environment -**1. Plugin the Device** +1. Plugin the Device You can use `lsusb` command to check if the device is visible. ## Demo -**1. Clone the Repository** +1. Clone the Repository + + ```bash + git clone https://github.com/husarion/rplidar-docker.git + cd rplidar-docker/demo + ``` + +2. Select the Appropriate Baudrate + + ```bash + export RPLIDAR_BAUDRATE= + ``` + + Replace `` with appropriate baudrate for your LiDAR from below table. + + | **Product Name** | **Baudrate** | + | ---------------- | ------------- | + | RPlidar A2M8 | **`115200`** | + | RPlidar A2M12 | **`256000`** | + | RPlidar A3 | **`256000`** | + | RPlidar S1 | **`256000`** | + | RPlidar S2 | **`1000000`** | + | RPlidar S3 | **`1000000`** | + +3. Activate the Device -```bash -git clone https://github.com/husarion/rplidar-docker.git -cd rplidar-docker/demo -``` + ```bash + docker compose up rplidar + ``` -**2. Select the Appropriate Baudrate** +4. Launch Visualization -```bash -export RPLIDAR_BAUDRATE= -``` + ```bash + xhost local:root + docker compose up rviz + ``` -Replace `` with appropriate baudrate for your LiDAR from below table. + > [!NOTE] + > To use the latest version of the image, run the `docker compose pull` command. -| **Product Name** | **Baudrate** | -| ---------------- | ------------- | -| RPlidar A2M8 | **`115200`** | -| RPlidar A2M12 | **`256000`** | -| RPlidar A3 | **`256000`** | -| RPlidar S1 | **`256000`** | -| RPlidar S2 | **`1000000`** | -| RPlidar S3 | **`1000000`** | +## Parameters -**3. Activate the Device** +The original launch has been modified with the new parameters: -```bash -docker compose up rplidar -``` +| **Product Name** | **Description** | **Default Value** | +| ------------------ | ------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------- | +| `launch_file` | Name of launch file from repo `sllidar_ros2` to run | `sllidar_launch.py` | +| `serial_baudrate` | Baudrate of connected lidar (depend on your RPlidar model) | `115200` | +| `serial_port` | USB port of connected lidar | `/dev/ttyUSB0` | +| `robot_namespace` | Namespace which will appear in front of all topics (including `/tf` and `/tf_static`). | `env("ROS_NAMESPACE")` (`""` if not specified) | +| `device_namespace` | Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot. | `""` | -**4. Launch Visualization** +Using both `device_namespace` and `robot_namespace` makes: -```bash -xhost local:root -docker compose up rviz -``` +- Topic: `///` +- Topic TF: `//tf` +- URDF Links: `_link` -> [!NOTE] -> You can run the visualization on any device, provided that it is connected to the computer to which the sensor is connected. +If any of the namespaces are missing, the field with `/` is omitted for topics, or replaced with default ones for URDF links. diff --git a/demo/.env b/demo/.env new file mode 100644 index 0000000..2020522 --- /dev/null +++ b/demo/.env @@ -0,0 +1,7 @@ +# ======================================= +# Namespace config +# ======================================= +# To use multimle devices on multible robot you have to specify following field + +# Uncomment to add robot namespace +# ROS_NAMESPACE=robot diff --git a/demo/compose.yaml b/demo/compose.yaml index c74a0ab..9c32e78 100644 --- a/demo/compose.yaml +++ b/demo/compose.yaml @@ -1,32 +1,33 @@ -# Quick Start -# docker compose up +x-common-config: + &common-config + network_mode: host + ipc: host + env_file: .env services: - rplidar: - image: husarion/rplidar:humble - restart: unless-stopped - network_mode: host - ipc: host + # image: husarion/rplidar:humble + build: + context: .. + dockerfile: Dockerfile + <<: *common-config devices: - /dev/ttyUSB0 - environment: - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0} - - RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp} command: > ros2 launch sllidar_ros2 sllidar_launch.py - serial_baudrate:=${RPLIDAR_BAUDRATE:-256000} + serial_baudrate:=${RPLIDAR_BAUDRATE:-115200} + device_namespace:=lidar rviz: image: husarion/rviz2:humble - restart: on-failure - network_mode: host - ipc: host + <<: *common-config volumes: - /tmp/.X11-unix:/tmp/.X11-unix:rw - ./default.rviz:/root/.rviz2/default.rviz + - ./rviz.launch.py:/ros2_ws/rviz.launch.py environment: - DISPLAY=${DISPLAY:?err} - LIBGL_ALWAYS_SOFTWARE=1 - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0} - - RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp} + command: > + ros2 launch rviz.launch.py + device_namespace:=lidar diff --git a/demo/default.rviz b/demo/default.rviz index 2fe6850..2c3440a 100644 --- a/demo/default.rviz +++ b/demo/default.rviz @@ -77,7 +77,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /scan + Value: scan Use Fixed Frame: true Use rainbow: true Value: true diff --git a/demo/rviz.launch.py b/demo/rviz.launch.py new file mode 100644 index 0000000..82ee370 --- /dev/null +++ b/demo/rviz.launch.py @@ -0,0 +1,66 @@ +from launch import LaunchDescription +from launch_ros.actions import Node, PushRosNamespace +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import EnvironmentVariable, LaunchConfiguration + + +def replace_rviz_fixed_frame(input_file, output_file, fix_frame): + with open(input_file, "r") as file: + rviz_content = file.read() + + rviz_content = rviz_content.replace("Fixed Frame: laser", f"Fixed Frame: {fix_frame}") + + with open(output_file, "w") as file: + file.write(rviz_content) + + +def launch_setup(context, *args, **kwargs): + robot_namespace = LaunchConfiguration("robot_namespace").perform(context) + device_namespace = LaunchConfiguration("device_namespace").perform(context) + + default_rviz_conf = "/root/.rviz2/default.rviz" + rviz_conf = "/root/.rviz2/modified_default.rviz" + if device_namespace: + frame_id = f"{device_namespace}_link" + else: + frame_id = "laser" + replace_rviz_fixed_frame(default_rviz_conf, rviz_conf, frame_id) + + remapping = [] + if robot_namespace: + remapping.append(("/clicked_point", f"/{robot_namespace}/clicked_point")) + remapping.append(("/goal_pose", f"/{robot_namespace}/goal_pose")) + remapping.append(("/initialpose", f"/{robot_namespace}/initialpose")) + remapping.append(("/tf", f"/{robot_namespace}/tf")) + remapping.append(("/tf_static", f"/{robot_namespace}/tf_static")) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=device_namespace, + remappings=remapping, + arguments=["-d", rviz_conf], + output="screen", + ) + + return [PushRosNamespace(robot_namespace), rviz] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "robot_namespace", + default_value=EnvironmentVariable("ROS_NAMESPACE", default_value=""), + description="Namespace which will appear in front of all topics (including /tf and /tf_static).", + ), + DeclareLaunchArgument( + "device_namespace", + default_value="", + description="Sensor namespace that will appear after all topics and TF frames, used for distinguishing multiple cameras on the same robot.", + ), + OpaqueFunction(function=launch_setup), + PushRosNamespace(LaunchConfiguration("robot_namespace")), + ] + ) diff --git a/healthcheck.cpp b/healthcheck.cpp deleted file mode 100755 index a114657..0000000 --- a/healthcheck.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "fstream" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" - -using namespace std::chrono_literals; - -#define LOOP_PERIOD 500ms -#define MSG_VALID_TIME 2s - -std::chrono::steady_clock::time_point last_msg_time; - -void write_health_status(const std::string &status) { - std::ofstream healthFile("/var/tmp/health_status.txt"); - healthFile << status; -} - -void msg_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { - last_msg_time = std::chrono::steady_clock::now(); -} - -void healthy_check() { - std::chrono::steady_clock::time_point current_time = - std::chrono::steady_clock::now(); - std::chrono::duration elapsed_time = current_time - last_msg_time; - bool is_msg_valid = elapsed_time < MSG_VALID_TIME; - - if (is_msg_valid) { - write_health_status("healthy"); - } else { - write_health_status("unhealthy"); - } -} - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("healthcheck_rplidar"); - auto sub = node->create_subscription( - "scan", rclcpp::SensorDataQoS().keep_last(1), msg_callback); - - while (rclcpp::ok()) { - rclcpp::spin_some(node); - healthy_check(); - std::this_thread::sleep_for(LOOP_PERIOD); - } - - return 0; -} diff --git a/husarion_utils/healthcheck.cpp b/husarion_utils/healthcheck.cpp new file mode 100644 index 0000000..119fa49 --- /dev/null +++ b/husarion_utils/healthcheck.cpp @@ -0,0 +1,58 @@ +#include "fstream" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + +using namespace std::chrono_literals; + +#define HEALTHCHECK_PERIOD 500ms +#define MSG_VALID_TIME 2s + +class HealthCheckNode : public rclcpp::Node { +public: + HealthCheckNode() + : Node("healthcheck_astra"), + last_msg_time(std::chrono::steady_clock::now()) { + + subscription_ = create_subscription( + "scan", rclcpp::SensorDataQoS().keep_last(1), + std::bind(&HealthCheckNode::msgCallback, this, std::placeholders::_1)); + + timer_ = create_wall_timer(HEALTHCHECK_PERIOD, + std::bind(&HealthCheckNode::healthyCheck, this)); + } + +private: + std::chrono::steady_clock::time_point last_msg_time; + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::TimerBase::SharedPtr timer_; + + void writeHealthStatus(const std::string &status) { + std::ofstream healthFile("/var/tmp/health_status.txt"); + healthFile << status; + } + + void msgCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { + RCLCPP_DEBUG(get_logger(), "Msg arrived"); + last_msg_time = std::chrono::steady_clock::now(); + } + + void healthyCheck() { + std::chrono::steady_clock::time_point current_time = + std::chrono::steady_clock::now(); + std::chrono::duration elapsed_time = current_time - last_msg_time; + bool is_msg_valid = elapsed_time < MSG_VALID_TIME; + + if (is_msg_valid) { + writeHealthStatus("healthy"); + } else { + writeHealthStatus("unhealthy"); + } + } +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/healthcheck.sh b/husarion_utils/healthcheck.sh similarity index 100% rename from healthcheck.sh rename to husarion_utils/healthcheck.sh diff --git a/husarion_utils/rplidar.launch.py b/husarion_utils/rplidar.launch.py new file mode 100755 index 0000000..e13b525 --- /dev/null +++ b/husarion_utils/rplidar.launch.py @@ -0,0 +1,93 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + GroupAction, + OpaqueFunction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node, PushRosNamespace, SetRemap +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + launch_file = LaunchConfiguration("launch_file").perform(context) + serial_baudrate = LaunchConfiguration("serial_baudrate").perform(context) + serial_port = LaunchConfiguration("serial_port").perform(context) + robot_namespace = LaunchConfiguration("robot_namespace").perform(context) + device_namespace = LaunchConfiguration("device_namespace").perform(context) + + if device_namespace: + frame_id = device_namespace + "_link" + else: + frame_id = "scan" + + rplidar_actions = [] + + # Remappings + if robot_namespace: + rplidar_actions.append(SetRemap("/tf", f"/{robot_namespace}/tf")) + rplidar_actions.append(SetRemap("/tf_static", f"/{robot_namespace}/tf_static")) + + # Device Namespace + rplidar_actions.append(PushRosNamespace(device_namespace)) + + # Rplidar + rplidar_path = PathJoinSubstitution([FindPackageShare("sllidar_ros2"), "launch", launch_file]) + rplidar_actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource([rplidar_path]), + launch_arguments={ + "frame_id": frame_id, + "serial_baudrate": serial_baudrate, + "serial_port": serial_port, + }.items(), + ) + ) + + rplidar_ns = GroupAction(actions=rplidar_actions) + + # Health check + healthcheck_node = Node( + package="healthcheck_pkg", + executable="healthcheck_node", + name="healthcheck_rplidar", + namespace=device_namespace, + output="screen", + ) + + return [PushRosNamespace(robot_namespace), rplidar_ns, healthcheck_node] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "launch_file", + default_value="sllidar_launch.py", + description="Select launch file from repo `sllidar_ros2`, based on plugged lidar model", + ), + DeclareLaunchArgument( + "serial_baudrate", + default_value="115200", + description="Specifying usb port baudrate to connected lidar", + ), + DeclareLaunchArgument( + "serial_port", + default_value="/dev/ttyUSB0", + description="Specifying usb port to connected lidar", + ), + DeclareLaunchArgument( + "robot_namespace", + default_value=EnvironmentVariable("ROS_NAMESPACE", default_value=""), + description="Namespace which will appear in front of all topics (including /tf and /tf_static).", + ), + DeclareLaunchArgument( + "device_namespace", + default_value="", + description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.", + ), + OpaqueFunction(function=launch_setup), + ] + )