diff --git a/Dockerfile.dock b/Dockerfile.dock index daf12f7..b001f88 100644 --- a/Dockerfile.dock +++ b/Dockerfile.dock @@ -1,12 +1,16 @@ -FROM isaac_ros_dev-aarch64 +# 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 \ - ros-humble-isaac-ros-visual-slam \ - ros-humble-isaac-ros-occupancy-grid-localizer\ + libyaml-cpp-dev \ + # ros-humble-isaac-ros-visual-slam \ + # ros-humble-isaac-ros-occupancy-grid-localizer\ + libboost-all-dev\ build-essential \ cmake \ git \ @@ -17,9 +21,9 @@ RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /uni 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 "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=' '" >> /usr/local/bin/scripts/workspace-entrypoint.sh # copy the go2py ros2 nodes @@ -36,4 +40,4 @@ COPY deploy/scripts /root/scripts COPY deploy/launch /root/launch # set the entrypoint to bash # ENTRYPOINT ["/bin/bash"] -ENTRYPOINT ["/bin/bash", "/root/scripts/hw_start.sh"] +ENTRYPOINT ["/bin/bash", "/root/scripts/dock_hw_start.sh"] diff --git a/Makefile b/Makefile index bb1a93c..df3b941 100644 --- a/Makefile +++ b/Makefile @@ -1,12 +1,12 @@ -docker: +docker_dock: @docker build --no-cache --tag go2py:latest -f Dockerfile.dock . -docker_install: +docker_dock_install: @cp deploy/scripts/go2py-hw-nodes.service /etc/systemd/system/ @systemctl enable go2py-hw-nodes.service @systemctl start go2py-hw-nodes.service -docker_uninstall: +docker_dock_uninstall: @systemctl disable go2py-hw-nodes.service @systemctl stop go2py-hw-nodes.service @rm /etc/systemd/system/go2py-hw-nodes.service \ No newline at end of file diff --git a/deploy/dock_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt b/deploy/dock_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt index 774542e..ba387cf 100644 --- a/deploy/dock_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt +++ b/deploy/dock_ws/src/lidar_node/src/driver/HesaiLidar_SDK_2.0/CMakeLists.txt @@ -86,7 +86,8 @@ target_link_libraries(sample # ) find_package(CUDA ) -if(${CUDA_FOUND}) +if(OFF) +# if(${CUDA_FOUND}) set(CUDA_SOURCE_PROPERTY_FORMAT OBJ) set(CUDA_SEPARABLE_COMPILATION ON) include_directories(${CUDA_INCLUDE_DIRS}) diff --git a/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp b/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp index 127f7e7..d9048bb 100644 --- a/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp +++ b/deploy/dock_ws/src/lidar_node/src/manager/source_driver_ros2.hpp @@ -229,19 +229,22 @@ inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFr for (size_t i = 0; i < frame.points_num; i++) { - LidarPointXYZIRT point = frame.points[i]; - *iter_x_ = point.x; - *iter_y_ = point.y; - *iter_z_ = point.z; - *iter_intensity_ = point.intensity; - *iter_ring_ = point.ring; - *iter_timestamp_ = point.timestamp; - ++iter_x_; - ++iter_y_; - ++iter_z_; - ++iter_intensity_; - ++iter_ring_; - ++iter_timestamp_; + // if(i%2==0) //downsample the number of points by two + // { + LidarPointXYZIRT point = frame.points[i]; + *iter_x_ = point.x; + *iter_y_ = point.y; + *iter_z_ = point.z; + *iter_intensity_ = point.intensity; + *iter_ring_ = point.ring; + *iter_timestamp_ = point.timestamp; + ++iter_x_; + ++iter_y_; + ++iter_z_; + ++iter_intensity_; + ++iter_ring_; + ++iter_timestamp_; + // } } printf("frame:%d points:%u packet:%d start time:%lf end time:%lf\n",frame.frame_index, frame.points_num, frame.packet_num, frame.points[0].timestamp, frame.points[frame.points_num - 1].timestamp) ; diff --git a/deploy/launch/dock.launch.py b/deploy/launch/dock.launch.py new file mode 100644 index 0000000..085205c --- /dev/null +++ b/deploy/launch/dock.launch.py @@ -0,0 +1,37 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +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', + remappings=[('cloud_in', '/go2/lidar_points'), + ('scan', '/go2/lidar_scans')], + 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='go2_hesai_ros_driver_node' + ), + ]) \ No newline at end of file diff --git a/deploy/launch/hw.launch.py b/deploy/launch/robot.launch.py similarity index 73% rename from deploy/launch/hw.launch.py rename to deploy/launch/robot.launch.py index 5267246..9261da7 100644 --- a/deploy/launch/hw.launch.py +++ b/deploy/launch/robot.launch.py @@ -35,26 +35,26 @@ def generate_launch_description(): 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 - }] - ), + # 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( diff --git a/deploy/scripts/dock_hw_start.sh b/deploy/scripts/dock_hw_start.sh new file mode 100644 index 0000000..99a38ae --- /dev/null +++ b/deploy/scripts/dock_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 /dock_ws/install/setup.bash && ros2 launch /root/launch/dock.launch.py \ No newline at end of file diff --git a/deploy/scripts/hw_start.sh b/deploy/scripts/hw_start.sh deleted file mode 100644 index c5804bb..0000000 --- a/deploy/scripts/hw_start.sh +++ /dev/null @@ -1 +0,0 @@ -source /dock_ws/install/setup.bash && ros2 launch /root/launch/hw.launch.py \ No newline at end of file