go2py bridge docker added

Dockerfile.robot

@@ -0,0 +1,41 @@
+# 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 \
+    libyaml-cpp-dev \
+    # ros-humble-isaac-ros-visual-slam \
+    # ros-humble-isaac-ros-occupancy-grid-localizer\
+    libboost-all-dev\
+    build-essential \
+    cmake \
+    git \
+    && 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 echo "source /opt/ros/humble/setup.bash" >>  /usr/local/bin/scripts/workspace-entrypoint.sh
+# RUN echo "source /unitree_ros2/cyclonedds_ws/install/setup.bash" >>  /usr/local/bin/scripts/workspace-entrypoint.sh
+# RUN echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >>  /usr/local/bin/scripts/workspace-entrypoint.sh
+# RUN echo "export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces> <NetworkInterface name="eth0" priority="default" multicast="default" />  </Interfaces></General></Domain></CycloneDDS>'" >>  /usr/local/bin/scripts/workspace-entrypoint.sh
+
+# copy the go2py ros2 nodes
+COPY deploy/robot_ws/src /robot_ws/src
+RUN cd /robot_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install
+
+# Compile the C++ hypervisor bridge
+COPY deploy/dds_bridge /dds_bridge
+WORKDIR /dds_bridge
+RUN ./install.sh && mkdir build && cd build && cmake .. && make
+
+# Copy the script to start the nodes
+COPY deploy/scripts /root/scripts
+COPY deploy/launch /root/launch
+# set the entrypoint to bash
+# ENTRYPOINT ["/bin/bash"]
+ENTRYPOINT ["/bin/bash", "/root/scripts/robot_hw_start.sh"]

Makefile

@@ -1,12 +1,15 @@
 docker_dock:
-	@docker build --no-cache --tag go2py:latest -f Dockerfile.dock .
+	@docker build --no-cache --tag go2py_dock_hw:latest -f Dockerfile.dock .
+
+docker_robot:
+	@docker build --no-cache --tag go2py_robot_hw:latest -f Dockerfile.robot .
 
 docker_dock_install:
-	@cp deploy/scripts/go2py-hw-nodes.service /etc/systemd/system/
+	@cp deploy/scripts/go2py-dock-hw-nodes.service /etc/systemd/system/
-	@systemctl enable go2py-hw-nodes.service
+	@systemctl enable go2py-dock-hw-nodes.service
-	@systemctl start go2py-hw-nodes.service
+	@systemctl start go2py-dock-hw-nodes.service
 
 docker_dock_uninstall:
-	@systemctl disable go2py-hw-nodes.service
+	@systemctl disable go2py-dock-hw-nodes.service
-	@systemctl stop go2py-hw-nodes.service
+	@systemctl stop go2py-dock-hw-nodes.service
-	@rm /etc/systemd/system/go2py-hw-nodes.service
+	@rm /etc/systemd/system/go2py-dock-hw-nodes.service

deploy/launch/robot.launch.py

@@ -4,81 +4,12 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import ThisLaunchFileDir
 from launch_ros.actions import Node
 
-
 def generate_launch_description():
     return LaunchDescription([
         # launch the pointcloud to laser scan converter
         Node(
-            package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
+            package='go2py_node',
-            remappings=[('cloud_in', '/go2/lidar_points'),
+            executable='bridge',
-                        ('scan', '/go2/lidar_scans')],
+            name='go2py_bridge'
-            parameters=[{
-                'target_frame': 'go2/hesai_lidar',
-                'transform_tolerance': 0.01,
-                'min_height': 0.0,
-                'max_height': 1.0,
-                'angle_min': -1.5708,  # -M_PI/2
-                'angle_max': 1.5708,  # M_PI/2
-                'angle_increment': 0.0087,  # M_PI/360.0
-                'scan_time': 0.3333,
-                'range_min': 0.45,
-                'range_max': 100.0,
-                'use_inf': True,
-                'inf_epsilon': 1.0
-            }],
-            name='pointcloud_to_laserscan'
         ),
-
-        Node(
-            package='hesai_ros_driver',
-            executable='hesai_ros_driver_node',
-            name='b1_hesai_ros_driver_node'
-        ),
-
-        # Node(
-        #     name='go2_d455_cam',
-        #     namespace='go2/d435i_cam',
-        #     package='realsense2_camera',
-        #     executable='realsense2_camera_node',
-        #     parameters=[{
-        #             'enable_infra1': True,
-        #             'enable_infra2': True,
-        #             'enable_color': False,
-        #             'enable_depth': False,
-        #             'depth_module.emitter_enabled': 0,
-        #             'depth_module.profile': '640x480x60',
-        #             'enable_gyro': True,
-        #             'enable_accel': True,
-        #             'gyro_fps': 400,
-        #             'accel_fps': 200,
-        #             'unite_imu_method': 2,
-        #             # 'tf_publish_rate': 0.0
-        #     }]
-        # ),
-
-        # Launch the front looking D455 camera
-        # IncludeLaunchDescription(
-        #     PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/d455.launch.py'])
-        # ),
-        # Run the B1py node
-        # Node(
-        #     package='b1py_node',
-        #     executable='highlevel',
-        #     name='b1_highlevel_node'
-        # ),
-
-        # Run the B1py calibration TF broadcaster
-        # Node(
-        #     package='b1py_calib',
-        #     executable='calib_broadcaster',
-        #     name='b1_calib_broadcaster_node'
-        # ),
-        # Launch the LiDAR sensor
-        # IncludeLaunchDescription(
-        #     PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rslidar.launch.py'])
-        # ),
-        # Launch the LiDAR sensor
-        # IncludeLaunchDescription(
-        #     PythonLaunchDescriptionSource(['/B1Py/deploy/docker/launch/state_estimation/ekf.launch.py'])
-        # ),
     ])

deploy/scripts/go2py-dock-hw-nodes.service

@@ -6,7 +6,7 @@ After=docker.service
 [Service]
 Restart=always
 ExecStartPre=/usr/bin/docker rm -f go2py_docker || true
-ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_docker --privileged --network host -v /home/unitree/locomotion:/home/locomotion -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py:latest' 
+ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_docker --privileged --network host -v /home/unitree/locomotion:/home/locomotion -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_dock_hw:latest' 
 ExecStop=/usr/bin/docker stop -t 2 go2py_docker
 
 [Install]

deploy/scripts/go2py-robot-hw-nodes.service

@@ -0,0 +1,13 @@
+[Unit]
+Description=ROS2 device driver container
+Requires=docker.service
+After=docker.service
+
+[Service]
+Restart=always
+ExecStartPre=/usr/bin/docker rm -f go2py_robot_docker || true
+ExecStart=/bin/bash -c '/usr/bin/docker run --rm --name go2py_robot_docker --privileged --network host -v /home/unitree/locomotion:/home/locomotion -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia go2py_robot_hw:latest' 
+ExecStop=/usr/bin/docker stop -t 2 go2py_robot_docker
+
+[Install]
+WantedBy=default.target

deploy/scripts/robot_hw_start.sh

@@ -0,0 +1,4 @@
+source /opt/ros/humble/setup.bash
+source /unitree_ros2/cyclonedds_ws/install/setup.bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+source /robot_ws/install/setup.bash && ros2 launch /root/launch/robot.launch.py