diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000000000000000000000000000000000000..c29ccf8fbdd3488f30faeaa416ff524a9794a20a --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,27 @@ +{ + "name": "pb_rm_simulation", + "image": "lihanchen2004/pb_rm_simulation:1.0.0", + "runArgs": [ + "--init", + "--gpus=all", + "--privileged", + "--env=NVIDIA_DRIVER_CAPABILITIES=all", + "--env=DISPLAY=${localEnv:DISPLAY}", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix", + "--network=host" + ], + "customizations": { + "vscode": { + "extensions": [ + "llvm-vs-code-extensions.vscode-clangd", + "vadimcn.vscode-lldb", + "xaver.clang-format", + "ms-python.python", + "ms-iot.vscode-ros", + "ms-vscode.cmake-tools", + "usernamehw.errorlens" + ] + } + } +} diff --git a/README.md b/README.md index b60f7aa4cbf2f665c50cc953d53d35ffcd9cf484..1ab25578f19a88bc4057eed9dc10f1014cd95281 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ | /livox/lidar | livox_ros_driver2/msg/CustomMsg | Mid360 自定义消息类型 | | /livox/lidar/pointcloud | sensor_msgs/msg/PointCloud2 | ROS2 点云消息类型 | | /livox/imu | sensor_msgs/msg/Imu | Gazebo 插件仿真 IMU | -| /cmd_vel | geometry_msgs/msg/Twist | 麦克纳姆轮小车运动控制接口 | +| /cmd_vel | geometry_msgs/msg/Twist | 全向小车运动控制接口 | ### 1.2 整体框图 @@ -37,6 +37,31 @@ 当前开发环境为 Ubuntu22.04, ROS2 humble, Gazebo Classic 11.10.0 +### 方式一：使用 Docker + +本项目已配置基础 Dockerfile，并可使用 [Dev Container](https://containers.dev/) 进行仿真测试和开发。 + +> DevContainer 的一个特征是将代码的运行环境与代码本身完全隔离开来，在效果上类似将 workspace 挂载到了容器中。 +> 在 Docker-image 中仅仅包含有关系统的配置（例如修改 .baserc 或安装依赖包等），其本身不存储任何项目代码和工作空间，做到了代码与环境的完全隔离。 +> 可以通过 devcontainer.json 配置文件，快速修改和分发容器配置。 +> 与 VSCode 深度融合，一键启动，无需任何命令。 + +镜像仓库地址：[DockerHub: lihanchen2004/pb_rm_simulation](https://hub.docker.com/repository/docker/lihanchen2004/pb_rm_simulation) + +1. 安装 Docker + +2. 拉取镜像 + + ```sh + docker pull lihanchen2004/pb_rm_simulation:1.0.0 + ``` + +3. 在宿主机 VSCode 中 安装 `ms-vscode-remote.remote-containers` 插件 + +4. 快捷键 `Ctrl+Shift+P`， 输入并点击 `Dev Containers:Rebuild and Reopen in Container` + +### 方式二：源码安装 + 1. 克隆仓库 ```sh @@ -61,7 +86,7 @@ 3. 安装依赖 ```sh - cd pb_rmsimulation + cd pb_rm_simulation rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y ``` diff --git a/dockerfile b/dockerfile new file mode 100644 index 0000000000000000000000000000000000000000..5d6af58a50c2d85693e0c687f2062cf5190c8145 --- /dev/null +++ b/dockerfile @@ -0,0 +1,54 @@ +FROM ros:humble-ros-base + +# create workspace +RUN mkdir -p /ros_ws +WORKDIR /ros_ws/ + +# 小鱼一键换源 +RUN apt update \ + && apt install wget python3-yaml -y \ + && echo "chooses:\n" > fish_install.yaml \ + && echo "- {choose: 5, desc: '一键配置:系统源(更换系统源,支持全版本Ubuntu系统)'}\n" >> fish_install.yaml \ + && echo "- {choose: 2, desc: 更换系统源并清理第三方源}\n" >> fish_install.yaml \ + && echo "- {choose: 1, desc: 添加ROS/ROS2源}\n" >> fish_install.yaml \ + && wget http://fishros.com/install -O fishros && /bin/bash fishros \ + && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/* + +# 初始化 rosdepc +RUN apt-get update && apt-get install python3-pip -y && \ + pip install rosdepc && \ + sudo rosdepc init && \ + rosdepc update + +# 安装 Clang +RUN apt-get -y install clang && \ + apt-get -y install clangd && \ + apt-get -y install clang-format && \ + apt-get clean && apt autoclean + +RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git && \ + cd ./Livox-SDK2/ && \ + mkdir build && \ + cd build && \ + cmake .. && make -j && \ + sudo make install + +# setup zsh +RUN sh -c "$(wget -O- https://github.com/deluan/zsh-in-docker/releases/download/v1.2.0/zsh-in-docker.sh)" -- \ + -t jispwoso -p git \ + -p https://github.com/zsh-users/zsh-autosuggestions \ + -p https://github.com/zsh-users/zsh-syntax-highlighting && \ + chsh -s /bin/zsh && \ + rm -rf /var/lib/apt/lists/* + +# setup .zshrc +RUN echo 'export TERM=xterm-256color\n\ +source /opt/ros/humble/setup.zsh\n\ +eval "$(register-python-argcomplete3 ros2)"\n\ +eval "$(register-python-argcomplete3 colcon)"\n'\ +>> /root/.zshrc + +# source entrypoint setup +RUN sed --in-place --expression \ + '$isource "/opt/ros/humble/setup.sh"' \ + /ros_entrypoint.sh diff --git a/src/rm_localization/icp_registration/package.xml b/src/rm_localization/icp_registration/package.xml index a9583d320585ea10592dccf34a65ead1f2347f67..e9dadca5103e0c2bd84cdc9654a3562da498a5f1 100644 --- a/src/rm_localization/icp_registration/package.xml +++ b/src/rm_localization/icp_registration/package.xml @@ -12,7 +12,6 @@ rclcpp rclcpp_components sensor_msgs - pcl pcl_conversions tf2 tf2_ros diff --git a/src/rm_nav_bringup/config/reality/nav2_params_real.yaml b/src/rm_nav_bringup/config/reality/nav2_params_real.yaml index fe9e48b54f4eaa06c36d3a5d58e535aaeeadc29a..d89ce7fa84eabeefc4ed0b1c0a4ff4acf1aafc23 100644 --- a/src/rm_nav_bringup/config/reality/nav2_params_real.yaml +++ b/src/rm_nav_bringup/config/reality/nav2_params_real.yaml @@ -313,37 +313,51 @@ global_costmap: robot_radius: 0.2 resolution: 0.04 track_unknown_space: true - plugins: ["static_layer", "voxel3d_layer", "inflation_layer"] - voxel3d_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - mark_threshold: 1 - observation_sources: livox - min_obstacle_height: 0.00 - max_obstacle_height: 2.0 - livox: + plugins: ["static_layer", "stvl_layer", "inflation_layer"] + stvl_layer: + plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" + # https://github.com/SteveMacenski/spatio_temporal_voxel_layer + enabled: true + voxel_decay: 15.0 # 如果是线性衰减，单位为秒；如果是指数衰减，则为 e 的 n 次方 + decay_model: 0 # 衰减模型，0=线性，1=指数，-1=持久 + voxel_size: 0.05 # 每个体素的尺寸，单位为米 + track_unknown_space: true # default space is unknown + mark_threshold: 0 # voxel height + update_footprint_enabled: true + combination_method: 1 # 1=max, 0=override + origin_z: 0.0 # 单位为米 + publish_voxel_map: false # default false, 是否发布体素地图 + transform_tolerance: 0.2 # 单位为秒 + mapping_mode: false # default off, saves map not for navigation + map_save_duration: 60.0 # default 60s, how often to autosave + observation_sources: livox_mark livox_clear + livox_mark: + data_type: PointCloud2 topic: /segmentation/obstacle - obstacle_max_range: 10.0 - obstacle_min_range: 0.0 - raytrace_min_range: 0.0 - raytrace_max_range: 20.0 - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 8.0 - inflation_radius: 0.7 - always_send_full_costmap: True + marking: true + clearing: false + obstacle_range: 3.0 # 障碍物检测范围，单位为米 + min_obstacle_height: 0.2 # 最小障碍物高度，单位为米 + max_obstacle_height: 2.0 # 最大障碍物高度，单位为米 + expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer + observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest + inf_is_valid: false # default false, for laser scans + filter: "voxel" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on + voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter + clear_after_reading: true # default false, clear the buffer after the layer gets readings from it + livox_clear: + enabled: true # default true, can be toggled on/off with associated service call + data_type: PointCloud2 + topic: /segmentation/obstacle + marking: false + clearing: true + max_z: 8.0 # 最大 Z 坐标，单位为米 + min_z: 1.0 # 最小 Z 坐标，单位为米 + vertical_fov_angle: 1.029 # 垂直视场角，单位为弧度，For 3D lidars it's the symmetric FOV about the planar axis. + vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters + horizontal_fov_angle: 6.29 # 3D 激光雷达水平视场角 + decay_acceleration: 5.0 # default 0, 1/s^2. + model_type: 1 # 0=深度相机，1=3D激光雷达 map_saver: ros__parameters: diff --git a/src/rm_nav_bringup/config/simulation/nav2_params_sim.yaml b/src/rm_nav_bringup/config/simulation/nav2_params_sim.yaml index 694f812f3d820a0a60daf2d58a90afa358c698e5..341c5b4fb2bb1e7a7dfbc407f6218e3c921e826a 100644 --- a/src/rm_nav_bringup/config/simulation/nav2_params_sim.yaml +++ b/src/rm_nav_bringup/config/simulation/nav2_params_sim.yaml @@ -313,29 +313,52 @@ global_costmap: robot_radius: 0.2 resolution: 0.04 track_unknown_space: true - plugins: ["static_layer", "voxel3d_layer", "inflation_layer"] - voxel3d_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - mark_threshold: 1 - observation_sources: livox - min_obstacle_height: 0.00 - max_obstacle_height: 2.0 - livox: + plugins: ["static_layer", "stvl_layer", "inflation_layer"] + stvl_layer: + plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" + # https://github.com/SteveMacenski/spatio_temporal_voxel_layer + enabled: true + voxel_decay: 15.0 # 如果是线性衰减，单位为秒；如果是指数衰减，则为 e 的 n 次方 + decay_model: 0 # 衰减模型，0=线性，1=指数，-1=持久 + voxel_size: 0.05 # 每个体素的尺寸，单位为米 + track_unknown_space: true # default space is unknown + mark_threshold: 0 # voxel height + update_footprint_enabled: true + combination_method: 1 # 1=max, 0=override + origin_z: 0.0 # 单位为米 + publish_voxel_map: true # default false, 是否发布体素地图 + transform_tolerance: 0.2 # 单位为秒 + mapping_mode: false # default off, saves map not for navigation + map_save_duration: 60.0 # default 60s, how often to autosave + observation_sources: livox_mark livox_clear + livox_mark: + data_type: PointCloud2 topic: /segmentation/obstacle - obstacle_max_range: 10.0 - obstacle_min_range: 0.0 - raytrace_min_range: 0.0 - raytrace_max_range: 20.0 - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" + marking: true + clearing: false + obstacle_range: 3.0 # 障碍物检测范围，单位为米 + min_obstacle_height: 0.2 # 最小障碍物高度，单位为米 + max_obstacle_height: 2.0 # 最大障碍物高度，单位为米 + expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer + observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest + inf_is_valid: false # default false, for laser scans + filter: "voxel" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on + voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter + clear_after_reading: true # default false, clear the buffer after the layer gets readings from it + livox_clear: + enabled: true # default true, can be toggled on/off with associated service call + data_type: PointCloud2 + topic: /segmentation/obstacle + marking: false + clearing: true + max_z: 8.0 # 最大 Z 坐标，单位为米 + min_z: 1.0 # 最小 Z 坐标，单位为米 + vertical_fov_angle: 1.029 # 垂直视场角，单位为弧度，For 3D lidars it's the symmetric FOV about the planar axis. + vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters + horizontal_fov_angle: 6.29 # 3D 激光雷达水平视场角 + decay_acceleration: 5.0 # default 0, 1/s^2. + model_type: 1 # 0=深度相机，1=3D激光雷达 + static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True diff --git a/src/rm_nav_bringup/package.xml b/src/rm_nav_bringup/package.xml index eaf9e5d98dc952ca899197c51edd4af5b7abf53a..d5c21314d7ab962bf4193bba14460fd324374c1c 100644 --- a/src/rm_nav_bringup/package.xml +++ b/src/rm_nav_bringup/package.xml @@ -10,6 +10,8 @@ ament_cmake + rviz2 + ament_cmake diff --git a/src/rm_navigation/fake_vel_transform/README.md b/src/rm_navigation/fake_vel_transform/README.md index 5eef02da47945ee7bf6dd3226cf3094b08e1c288..a43c7183529573775749b6dedfbeebf5e0eca9d9 100644 --- a/src/rm_navigation/fake_vel_transform/README.md +++ b/src/rm_navigation/fake_vel_transform/README.md @@ -15,7 +15,7 @@ nav2 发布的速度也是基于 `base_link_fake` 坐标系的，通过 tf2 将 订阅： - nav2 发布的基于 base_link_fake 坐标系的速度指令 `/cmd_vel` -- teb_local_planner 发布的局部路径朝向 `/teb_poses` +- nav2 controller 发布的局部路径朝向 `/local_path` - `odom` 到 `base_link` 的 tf 变换 发布： diff --git a/src/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp b/src/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp index b2add1ae17ca9b26eb5ad14f6a435d87856761c6..3ca38928a737e3e5e207c57342af29a7bd764db6 100644 --- a/src/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp +++ b/src/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -26,7 +27,7 @@ public: private: void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); - void localPoseCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg); + void localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg); void publishTransform(); @@ -36,7 +37,7 @@ private: std::shared_ptr tf2_listener_; rclcpp::Subscription::SharedPtr cmd_vel_sub_; - rclcpp::Subscription::SharedPtr local_pose_sub_; + rclcpp::Subscription::SharedPtr local_pose_sub_; rclcpp::Publisher::SharedPtr cmd_vel_chassis_pub_; diff --git a/src/rm_navigation/fake_vel_transform/package.xml b/src/rm_navigation/fake_vel_transform/package.xml index b90975a332b273f9b7d77bbca3b9b2c7ac4bc16c..9be9e08989fa05649caa844da09c3d1926e87b49 100644 --- a/src/rm_navigation/fake_vel_transform/package.xml +++ b/src/rm_navigation/fake_vel_transform/package.xml @@ -20,7 +20,6 @@ nav_msgs tf2_ros tf2_geometry_msgs - auto_aim_interfaces ament_lint_auto ament_lint_common diff --git a/src/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp b/src/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp index 0f923e7a4dc4a4cd01c076f11933e4225b4a77ed..53b0a86ca01394eaa9f22e89ab412d7dbdc8d78a 100644 --- a/src/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp +++ b/src/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp @@ -10,7 +10,7 @@ namespace fake_vel_transform { const std::string CMD_VEL_TOPIC = "/cmd_vel"; const std::string AFTER_TF_CMD_VEL = "/cmd_vel_chassis"; -const std::string TRAJECTORY_TOPIC = "/teb_poses"; +const std::string TRAJECTORY_TOPIC = "/local_plan"; const int TF_PUBLISH_FREQUENCY = 20; // base_link to base_link_fake. Frequency in Hz. FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options) @@ -32,7 +32,7 @@ FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options) CMD_VEL_TOPIC, 1, std::bind(&FakeVelTransform::cmdVelCallback, this, std::placeholders::_1)); cmd_vel_chassis_pub_ = this->create_publisher( AFTER_TF_CMD_VEL, rclcpp::QoS(rclcpp::KeepLast(1))); - local_pose_sub_ = this->create_subscription( + local_pose_sub_ = this->create_subscription( TRAJECTORY_TOPIC, 1, std::bind(&FakeVelTransform::localPoseCallback, this, std::placeholders::_1)); @@ -43,7 +43,7 @@ FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options) } // Get the local pose from planner -void FakeVelTransform::localPoseCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg) +void FakeVelTransform::localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg) { if (!msg || msg->poses.empty()) { RCLCPP_WARN(get_logger(), "Received empty or invalid PoseArray message"); @@ -52,7 +52,7 @@ void FakeVelTransform::localPoseCallback(const geometry_msgs::msg::PoseArray::Sh // Choose the pose based on the size of the poses array size_t index = std::min(msg->poses.size() / 4, msg->poses.size() - 1); - const geometry_msgs::msg::Pose& selected_pose = msg->poses[index]; + const geometry_msgs::msg::Pose & selected_pose = msg->poses[index].pose; // Update current angle based on the difference between teb_angle and base_link_angle_ double teb_angle = tf2::getYaw(selected_pose.orientation); diff --git a/src/rm_navigation/rm_navigation/package.xml b/src/rm_navigation/rm_navigation/package.xml index 7081c7d738d2bc75ccca308e8e420a0c2275e7c7..80301128b998a7a699c35aa9652e5417852c5457 100644 --- a/src/rm_navigation/rm_navigation/package.xml +++ b/src/rm_navigation/rm_navigation/package.xml @@ -9,6 +9,8 @@ ament_cmake + spatio_temporal_voxel_layer + ament_lint_auto ament_lint_common diff --git a/src/rm_navigation/rm_navigation/rviz/nav2.rviz b/src/rm_navigation/rm_navigation/rviz/nav2.rviz index 13c8492ef994eb8639178c96e4c907ae9b95a518..1925af2622f08c184add4a43c2cb6c51928d71de 100644 --- a/src/rm_navigation/rm_navigation/rviz/nav2.rviz +++ b/src/rm_navigation/rm_navigation/rviz/nav2.rviz @@ -66,7 +66,7 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - base_link_fake: + dummy: Alpha: 1 Show Axes: false Show Trail: false @@ -116,6 +116,8 @@ Visualization Manager: Value: true base_link_fake: Value: true + dummy: + Value: true imu_link: Value: true lidar_odom: @@ -194,6 +196,40 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: voxel_grid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_grid + Use Fixed Frame: true + Use rainbow: true + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: diff --git a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation/package.xml b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation/package.xml index 8c78f64218496c230234f46356f9d9825d77bad9..77b9b7db97353a4bd5fdc79fdd328d76b9cccc6f 100644 --- a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation/package.xml +++ b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation/package.xml @@ -11,7 +11,6 @@ ament_cmake eigen - pcl pcl_ros pcl_conversions diff --git a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/package.xml b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/package.xml index 3abfb20eb9b0960ff7a245ce70c3af9590551885..542b350a09ab61ccbdc2edaf2d575cae0abee8c5 100644 --- a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/package.xml +++ b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/package.xml @@ -10,7 +10,6 @@ ament_cmake - eigen_conversions rclcpp tf2_ros sensor_msgs diff --git a/src/rm_simulation/pb_rm_simulation/package.xml b/src/rm_simulation/pb_rm_simulation/package.xml index 6b3b82742f9b5c8a4bd34e8261ac2062cfd47ab2..5f3857b40459fbf9287690d02b1d42fefa43fc53 100644 --- a/src/rm_simulation/pb_rm_simulation/package.xml +++ b/src/rm_simulation/pb_rm_simulation/package.xml @@ -13,9 +13,9 @@ ament_cmake gazebo_ros_pkgs - launch_ros - launch_ros - gazebo_ros + xacro + joint_state_publisher + robot_state_publisher ament_cmake