下载 mobile-3d-lidar-sim
mobile-3d-lidar-sim：ROS2 Humble 社区中最轻量、专门用于 3D 雷达 仿真的项目
这个项目结构非常简单，只有一个机器人模型，且原生配置了 Velodyne 3D 雷达 插件。
mkdir -p ~/ros2/mobile-3d-lidar-sim/src
cd ~/ros2/mobile-3d-lidar-sim/src
sudo apt install ros-humble-velodyne-simulator -y
cd ..
colcon build --symlink-install发布全局静态地图
cd /home/zylyehuo/ros2/pcd2pgm_ws2/map/custom
ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=drone_map_03.yaml -p use_sim_time:=true
ros2 run nav2_lifecycle_manager lifecycle_manager --ros-args -p node_names:="['map_server']" -p autostart:=true
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom发布局部代价地图
cd /home/zylyehuo/ros2/ros2_lexi/src/lexigraph/scripts
python3 ./my_costmap.pymy_costmap.py
python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import numpy as np
from sensor_msgs.msg import PointCloud2
from nav_msgs.msg import OccupancyGrid
import sensor_msgs_py.point_cloud2 as pc2
from scipy.ndimage import distance_transform_edt
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
class MapFixedCostmap(Node):
def __init__(self):
super().__init__('map_fixed_costmap_node')
# --- 参数配置 ---
self.declare_parameter('resolution', 0.1) # 分辨率
self.declare_parameter('width_m', 8.0) # 局部窗口在 map 中的大小
self.declare_parameter('inflation_r', 0.8)
self.declare_parameter('robot_r', 0.3)
self.res = self.get_parameter('resolution').value
self.width_m = self.get_parameter('width_m').value
self.inflation_r = self.get_parameter('inflation_r').value
self.robot_r = self.get_parameter('robot_r').value
self.grid_dim = int(self.width_m / self.res)
# --- TF 监听器 ---
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# --- 订阅与发布 ---
self.subscription = self.create_subscription(
PointCloud2, '/velodyne2/velodyne_points2', self.pc_callback, 10)
self.publisher = self.create_publisher(OccupancyGrid, '/my_costmap', 10)
self.get_logger().info("Costmap Node Started: Fixed to MAP frame.")
def pc_callback(self, msg):
try:
# 1. 获取机器人 (base_link) 在 map 系下的实时位置
try:
# 获取 map 到 base_link 的变换
t = self.tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())
robot_x = t.transform.translation.x
robot_y = t.transform.translation.y
except TransformException as ex:
self.get_logger().warn(f'Could not transform base_link to map: {ex}')
return
# 2. 解析点云
gen = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
points_list = list(gen)
if not points_list:
self.publish_empty_map(robot_x, robot_y)
return
points = np.array([[p[0], p[1], p[2]] for p in points_list], dtype=np.float32)
# 3. 高度过滤
mask = (points[:, 2] > 0.1) & (points[:, 2] < 1.2)
obs_points = points[mask]
# 4. 初始化栅格
grid = np.zeros((self.grid_dim, self.grid_dim), dtype=np.int8)
# 这里的投影逻辑：
# 由于点云是在 body 系下的，我们要发布的地图虽然在 map 系，
# 但栅格的内容依然是机器人观察到的局部障碍物。
# 我们将栅格的中心(cx, cy)对应机器人当前的 (robot_x, robot_y)
cx, cy = self.grid_dim // 2, self.grid_dim // 2
ix = (obs_points[:, 0] / self.res + cx).astype(int)
iy = (obs_points[:, 1] / self.res + cy).astype(int)
valid = (ix >= 0) & (ix < self.grid_dim) & (iy >= 0) & (iy < self.grid_dim)
grid[iy[valid], ix[valid]] = 100
# 5. 膨胀
final_data = self.inflate_map(grid)
# 6. 发布 (传入机器人当前 map 坐标作为原点参考)
self.publish_map(final_data, robot_x, robot_y)
except Exception as e:
self.get_logger().error(f"Error: {str(e)}")
def inflate_map(self, grid):
if not np.any(grid == 100):
return grid.flatten().astype(np.int8)
dist_map = distance_transform_edt(grid != 100) * self.res
costmap = np.zeros_like(grid, dtype=np.int8)
costmap[dist_map <= self.robot_r] = 100
inf_mask = (dist_map > self.robot_r) & (dist_map <= self.inflation_r)
norm_dist = (dist_map[inf_mask] - self.robot_r) / (self.inflation_r - self.robot_r)
costmap[inf_mask] = (99 * np.exp(-5.0 * norm_dist)).astype(np.int8)
return costmap.flatten()
def publish_empty_map(self, rx, ry):
self.publish_map(np.zeros(self.grid_dim**2, dtype=np.int8), rx, ry)
def publish_map(self, data, rx, ry):
grid_msg = OccupancyGrid()
grid_msg.header.stamp = self.get_clock().now().to_msg()
grid_msg.header.frame_id = 'map' # 核心：固定在 map 系
grid_msg.info.resolution = self.res
grid_msg.info.width = self.grid_dim
grid_msg.info.height = self.grid_dim
# 核心：将 OccupancyGrid 的原点动态设为机器人当前坐标减去地图一半
# 这样局部地图就会像一个"探照灯"一样在 map 系下跟随机器人移动
grid_msg.info.origin.position.x = rx - (self.grid_dim * self.res) / 2.0
grid_msg.info.origin.position.y = ry - (self.grid_dim * self.res) / 2.0
grid_msg.info.origin.position.z = 0.0
grid_msg.info.origin.orientation.w = 1.0
grid_msg.data = data.tolist()
self.publisher.publish(grid_msg)
def main(args=None):
rclpy.init(args=args)
node = MapFixedCostmap()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()运行仿真环境
cd /home/zylyehuo/ros2/mobile-3d-lidar-sim
source ./install/setup.bash
ros2 launch my_bot launch_sim.launch.py
全局路径规划
cd /home/zylyehuo/ros2/ros2_lexi/src/lexigraph/scripts
python3 ./hybrid_planner_ros2.py局部路径规划（lexi避障）
cd /home/zylyehuo/ros2/ros2_lexi
source ./install/setup.bash
ros2 launch lexigraph run.launch.py启动 rviz
rviz2
修改 rviz 配置
按照设置的话题对应添加组件
