diff --git a/Makefile b/Makefile index 29957b1..c56337c 100644 --- a/Makefile +++ b/Makefile @@ -37,6 +37,9 @@ realsense: hesai: @cd deploy && docker build --no-cache --tag go2py_hesai:latest -f docker/Dockerfile.hesai . +mid360: + @cd deploy && docker build --no-cache --tag go2py_mid360:latest -f docker/Dockerfile.mid360 . + bridge: @cd deploy && docker build --no-cache --tag go2py_bridge:latest -f docker/Dockerfile.bridge . @@ -54,6 +57,11 @@ hesai_install: @systemctl enable go2py-hesai.service @systemctl start go2py-hesai.service +mid360_install: + @cp deploy/services/go2py-mid360.service /etc/systemd/system/ + @systemctl enable go2py-mid360.service + @systemctl start go2py-mid360.service + bridge_install: @cp deploy/services/go2py-bridge.service /etc/systemd/system/ @systemctl enable go2py-bridge.service @@ -74,6 +82,11 @@ hesai_uninstall: @systemctl stop go2py-hesai.service @rm /etc/systemd/system/go2py-hesai.service +mid360_uninstall: + @systemctl disable go2py-mid360.service + @systemctl stop go2py-mid360.service + @rm /etc/systemd/system/go2py-mid360.service + bridge_uninstall: @systemctl disable go2py-bridge.service @systemctl stop go2py-bridge.service diff --git a/deploy/docker/Dockerfile.mid360 b/deploy/docker/Dockerfile.mid360 new file mode 100644 index 0000000..58bbed2 --- /dev/null +++ b/deploy/docker/Dockerfile.mid360 @@ -0,0 +1,34 @@ +# FROM isaac_ros_dev-aarch64 +FROM ros:humble +ENV DEBIAN_FRONTEND=noninteractive +SHELL ["/bin/bash", "-c"] +# uodate and install dependencies +RUN apt-get update && apt-get install -y \ + ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \ + # ros-humble-realsense2-camera \ + ros-humble-pointcloud-to-laserscan \ + libyaml-cpp-dev \ + libboost-all-dev\ + build-essential \ + cmake \ + git \ + ros-humble-pcl-ros \ + && rm -rf /var/lib/apt/lists/* + +# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree +RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \ +git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\ +cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build + +RUN cd /root && git clone https://github.com/Livox-SDK/Livox-SDK2.git && cd cd ./Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j4 && make install +RUN cd /root && git clone git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 && cd ws_livox/src/livox_ros_driver2 + +COPY docker/scripts/MID360_config.json /root/ws_livox/src/livox_ros_driver2/config/MID360_config.json +COPY launch_files/msg_MID360_launch.py /root/ws_livox/src/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py +RUN cd /root/ws_livox/src/livox_ros_driver2 && && source /opt/ros/humble/setup.sh && ./build.sh humble + +# Copy the script to start the nodes +COPY docker/scripts/mid360_start.sh /root/mid360_start.sh +# set the entrypoint to bash +# ENTRYPOINT ["/bin/bash"] +ENTRYPOINT ["/bin/bash", "/root/mid360_start.sh"] diff --git a/deploy/docker/scripts/MID360_config.json b/deploy/docker/scripts/MID360_config.json new file mode 100644 index 0000000..826ec94 --- /dev/null +++ b/deploy/docker/scripts/MID360_config.json @@ -0,0 +1,41 @@ +{ + "lidar_summary_info" : { + "lidar_type": 8 + }, + "MID360": { + "lidar_net_info" : { + "cmd_data_port": 56100, + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.123.18", + "cmd_data_port": 56101, + "push_msg_ip": "192.168.123.18", + "push_msg_port": 56201, + "point_data_ip": "192.168.123.18", + "point_data_port": 56301, + "imu_data_ip" : "192.168.123.18", + "imu_data_port": 56401, + "log_data_ip" : "", + "log_data_port": 56501 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.123.20", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} \ No newline at end of file diff --git a/deploy/docker/scripts/mid360_start.sh b/deploy/docker/scripts/mid360_start.sh new file mode 100644 index 0000000..eacb9b1 --- /dev/null +++ b/deploy/docker/scripts/mid360_start.sh @@ -0,0 +1,7 @@ +source /opt/ros/humble/setup.bash +source /unitree_ros2/cyclonedds_ws/install/setup.bash +# export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +# export CYCLONEDDS_URI=' +# +# ' +source /root/ws_livox/install/setup.bash && ros2 launch livox_ros_driver2 msg_MID360_launch.py \ No newline at end of file diff --git a/deploy/launch_files/msg_MID360_launch.py b/deploy/launch_files/msg_MID360_launch.py new file mode 100644 index 0000000..e4a6db4 --- /dev/null +++ b/deploy/launch_files/msg_MID360_launch.py @@ -0,0 +1,54 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +user_config_path = os.path.join(cur_config_path, 'MID360_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + return LaunchDescription([ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) \ No newline at end of file diff --git a/deploy/services/go2py-mid360.service b/deploy/services/go2py-mid360.service new file mode 100644 index 0000000..5364048 --- /dev/null +++ b/deploy/services/go2py-mid360.service @@ -0,0 +1,13 @@ +[Unit] +Description=ROS2 device driver container +Requires=multi-user.target +After=multi-user.target + +[Service] +Restart=always +ExecStartPre=/usr/bin/docker rm -f go2py_mid360 || true +ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_mid360 --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_mid360:latest' +ExecStop=/usr/bin/docker stop -t 2 go2py_mid360 + +[Install] +WantedBy=default.target \ No newline at end of file