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

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

下载 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 git clone https://github.com/louislelay/mobile-3d-lidar-sim.git 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.py

my_costmap.py

/* by 01130.hk - online tools website : 01130.hk/zh/editor.html */ #!/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

全局路径规划（hybrid-A*，Dubins）

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

hybrid_planner_ros2.py

/* by 01130.hk - online tools website : 01130.hk/zh/editor.html */ #!/usr/bin/env python3 import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy import numpy as np import heapq import math from nav_msgs.msg import OccupancyGrid, Path, Odometry from geometry_msgs.msg import PoseStamped, Point from scipy.ndimage import binary_dilation # --- 辅助函数: 四元数转欧拉角 (避免依赖 tf_transformations) --- def euler_from_quaternion(q): """ Convert a list/obj of [x, y, z, w] to [roll, pitch, yaw] """ x, y, z, w = q.x, q.y, q.z, q.w t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) roll_x = math.atan2(t0, t1) t2 = +2.0 * (w * y - z * x) t2 = +1.0 if t2 > +1.0 else t2 t2 = -1.0 if t2 < -1.0 else t2 pitch_y = math.asin(t2) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (y * y + z * z) yaw_z = math.atan2(t3, t4) return roll_x, pitch_y, yaw_z # --- Dubins 曲线规划器 --- class DubinsPath: def __init__(self, start, end, curvature, step_size=0.1): self.sx, self.sy, self.syaw = start self.gx, self.gy, self.gyaw = end self.c = curvature self.r = 1.0 / curvature self.step_size = step_size self.path_x = [] self.path_y = [] self.path_yaw = [] self.cost = float('inf') self.mode = "" def mod2pi(theta): return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi) def pi_2_pi(angle): return (angle + math.pi) % (2 * math.pi) - math.pi def dubins_path_planning(sx, sy, syaw, gx, gy, gyaw, c, step_size=0.1): """ 计算最短的 Dubins 路径 """ gx -= sx gy -= sy l_rot = math.atan2(gy, gx) l_dist = math.hypot(gx, gy) # 将目标旋转到以(0,0,0)为起点的坐标系 theta = mod2pi(l_rot - syaw) alpha = mod2pi(syaw - l_rot) beta = mod2pi(gyaw - l_rot) # 归一化距离 d = l_dist * c best_cost = float('inf') best_mode = None best_lengths = None # 定义6种Dubins模式 planners = [lsl, rsr, lsr, rsl, rlr, lrl] modes = ["LSL", "RSR", "LSR", "RSL", "RLR", "LRL"] for planner, mode in zip(planners, modes): lengths = planner(alpha, beta, d) if lengths is None: continue cost = sum(map(abs, lengths)) if cost < best_cost: best_cost = cost best_mode = mode best_lengths = lengths if best_mode is None: return None # 生成路径点 lengths = best_lengths px, py, pyaw = [sx], [sy], [syaw] # 辅助生成函数 def interpolate(length, mode_char, origin_x, origin_y, origin_yaw): dist = 0.0 curr_x, curr_y, curr_yaw = origin_x, origin_y, origin_yaw step = step_size if mode_char == 'S': d_step = step else: d_step = step * c # 弧度步长 while dist < length: dist += step if dist >= length: # 修正最后一步 d_step -= (dist - length) * (c if mode_char != 'S' else 1.0) dist = length if mode_char == 'L': curr_yaw += d_step elif mode_char == 'R': curr_yaw -= d_step # 移动 if mode_char == 'S': curr_x += d_step * math.cos(curr_yaw) curr_y += d_step * math.sin(curr_yaw) else: # 弧线运动 DX = 2*R*sin(d_theta/2)*cos(theta + d_theta/2) 近似 # 这里简单积分 curr_x += step * math.cos(curr_yaw) curr_y += step * math.sin(curr_yaw) px.append(curr_x) py.append(curr_y) pyaw.append(curr_yaw) return curr_x, curr_y, curr_yaw # 根据最佳模式生成 cx, cy, cyaw = sx, sy, syaw for i, m in enumerate(best_mode): l_segment = lengths[i] / c cx, cy, cyaw = interpolate(l_segment, m, cx, cy, cyaw) path = DubinsPath((sx, sy, syaw), (gx, gy, gyaw), c, step_size) path.path_x = px path.path_y = py path.path_yaw = pyaw path.cost = best_cost / c # 真实长度 path.mode = best_mode return path # --- Dubins 公式实现 (归一化坐标下) --- def lsl(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) tmp0 = d + sa - sb p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb)) if p_squared < 0: return None tmp1 = math.atan2((cb - ca), tmp0) t = mod2pi(-alpha + tmp1) p = math.sqrt(p_squared) q = mod2pi(beta - tmp1) return t, p, q def rsr(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) tmp0 = d - sa + sb p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa)) if p_squared < 0: return None tmp1 = math.atan2((ca - cb), tmp0) t = mod2pi(alpha - tmp1) p = math.sqrt(p_squared) q = mod2pi(-beta + tmp1) return t, p, q def lsr(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb)) if p_squared < 0: return None p = math.sqrt(p_squared) tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p) t = mod2pi(-alpha + tmp2) q = mod2pi(-mod2pi(beta) + tmp2) return t, p, q def rsl(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb)) if p_squared < 0: return None p = math.sqrt(p_squared) tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p) t = mod2pi(alpha - tmp2) q = mod2pi(beta - tmp2) return t, p, q def rlr(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0 if abs(tmp_rlr) > 1.0: return None p = mod2pi(2 * math.pi - math.acos(tmp_rlr)) t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0)) q = mod2pi(alpha - beta - t + mod2pi(p)) return t, p, q def lrl(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) cb = math.cos(beta) c_ab = math.cos(alpha - beta) tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0 if abs(tmp_lrl) > 1.0: return None p = mod2pi(2 * math.pi - math.acos(tmp_lrl)) t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0) q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p)) return t, p, q # --- 节点类 --- class NodeItem: def __init__(self, x_ind, y_ind, theta_ind, x, y, yaw, x_list, y_list, p_ind, cost, steer): self.x_ind = x_ind self.y_ind = y_ind self.theta_ind = theta_ind self.x = x self.y = y self.yaw = yaw self.x_list = x_list self.y_list = y_list self.p_ind = p_ind self.cost = cost self.steer = steer class HybridAStarPlanner(Node): def __init__(self): super().__init__('hybrid_astar_planner') # --- Ranger Mini 3.0 参数 --- self.WB = 0.494 self.MAX_STEER = 0.55 self.ROBOT_RADIUS = 0.45 # --- 规划参数 --- self.XY_RES = 0.1 self.YAW_RES = 0.15 # 稍微增大角度分辨率以减少搜索空间 self.MOTION_STEP = 0.1 # Dubins 最小转弯半径 R_min # tan(max_steer) = WB / R # R = WB / tan(max_steer) self.MIN_TURN_RADIUS = self.WB / math.tan(self.MAX_STEER) # 对应的曲率 (1/R) self.MAX_CURVATURE = 1.0 / self.MIN_TURN_RADIUS * 0.95 # 留一点余量 self.get_logger().info(f"Ranger Mini Params: WB={self.WB}, R_min={self.MIN_TURN_RADIUS:.2f}m") # --- 内部变量 --- self.map_data = None self.costmap = None self.map_info = None self.current_pose = None # --- ROS 2 通信 --- # Map QoS: 必须是 Transient Local 才能收到并在后续保留 map_server 发出的地图 map_qos = QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, ) self.path_pub = self.create_publisher(Path, '/bezai_path', 10) self.create_subscription(OccupancyGrid, '/map', self.map_callback, map_qos) self.create_subscription(Odometry, '/odom', self.odom_callback, 10) self.create_subscription(PoseStamped, '/goal_pose', self.goal_callback, 10) # RViz2 默认话题通常是 /goal_pose self.get_logger().info("Hybrid A* Planner (ROS 2 + Dubins) Ready.") def map_callback(self, msg): self.map_info = msg.info self.XY_RES = msg.info.resolution w, h = msg.info.width, msg.info.height grid = np.array(msg.data).reshape((h, w)) obstacles = np.where((grid > 50) | (grid == -1), 1, 0) # 膨胀 inflation_cells = int(np.ceil(self.ROBOT_RADIUS / self.XY_RES)) structure = np.ones((2*inflation_cells+1, 2*inflation_cells+1)) self.costmap = binary_dilation(obstacles, structure=structure).astype(int) self.costmap = self.costmap.T # 转置以匹配 x,y 索引 self.get_logger().info("Map updated & Inflated.") def odom_callback(self, msg): p = msg.pose.pose.position o = msg.pose.pose.orientation _, _, yaw = euler_from_quaternion(o) self.current_pose = (p.x, p.y, yaw) def goal_callback(self, msg): if self.costmap is None: self.get_logger().warn("Map not received!") return if self.current_pose is None: self.get_logger().warn("Odom not received!") return self.get_logger().info("Goal Received. Planning...") g_x = msg.pose.position.x g_y = msg.pose.position.y _, _, g_yaw = euler_from_quaternion(msg.pose.orientation) path_points = self.hybrid_a_star(self.current_pose, (g_x, g_y, g_yaw)) if path_points: self.publish_path(path_points) else: self.get_logger().warn("No path found!") def hybrid_a_star(self, start, goal): sx, sy, syaw = start gx, gy, gyaw = goal sx_ind, sy_ind = self.coord2grid(sx, sy) syaw_ind = int(pi_2_pi(syaw) / self.YAW_RES) start_node = NodeItem(sx_ind, sy_ind, syaw_ind, sx, sy, syaw, [sx], [sy], -1, 0, 0) open_list = {} open_list[self.calc_index(start_node)] = start_node pq = [] heapq.heappush(pq, (self.calc_cost(start_node, goal), self.calc_index(start_node))) closed_set = {} # 限制迭代次数 iter_count = 0 max_iter = 50000 while iter_count < max_iter: iter_count += 1 if not pq: return None _, c_id = heapq.heappop(pq) if c_id in open_list: current = open_list[c_id] del open_list[c_id] else: continue closed_set[c_id] = current # --- Analytic Expansion (Dubins Shot) --- # 尝试直接用 Dubins 曲线连接当前点和终点 # 为节省算力，每隔N次尝试或当距离足够近时尝试 dist_to_goal = np.hypot(current.x - gx, current.y - gy) # 策略：如果距离小于一定值，或者随机尝试（这里设置为始终尝试，追求最优解） # 如果障碍物很密集，这一步会经常失败，耗费计算 if dist_to_goal < 10.0: # 仅在 10m 范围内尝试直接连接 dubins_path = dubins_path_planning( current.x, current.y, current.yaw, gx, gy, gyaw, self.MAX_CURVATURE, self.MOTION_STEP ) if dubins_path and self.check_dubins_collision(dubins_path): self.get_logger().info(f"Dubins shot found! Mode: {dubins_path.mode}") return self.reconstruct_path_with_dubins(current, closed_set, dubins_path) # 正常 Hybrid A* 扩展 steer_inputs = [-self.MAX_STEER, 0, self.MAX_STEER] for steer in steer_inputs: node = self.kinematic_move(current, steer, self.MOTION_STEP) if not self.check_collision(node): continue node_ind = self.calc_index(node) if node_ind in closed_set: continue if node_ind not in open_list: new_cost = self.calc_cost(node, goal) open_list[node_ind] = node heapq.heappush(pq, (new_cost, node_ind)) else: if open_list[node_ind].cost > node.cost: open_list[node_ind] = node new_cost = self.calc_cost(node, goal) heapq.heappush(pq, (new_cost, node_ind)) return None def kinematic_move(self, node, steer, dt): x, y, yaw = node.x, node.y, node.yaw dist = 0.6 # 扩展弧长 step = 0.1 # 积分步长 traj_x, traj_y = [], [] d = 0 while d < dist: x += step * math.cos(yaw) y += step * math.sin(yaw) yaw += step / self.WB * math.tan(steer) d += step traj_x.append(x) traj_y.append(y) yaw = pi_2_pi(yaw) x_ind, y_ind = self.coord2grid(x, y) yaw_ind = int(yaw / self.YAW_RES) # cost 包含移动代价 + 转向惩罚 cost = node.cost + dist + abs(steer)*0.5 new_node = NodeItem(x_ind, y_ind, yaw_ind, x, y, yaw, traj_x, traj_y, self.calc_index(node), cost, steer) return new_node def check_collision(self, node): for x, y in zip(node.x_list, node.y_list): ix, iy = self.coord2grid(x, y) if self.is_collision(ix, iy): return False return True def check_dubins_collision(self, dubins_path): for x, y in zip(dubins_path.path_x, dubins_path.path_y): ix, iy = self.coord2grid(x, y) if self.is_collision(ix, iy): return False return True def is_collision(self, ix, iy): if ix < 0 or ix >= self.costmap.shape[0] or iy < 0 or iy >= self.costmap.shape[1]: return True # 出界视为碰撞 if self.costmap[ix][iy] == 1: return True return False def calc_cost(self, node, goal): # 启发式函数 # Euclidean Dist h = np.hypot(node.x - goal[0], node.y - goal[1]) return node.cost + h * 1.2 def reconstruct_path_with_dubins(self, current, closed_set, dubins_path): # 1. 回溯之前的路径 path_x, path_y = [], [] curr = current while curr.p_ind != -1: path_x.extend(reversed(curr.x_list)) path_y.extend(reversed(curr.y_list)) curr = closed_set[curr.p_ind] path_x.append(curr.x) path_y.append(curr.y) # 反转得到从起点到current的顺序 final_x = list(reversed(path_x)) final_y = list(reversed(path_y)) # 2. 加上 Dubins 部分 # Dubins路径本身就是顺序的，直接添加 final_x.extend(dubins_path.path_x) final_y.extend(dubins_path.path_y) return list(zip(final_x, final_y)) def publish_path(self, points): path = Path() path.header.frame_id = "map" path.header.stamp = self.get_clock().now().to_msg() for p in points: pose = PoseStamped() pose.header.frame_id = "map" pose.pose.position.x = p[0] pose.pose.position.y = p[1] pose.pose.position.z = 0.0 pose.pose.orientation.w = 1.0 path.poses.append(pose) self.path_pub.publish(path) def coord2grid(self, x, y): gx = int((x - self.map_info.origin.position.x) / self.XY_RES) gy = int((y - self.map_info.origin.position.y) / self.XY_RES) return gx, gy def calc_index(self, node): return (node.x_ind, node.y_ind, node.theta_ind) def main(args=None): rclpy.init(args=args) planner = HybridAStarPlanner() try: rclpy.spin(planner) except KeyboardInterrupt: pass finally: planner.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

局部路径规划（lexi避障）

cd /home/zylyehuo/ros2/ros2_lexi source ./install/setup.bash ros2 launch lexigraph run.launch.py

启动 rviz

rviz2

修改 rviz 配置

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