diff --git a/.devcontainer/Dockerfile.go2 b/.devcontainer/Dockerfile.go2
new file mode 100644
index 0000000..6afa223
--- /dev/null
+++ b/.devcontainer/Dockerfile.go2
@@ -0,0 +1,37 @@
+FROM nvcr.io/nvidia/isaac/ros:aarch64-ros2_humble_42f50fd45227c63eb74af1d69ddc2970
+ENV DEBIAN_FRONTEND noninteractive
+ARG CONDA_VER=latest
+ARG OS_TYPE=aarch64
+
+RUN apt-get update && apt-get install -y -qq --no-install-recommends \ cuda \
+ wget \
+ ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
+ && rm -rf /var/lib/apt/lists/*
+
+# MID360 Lidar Drivers
+RUN cd /root && git clone https://github.com/Livox-SDK/Livox-SDK2.git && cd ./Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j4 && make install
+RUN cd /root && git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 && cd ws_livox/src/livox_ros_driver2 && source /opt/ros/humble/setup.bash && cd /root/ws_livox/src/livox_ros_driver2 && /bin/bash build.sh humble
+# FAST-LIO Install
+RUN cd /root && mkdir -p fastlio-ws/src && cd fastlio-ws/src && git clone https://github.com/Ericsii/FAST_LIO.git --recursive && cd FAST_LIO && git checkout ros2 && cd /root/fastlio-ws && source /root/ws_livox/install/setup.bash && colcon build --symlink-install
+
+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 "export CYCLONEDDS_HOME=/unitree_ros2/cyclonedds_ws/install/cyclonedds/" >> ~/.bashrc
+RUN cd /root && git clone https://github.com/eclipse-cyclonedds/cyclonedds-python -b releases/0.10.x && cd cyclonedds-python && pip install .
+
+# CycloneDDS-cxx
+RUN cd /root && git clone https://github.com/eclipse-cyclonedds/cyclonedds-cxx -b releases/0.10.x &&\
+ cd cyclonedds-cxx && mkdir build install && cd build && cmake .. && \
+ cmake --build . --target install
+
+# Install Python dependencies
+RUN pip install matplotlib opencv-python scipy isort black onnx onnxruntime
+RUN pip install https://github.com/NVIDIA/warp/releases/download/v1.5.1/warp_lang-1.5.1+cu11-py3-none-manylinux2014_aarch64.whl
+RUN pip install cyclonedds pygame pynput jupyter ipykernel meshcat
+
+# RUN echo 'source /unitree_ros2/cyclonedds_ws/install/setup.bash' >> ~/.bashrc
+# RUN echo 'export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp' >> ~/.bashrc
+# RUN echo 'export CYCLONEDDS_URI=file:///home/Go2py/Go2Py/assets/cyclonedds.xml' >> ~/.bashrc
diff --git a/Makefile b/Makefile
index c56337c..82b1efc 100644
--- a/Makefile
+++ b/Makefile
@@ -38,7 +38,8 @@ 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 .
+ @cd deploy && docker build --tag go2py_mid360:latest -f docker/Dockerfile.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 .
diff --git a/deploy/docker/Dockerfile.mid360 b/deploy/docker/Dockerfile.mid360
index 58bbed2..a9edb0a 100644
--- a/deploy/docker/Dockerfile.mid360
+++ b/deploy/docker/Dockerfile.mid360
@@ -1,31 +1,32 @@
# FROM isaac_ros_dev-aarch64
-FROM ros:humble
+FROM robocaster/mid360:go2
+# 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/*
+# 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
+# # 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
+# RUN cd /root && git clone https://github.com/Livox-SDK/Livox-SDK2.git && cd ./Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j4 && make install
+# RUN cd /root && 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
+RUN cd /root/ws_livox/src/livox_ros_driver2 && source /opt/ros/humble/setup.sh && /bin/bash build.sh humble
# Copy the script to start the nodes
COPY docker/scripts/mid360_start.sh /root/mid360_start.sh
diff --git a/deploy/launch_files/fast_lio.launch.py b/deploy/launch_files/fast_lio.launch.py
new file mode 100644
index 0000000..3b0ea4d
--- /dev/null
+++ b/deploy/launch_files/fast_lio.launch.py
@@ -0,0 +1,48 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ package_path = get_package_share_directory('fast_lio')
+ default_config_path = os.path.join(package_path, 'config')
+
+ use_sim_time = LaunchConfiguration('use_sim_time')
+ config_path = LaunchConfiguration('config_path')
+ config_file = LaunchConfiguration('config_file')
+
+ declare_use_sim_time_cmd = DeclareLaunchArgument(
+ 'use_sim_time', default_value='false',
+ description='Use simulation (Gazebo) clock if true'
+ )
+ declare_config_path_cmd = DeclareLaunchArgument(
+ 'config_path', default_value=default_config_path,
+ description='Yaml config file path'
+ )
+ decalre_config_file_cmd = DeclareLaunchArgument(
+ 'config_file', default_value='mid360.yaml',
+ description='Config file'
+ )
+
+ fast_lio_node = Node(
+ package='fast_lio',
+ executable='fastlio_mapping',
+ parameters=[PathJoinSubstitution([config_path, config_file]),
+ {'use_sim_time': use_sim_time}],
+ output='screen'
+ )
+
+ ld = LaunchDescription()
+ ld.add_action(declare_use_sim_time_cmd)
+ ld.add_action(declare_config_path_cmd)
+ ld.add_action(decalre_config_file_cmd)
+ ld.add_action(fast_lio_node)
+
+ return ld
\ No newline at end of file
diff --git a/deploy/launch_files/msg_MID360_launch.py b/deploy/launch_files/msg_MID360_launch.py
index e4a6db4..741573d 100644
--- a/deploy/launch_files/msg_MID360_launch.py
+++ b/deploy/launch_files/msg_MID360_launch.py
@@ -5,7 +5,7 @@ from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################
-xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+xfer_format = 1 # 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.
diff --git a/deploy/ros2_nodes/go2_description/xacro/robot.xacro b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
index fe8ca35..59e9e71 100755
--- a/deploy/ros2_nodes/go2_description/xacro/robot.xacro
+++ b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
@@ -184,6 +184,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+