Ubuntu22.04(ROS2 humble)小车仿真环境搭建

博客地址：https://www.cnblogs.com/zylyehuo/

下载 mobile-3d-lidar-sim

mobile-3d-lidar-sim：ROS2 Humble 社区中最轻量、专门用于 3D 雷达 仿真的项目

这个项目结构非常简单，只有一个机器人模型，且原生配置了 Velodyne 3D 雷达 插件。

1. mkdir -p ~/ros2/mobile-3d-lidar-sim/src
3. cd ~/ros2/mobile-3d-lidar-sim/src
5. git clone https://github.com/louislelay/mobile-3d-lidar-sim.git
7. sudo apt install ros-humble-velodyne-simulator -y
9. cd ..
11. colcon build --symlink-install

复制代码

发布全局静态地图

1. cd /home/zylyehuo/ros2/pcd2pgm_ws2/map/custom
3. ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=drone_map_03.yaml -p use_sim_time:=true

复制代码

1. ros2 run nav2_lifecycle_manager lifecycle_manager --ros-args -p node_names:="['map_server']" -p autostart:=true

复制代码

1. ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom

复制代码

发布局部代价地图

1. cd /home/zylyehuo/ros2/ros2_lexi/src/lexigraph/scripts
3. python3 ./my_costmap.py

复制代码

my_costmap.py

1. #!/usr/bin/env python3
2. import rclpy
3. from rclpy.node import Node
4. import numpy as np
5. from sensor_msgs.msg import PointCloud2
6. from nav_msgs.msg import OccupancyGrid
7. import sensor_msgs_py.point_cloud2 as pc2
8. from scipy.ndimage import distance_transform_edt
9. from tf2_ros import TransformException
10. from tf2_ros.buffer import Buffer
11. from tf2_ros.transform_listener import TransformListener
13. class MapFixedCostmap(Node):
14.     def __init__(self):
15.         super().__init__('map_fixed_costmap_node')
17.         # --- 参数配置 ---
18.         self.declare_parameter('resolution', 0.05)      # 分辨率
19.         self.declare_parameter('width_m', 20.0)         # 局部窗口在 map 中的大小
20.         self.declare_parameter('inflation_r', 0.4)
21.         self.declare_parameter('robot_r', 0.3)
23.         self.res = self.get_parameter('resolution').value
24.         self.width_m = self.get_parameter('width_m').value
25.         self.inflation_r = self.get_parameter('inflation_r').value
26.         self.robot_r = self.get_parameter('robot_r').value
27.         self.grid_dim = int(self.width_m / self.res)
29.         # --- TF 监听器 ---
30.         self.tf_buffer = Buffer()
31.         self.tf_listener = TransformListener(self.tf_buffer, self)
33.         # --- 订阅与发布 ---
34.         self.subscription = self.create_subscription(
35.             PointCloud2, '/velodyne2/velodyne_points2', self.pc_callback, 10)
36.         self.publisher = self.create_publisher(OccupancyGrid, '/my_costmap', 10)
37.         
38.         self.get_logger().info("Costmap Node Started: Fixed to MAP frame.")
40.     def pc_callback(self, msg):
41.         try:
42.             # 1. 获取机器人 (base_link) 在 map 系下的实时位置
43.             try:
44.                 # 获取 map 到 base_link 的变换
45.                 t = self.tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())
46.                 robot_x = t.transform.translation.x
47.                 robot_y = t.transform.translation.y
48.             except TransformException as ex:
49.                 self.get_logger().warn(f'Could not transform base_link to map: {ex}')
50.                 return
52.             # 2. 解析点云
53.             gen = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
54.             points_list = list(gen)
55.             if not points_list:
56.                 self.publish_empty_map(robot_x, robot_y)
57.                 return
58.             
59.             points = np.array([[p[0], p[1], p[2]] for p in points_list], dtype=np.float32)
61.             # 3. 高度过滤
62.             mask = (points[:, 2] > 0.1) & (points[:, 2] < 1.2)
63.             obs_points = points[mask]
65.             # 4. 初始化栅格
66.             grid = np.zeros((self.grid_dim, self.grid_dim), dtype=np.int8)
67.             
68.             # 这里的投影逻辑：
69.             # 点云是在 body 系下的，要发布的地图在 map 系，
70.             # 但栅格的内容依然是机器人观察到的局部障碍物。
71.             # 将栅格的中心(cx, cy)对应机器人当前的 (robot_x, robot_y)
72.             cx, cy = self.grid_dim // 2, self.grid_dim // 2
73.             ix = (obs_points[:, 0] / self.res + cx).astype(int)
74.             iy = (obs_points[:, 1] / self.res + cy).astype(int)
76.             valid = (ix >= 0) & (ix < self.grid_dim) & (iy >= 0) & (iy < self.grid_dim)
77.             grid[iy[valid], ix[valid]] = 100
79.             # 5. 膨胀
80.             final_data = self.inflate_map(grid)
82.             # 6. 发布 (传入机器人当前 map 坐标作为原点参考)
83.             self.publish_map(final_data, robot_x, robot_y)
85.         except Exception as e:
86.             self.get_logger().error(f"Error: {str(e)}")
88.     def inflate_map(self, grid):
89.         if not np.any(grid == 100):
90.             return grid.flatten().astype(np.int8)
91.         dist_map = distance_transform_edt(grid != 100) * self.res
92.         costmap = np.zeros_like(grid, dtype=np.int8)
93.         costmap[dist_map <= self.robot_r] = 100
94.         inf_mask = (dist_map > self.robot_r) & (dist_map <= self.inflation_r)
95.         norm_dist = (dist_map[inf_mask] - self.robot_r) / (self.inflation_r - self.robot_r)
96.         costmap[inf_mask] = (99 * np.exp(-5.0 * norm_dist)).astype(np.int8)
97.         return costmap.flatten()
99.     def publish_empty_map(self, rx, ry):
100.         self.publish_map(np.zeros(self.grid_dim**2, dtype=np.int8), rx, ry)
102.     def publish_map(self, data, rx, ry):
103.         grid_msg = OccupancyGrid()
104.         grid_msg.header.stamp = self.get_clock().now().to_msg()
105.         grid_msg.header.frame_id = 'map'  # 固定在 map 系
106.         
107.         grid_msg.info.resolution = self.res
108.         grid_msg.info.width = self.grid_dim
109.         grid_msg.info.height = self.grid_dim
110.         
111.         # 将 OccupancyGrid 的原点动态设为机器人当前坐标减去地图一半
112.         # 在 map 系下跟随机器人移动
113.         grid_msg.info.origin.position.x = rx - (self.grid_dim * self.res) / 2.0
114.         grid_msg.info.origin.position.y = ry - (self.grid_dim * self.res) / 2.0
115.         grid_msg.info.origin.position.z = 0.0
116.         grid_msg.info.origin.orientation.w = 1.0
117.         
118.         grid_msg.data = data.tolist()
119.         self.publisher.publish(grid_msg)
121. def main(args=None):
122.     rclpy.init(args=args)
123.     node = MapFixedCostmap()
124.     try:
125.         rclpy.spin(node)
126.     except KeyboardInterrupt:
127.         pass
128.     finally:
129.         node.destroy_node()
130.         rclpy.shutdown()
132. if __name__ == '__main__':
133.     main()

复制代码

局部路径规划（lexi避障）

1. cd /home/zylyehuo/ros2/mobile-3d-lidar-sim
3. source ./install/setup.bash
5. ros2 launch my_bot launch_sim.launch.py

复制代码

启动 rviz

1. cd /home/zylyehuo/ros2/ros2_lexi/src/lexigraph/scripts
3. python3 ./hybrid_planner_ros2.py

复制代码

修改 rviz 配置

按照设置的话题对应添加组件

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！