# workspace_ros_contnav_global_planner **Repository Path**: yongwangzhiqiankai/workspace_ros_contnav_global_planner ## Basic Information - **Project Name**: workspace_ros_contnav_global_planner - **Description**: 板结构路径规划，demo试验版本，并不是落地项目；代码并不稳定，还存在bug，落地需要重构。 - **Primary Language**: Unknown - **License**: Not specified - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 4 - **Forks**: 6 - **Created**: 2023-11-09 - **Last Updated**: 2025-02-11 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README /home/sukai/workspace/0/137/robot_noetic 可运行的通用web系统 ================================================================== 开源 contnav_global_planner 半结构路径规划 sukai 2023 03-31 系统 ubuntu 20.04 ros noetic 作者： 苏凯 邮箱：422168787@qq.com 启动仿真 roslaunch turtlebot3_navigation contnavWebrviz.launch rosrun contnav_global_planner contnav_global_planner_node rosrun contnav_global_planner contnav_moveAction_node roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch roslaunch contnav_global_planner contnav_global_planner.launch rosrun rqt_service_caller rqt_service_caller /my_global_planner request_type="makePlan" request_algorithm="AStar" git clone https://github.com/rst-tu-dortmund/costmap_converter.git ---目标坐标 request.request_goal.pose.position.x = -0.876; request.request_goal.pose.position.y = 1.0; request.request_goal.pose.position.z = 0.0; request.request_goal.pose.orientation.x = 0.0; request.request_goal.pose.orientation.y = 0.0; request.request_goal.pose.orientation.z = 0.0; request.request_goal.pose.orientation.w = 1.0; =================================================================== --- 查看 action sudo apt-get install ros-noetic-actionlib-tools 发送action rosrun actionlib_tools axclient.py /move_base_flex/move_base Action消息 rosmsg list | grep Action --- /contnav_global_planner_node/manyPath --- string Dijkstra="Dijkstra" string AStar="AStar" ============================== --一阶贝塞尔 request_type="Bezier" request_BezierNum="FirstBezier" ---二阶贝塞尔 request_type="Bezier" request_BezierNum="QuadraticBezier" ---三阶贝塞尔 request_type="Bezier" request_BezierNum="CubicBezier" ---四阶贝塞尔 request_type="Bezier" request_BezierNum="EquationBezier" =========service srv======================================== SERVICE: /my_global_planner contnav_srvs/global_planner string request_type string makePlan=makePlan 路径规划 string Bezier=Bezier 贝赛尔曲线规划 string waypoint=waypoint 通过rviz或图形界面选择点规划路径 string readpose=readpose 读取txt中的path plan数据 /home/sukai/workspace/workspace_ros_car_noetic/src/contnav_global_planner/output/planpose/plan.txt string readpose=clear_costmaps 清空代价地图 string request_algorithm string Dijkstra=Dijkstra dijkstra 算法 string AStar=AStar A*算法 string request_BezierNum string FirstBezier=FirstBezier 一阶贝塞尔 string QuadraticBezier=QuadraticBezier 二阶贝塞尔 string CubicBezier=CubicBezier 三阶贝塞尔 string EquationBezier=EquationBezier 四阶贝塞尔 uint8 CALCULATEPATH=0 //计算一条路径 uint8 DELETE=2 //删除上一次增加的点 uint8 DELETEALL=3 //删除所有队列中的点 uint8 DELETEMARKER=4 //删除指定命名空间（用当前地图名称作命名空间）与id的一条marker路径 uint8 DELETEALLARKER=5 //删除所有marker路径 uint8 DELETEMORPLANTOMAP=6 ////清空 execute_plan_容器中所有需要执行的路径 uint8 LOADINGPATH=7 //把所有半结构化路径在地图中的索引重新放在一个容器里 uint8 GETPLAN=8 //获取路径 int32 waypointAction string waypointActionPlanType // 路径规划:"makePlan" , 贝赛尔曲线规划:"Bezier" ;当参数waypointAction =CALCULATEPATH=7 ，可选择路径规划类型 :makePlan=makePlan 路径规划（取规划器 string request_algorithm 的参数作为当前规划器 ，默认a*），Bezier=Bezier 贝赛尔曲线规划（调用1阶贝赛尔作为直线插值） bool isthresholdClosestPoint // true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择） bool isSendGoalPlanOverturn // 是否反转路径导航，true 反转路径导航，false 正常路径导航 bool isOneWay // (给一次反转规划)是否单向导航 true 单向导航，false 双向导航;当路径的起始点与结束点在一条路径上，且结束的点在起始点前方，是否允许它反转路径重新规划路径 string request_marker_namespace //用当前地图名称+场景编号作命名空间 int32 request_marker_id //marker路径的id string request_plan_type //路径规划的类型 bool request_isPlanArc //true 转角处做圆弧导航， 是否用规划圆弧的路径做全局规划，false 转角处不做圆弧导航，LOADINGPATH=7的时候使用 int32 request_plan_id //唯一id string request_plan_name //随意起名称 string request_plan_ns //用当前地图名称+场景编号作命名空间 int32[] request_target_plan_ids //当前路径行走结束后，可以到达的路径id geometry_msgs/PoseStamped request_start 起始点 geometry_msgs/PoseStamped request_goal 目标点 geometry_msgs/PoseStamped request_default_goal 默认目标点,可以是充电点位置 （必须给） geometry_msgs/PoseStamped request_pc1 控制点1 geometry_msgs/PoseStamped request_pc2 控制点2 geometry_msgs/PoseStamped request_pc3 控制点3 --- string result 返回 string message 返回信息 nav_msgs/Path result_Path 返回路径 =========================================================== moveAction =========================================================== 路径发布topic /contnav_global_planner_node/plan =========================================================== turtlebot3_navigation_my.launch contnavWebrviz.launch roslaunch turtlebot3_gazebo turtlebot3_house.launch roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch roslaunch turtlebot3_navigation turtlebot3_navigation_neo.launch map_file:=/home/sukai/slam/map/map_1.yaml roslaunch contnav contnavWebrviz.launch roslaunch contnav contnavTest.launch rosrun contnav currency_web_node roslaunch contnav contnav.launch ======================================================================================= ----------------------------------------------- /pause_cmd_vel std_srvs::SetBool> 这个话题会控制 move_base_flex 中的 cmd_vel 的启动和停止，会导致车不能行走；（自己改的源码 move_base_flex ） https://blog.csdn.net/qq_15204179/article/details/130639912 记录了博客 ---------------------------------------------------- /move_base_simple/goal --------------------------------------------------- cmd_vel_client_暂停速度发布 #include #include int main(int argc, char **argv) { ros::init(argc, argv, "cmd_vel_control_client"); ros::NodeHandle nh; ros::ServiceClient client = nh.serviceClient("pause_cmd_vel"); std_srvs::SetBool srv; srv.request.data = true; // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布 if (client.call(srv)) { ROS_INFO("Service call succeeded: %s", srv.response.message.c_str()); } else { ROS_ERROR("Failed to call service pause_cmd_vel"); } return 0; } ---- 在 move_base_flex/mbf_abstract_nav/include/abstract_controller_execution.h 文件中添加一个新的服务服务器和布尔变量，如下所示： 改 //sukai #include ros::ServiceServer pause_cmd_vel_service_; bool pause_cmd_vel_; bool pauseCmdVelCB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); move_base_flex/mbf_abstract_nav/src/abstract_controller_execution.cpp 改 pause_cmd_vel_ = false; pause_cmd_vel_service_ = nh.advertiseService("pause_cmd_vel", &AbstractControllerExecution::pauseCmdVelCB, this); bool AbstractControllerExecution::pauseCmdVelCB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { pause_cmd_vel_ = req.data; res.success = true; res.message = req.data ? "Pausing /cmd_vel topic." : "Resuming /cmd_vel topic."; return true; } if (pause_cmd_vel_) //389 { //sukai 控制/cmd_vel 这个话题的暂停和启动 continue; } =========================================================================================== //1.路径可视化 带圆弧 visualization_msgs::Marker markerArc; //std::vector planArc; nav_msgs::Path gui_arc_path;//生成的路径,带圆弧 bool publishPlanMarkerFunfig1 = publishPlanMarkerFun(markerArc,plan,gui_arc_path); if(publishPlanMarkerFunfig1) { //发布可视化 plan_arc_pub_.publish(gui_path);//发布路径 带圆弧 marker_arc_pub_.publish(markerArc);//发布多路径 带圆弧 } ============================================ 这里选择global_planner做为全局规划器，对于global_planner的具体参数，这里创建一个global_planner_params.yaml文件，用于管理相关参数，完整参数介绍可参见：http://wiki.ros.org/global_planner 下面来依次解释下各参数意义： allow_unknown(default: true): 是否允许规划器规划穿过未知区域的路径,只设计该参数为true还不行,还要在costmap_commons_params.yaml中设置track_unknown_space参数也为false才行。 default_tolerance(default: 0.0): 当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点. visualize_potential(default: false): 是否显示从PointCloud2计算得到的势区域. use_dijkstra(default: true): 如果设置为true,将使用dijkstra算法,否则使用A*算法. use_quadratic(default: true): 如果设置为true,将使用二次函数近似函数,否则使用更加简单的计算方式,这样节省硬件计算资源. use_grid_path(default: false): 如果如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点. old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true. lethal_cost(default: 253): 致命代价值 neutral_cost(default: 50): 中等代价值 cost_factor(default: 3.0): 代价地图与每个代价值相乘的因子 publish_potential(default: true): 是否发布costmap的势函数 orientation_mode(default: 0): 如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6） orientation_window_size(default: 1): 根据orientation_mode指定的位置积分来得到使用窗口的方向。 典型建议值： /home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/img 效果 lethal_cost = 253 //致命代价值 neutral_cost = 66 //中等代价值 cost_factor = 0.55 //代价地图与每个代价值相乘的因子 ———————————————— ========================================================================= string request_type geometry_msgs/PoseStamped start geometry_msgs/PoseStamped goal geometry_msgs/Pose pose --- string result string message geometry_msgs/PoseStamped[] result_plans ===================================== ---- ============================================================= 主要用到的movebaseflex mbf_bridge_node.cpp 实现与原生 maovebase 在rviz上一样的功能； state_machine.cpp rosrun rqt_service_caller rqt_service_caller =========================================== 一条路径拐角处改成圆弧路径 假设有一条路径，由一系列线段连接而成。希望在路径的某个拐角处，将两个相邻的线段替换成圆弧路径，以使路径更加平滑。 找到路径中的拐角处 需要遍历路径上的每个点，并检查的曲率是否足够大，以被认为是一个拐角。可以使用一些曲率计算方法，例如计算相邻线段的夹角或计算相邻线段的交叉点到拐角点的距离等。 计算圆弧路径的参数 一旦找到拐角点，需要计算圆弧路径的参数，包括圆弧半径、圆心位置和起始角度/终止角度等。通常，可以使用相邻线段的起始点和终止点以及拐角点，来计算这些参数。 用圆弧路径替换线段 最后，使用计算出的参数，创建一条圆弧路径，并将其插入到路径中，替换原来的线段。还需要更新相邻线段的起点和终点，以确保与新的圆弧路径相连，并形成一个平滑的路径。 --- #include #include #include using namespace std; const double PI = 3.14159265358979323846; struct Point { double x; double y; }; struct Arc { Point center; double radius; double start_angle; double end_angle; }; double distance(Point a, Point b) { double dx = a.x - b.x; double dy = a.y - b.y; return sqrt(dx * dx + dy * dy); } // 计算两个向量之间的夹角 double angle_between_vectors(Point a, Point b, Point c) { double ux = a.x - b.x; double uy = a.y - b.y; double vx = c.x - b.x; double vy = c.y - b.y; double dot_product = ux * vx + uy * vy; double cos_theta = dot_product / (distance(a, b) * distance(c, b)); return acos(cos_theta); } // 计算两点之间的角度 double angle_between_points(Point a, Point b, Point c) { double theta = angle_between_vectors(a, b, c); if (b.y > a.y) { theta = 2 * PI - theta; } return theta; } // 将拐角处替换为圆弧路径 vector path_with_rounded_corners(vector path, double corner_radius) { vector rounded_path; for (int i = 0; i < path.size() - 2; i++) { Point a = path[i]; Point b = path[i+1]; Point c = path[i+2]; double angle = angle_between_points(a, b, c); if (angle < PI) { rounded_path.push_back(b); } else { double half_angle = angle / 2; double chord_length = distance(b, c); double arc_length = corner_radius * tan(half_angle); double arc_radius = corner_radius / sin(half_angle); Point chord_midpoint = { (b.x + c.x) / 2, (b.y + c.y) / 2 }; Point arc_center = { chord_midpoint.x - arc_radius * cos(angle_between_points(b, chord_midpoint, c)), chord_midpoint.y - arc_radius * sin(angle_between_points(b, chord_midpoint, c)) }; double start_angle = angle_between_points(arc_center, a, b); double end_angle = angle_between_points(arc_center, b, c); if (start_angle < 0) { start_angle += 2 * PI; } if (end_angle < 0) { end_angle += 2 * PI; } if (end_angle < start_angle) { end_angle += 2 * PI; } double current_angle = start_angle; while (current_angle < end_angle) { Point arc_point = { arc_center.x + arc_radius * cos(current_angle), arc_center.y + arc_radius * sin(current_angle) }; rounded_path.push_back(arc_point); current_angle += arc_length / arc_radius; } } } return rounded_path; } int main() { vector path = {{0, 0}, {1, 1}, {2, 1}, {3, 2}, {4, 2}, {5, 0}}; double corner_radius = 1.0; vector rounded_path = path_with_rounded_corners(path, corner_radius); cout << "Original path: "; for (Point p : path) { cout << "(" << p.x << ", " << p.y << ") "; } cout << endl; cout << "Path with rounded corners: "; for (Point p : rounded_path) { cout << "(" << p.x << ", " << p.y << ") "; } cout << endl; return 0; } 代码中，`Point`结构体代表二维空间中的点，`Arc`结构体代表圆弧，`distance`函数计算两个点之间的距离，`angle_between_vectors`函数计算两个向量之间的夹角，`angle_between_points`函数计算三个点之间的角度，`path_with_rounded_corners`函数将路径上的拐角替换为圆弧路径。 在`main`函数中，首先定义了一条路径，然后指定了圆弧半径，并调用`path_with_rounded_corners`函数将路径上的拐角替换为圆弧路径。最后，打印出原始路径和替换后的路径。 定义了一个 Point 结构体来表示二维平面上的点，包含了 x 和 y 两个成员变量。然后定义了一个 Arc 结构体来表示一个圆弧，包含了圆心坐标、半径和起始角度和结束角度。还定义了一个常量 PI，用于存储圆周率的值。 struct Point { double x; double y; }; struct Arc { Point center; double radius; double start_angle; double end_angle; }; const double PI = 3.14159265358979323846; ##定义了一个 distance 函数来计算两个点之间的距离，使用欧几里得距离公式来实现。 double distance(Point a, Point b) { double dx = a.x - b.x; double dy = a.y - b.y; return sqrt(dx * dx + dy * dy); } ## angle_between_vectors 函数来计算两个向量之间的夹角，使用向量点积和余弦定理来实现。 double angle_between_vectors(Point a, Point b, Point c) { double ux = a.x - b.x; double uy = a.y - b.y; double vx = c.x - b.x; double vy = c.y - b.y; double dot_product = ux * vx + uy * vy; double cos_theta = dot_product / (distance(a, b) * distance(c, b)); return acos(cos_theta); } ## angle_between_points 函数来计算三个点之间的角度，它首先调用 angle_between_vectors 函数来计算夹角，然后根据向量的位置关系计算三个点之间的角度。 double angle_between_points(Point a, Point b, Point c) { double theta = angle_between_vectors(a, b, c); if (b.y > a.y) { theta = 2 * PI - theta; } return theta; } 主函数 path_with_rounded_corners。这个函数接受一条路径和一个圆角半径作为输入，输出经过圆角替换的路径。首先定义了一个 rounded_path 数组来存储替换后的路径。 这个函数使用一个循环来遍历整个路径，对于每个拐角点，它判断这个拐角点的角度是否小于 180 度。如果是，则说明这个拐角点是一个锐角，就直接将这个点加入到 rounded_path 数组中。如果不是，则说明这个拐角点是一个钝角，需要用圆弧来替换这个拐角点。 对于钝角，需要计算圆弧的半径、圆心和起始角度和结束角度。这里使用了三个点来计算圆心和起始角度和结束角度。首先计算前一个点和当前拐角点之间的向量和后一个点和当前拐角点之间的向量，然后使用 angle_between_vectors 函数计算这两个向量之间的夹角，再用 angle_between_points 函数计算三个点之间的角度。最后使用一些三角函数来计算圆弧的半径、圆心和起始角度和结束角度。 vector path_with_rounded_corners(vector path, double corner_radius) { vector rounded_path; for (int i = 1; i < path.size() - 1; i++) { Point prev = path[i - 1]; Point curr = path[i]; Point next = path[i + 1]; double angle = angle_between_points(prev, curr, next); if (angle < PI) { rounded_path.push_back(curr); } else { double r = corner_radius; double d1 = distance(prev, curr); double d2 = distance(curr, next); double alpha = angle_between_vectors(next, curr, prev); double beta = angle / 2; double gamma = PI - alpha - beta; double h = r * sin(beta) / sin(gamma); Point center1 = {curr.x + h * (prev.y - curr.y) / d1, curr.y + h * (curr.x - prev.x) / d1}; Point center2 = {curr.x + h * (next.y - curr.y) / d2, curr.y + h * (curr.x - next.x) / d2}; double start_angle = angle_between_points(center1, curr, prev); double end_angle = angle_between_points(center2, curr, next); if (start_angle > end_angle) { end_angle += 2 * PI; } for (double t = start_angle; t < end_angle; t += r / h) { double x = center1.x + h * cos(t); double y = center1.y + h * sin(t); rounded_path.push_back({x, y}); } } } rounded_path.push_back(path.back()); return rounded_path; } 在 main 函数中，定义了一条路径，指定了圆弧半径，最后调用 path_with_rounded_corners 函数将路径上的拐角替换为圆弧路径。分别打印出原始路径和替换后的路径。 int main() { vector path = {{0, 0}, {1, 1}, {2, 1}, {3, 2}, {4, 2}, {5, 0}}; double corner_radius = 1.0; vector rounded_path = path_with_rounded_corners(path, corner_radius); cout << "Original path: "; for (Point p : path) { cout << "(" << p.x << ", " << p.y << ") "; } cout << endl; cout << "Path with rounded corners: "; for (Point p : rounded_path) { cout << "(" << p.x << ", " << p.y << ") "; } cout << endl; return 0; } --- 用于根据已知的等边三角形的定点坐标 $A$ 和 $B$ 计算顶点 $C$ 的坐标： cpp #include #include using namespace std; struct Point { double x, y; }; int main() { Point A, B, C; double a; // 等边三角形的边长 // 输入 A、B 点坐标和等边三角形的边长 cout << "请输入等边三角形的边长："; cin >> a; cout << "请输入点 A 的坐标（x,y）："; cin >> A.x >> A.y; cout << "请输入点 B 的坐标（x,y）："; cin >> B.x >> B.y; // 计算顶点 C 的坐标 double dx = B.x - A.x; double dy = B.y - A.y; double angle = atan2(dy, dx) + M_PI / 3.0; // 顶角的角度为 60 度 C.x = A.x + a * cos(angle); C.y = A.y + a * sin(angle); // 输出顶点 C 的坐标 cout << "顶点 C 的坐标为：" << C.x << "," << C.y << endl; return 0; } 注意，在计算顶点 $C$ 的坐标时，我们需要先计算向量 $\vec{AB}$ 的方向角度 $\theta$，然后将其增加 $60^{\circ}$，得到顶角的角度 $\alpha = \theta + 60^{\circ}$，最后根据正弦余弦函数计算出顶点 $C$ 的坐标。 --- 半结构化全局路径规划通常是指需要在复杂的环境中为机器人或自主车辆等智能体规划路径，考虑到不同约束和目标的情况下能够从起点到达终点。 以下是一个基本的半结构化全局路径规划的流程： 定义地图：确定规划环境的地图，可以使用现有的地图数据或构建自己的地图。 定义起点和终点：确定需要规划路径的起点和终点。 定义约束条件：确定路径规划的约束条件，例如障碍物、禁止区域、车辆的最大速度等。 选择算法：根据环境和约束条件选择合适的路径规划算法，例如A*算法、Dijkstra算法、RRT算法等。 执行路径规划：使用所选算法在地图上执行路径规划，并生成从起点到终点的最优路径。 优化路径：对生成的路径进行优化，例如减少路径长度或者时间，使路径更加平滑或者满足其他目标。 实施路径：将最终路径导航给机器人或自主车辆等智能体，让其按照路径行驶。 =============================== C++ map和multimap的键查找和值查找以及删除操作 map——key是唯一的。 multimap——key是不唯一的。 另外需要提及的一点是它们的删除操作，在删除某个迭代器的时候会导致迭代器失效。下面的代码主要介绍几个特殊的查找函数: find——已知key查找map或者multimap中的第一个满足条件的值。 find_if——已知起始迭代器，终止迭代器，bool表达式的第一个满足表达式的值。（该函数来自algorithm包） lower_bound——已知key，查找>=key的第一个迭代器 upper_bound——已知key，查找>key的第一个迭代器 equal_range——已知key，查找key的起始迭代器和终止迭代器 ==================================== class MorPlanToMap {//封装多条路径【储存像素】，包括起点，终点，路径点，路径长度 public: int plan_id;;//当前路径id std::vector next_plan_ids;//当前路径行走结束后，可以到达的路径id MorPlanToMap() {} MorPlanToMap(int &plan_id, std::vector &next_plan_ids) { this->plan_id =plan_id;//路径唯一id this->next_plan_ids = next_plan_ids;//当前路径行走结束后，可以到达的路径id } }; map execute_map_plan_;//需要执行的多个路径 void init_execute_map_plan() {//初始化数据 std::vector next_plan_ids; next_plan_ids.push_back(1); next_plan_ids.push_back(2); next_plan_ids.push_back(3); MorPlanToMap morPlanToMap(0, next_plan_ids);//第0条路径可以到达1,2,3,的路径上 execute_map_plan_.insert(std::pair(0, morPlanToMap)); next_plan_ids.clear(); next_plan_ids.push_back(0); next_plan_ids.push_back(4); next_plan_ids.push_back(5); MorPlanToMap morPlanToMap1(1, next_plan_ids);//第1条路径可以到达0,2,3,4,5,6的路径上 execute_map_plan_.insert(std::pair(1, morPlanToMap1)); next_plan_ids.clear(); next_plan_ids.push_back(3); MorPlanToMap morPlanToMap2(2, next_plan_ids);//第2条路径可以到达1,0,7,8,9的路径上 execute_map_plan_.insert(std::pair(2, morPlanToMap2)); next_plan_ids.clear(); next_plan_ids.push_back(4); next_plan_ids.push_back(5); MorPlanToMap morPlanToMap3(3, next_plan_ids); execute_map_plan_.insert(std::pair(3, morPlanToMap3)); next_plan_ids.clear(); next_plan_ids.push_back(0); next_plan_ids.push_back(3); MorPlanToMap morPlanToMap4(4, next_plan_ids); execute_map_plan_.insert(std::pair(4, morPlanToMap4)); next_plan_ids.clear(); next_plan_ids.push_back(0); next_plan_ids.push_back(1); next_plan_ids.push_back(4); MorPlanToMap morPlanToMap5(5, next_plan_ids); execute_map_plan_.insert(std::pair(5, morPlanToMap5)); } 起始点 id = 0； 目标点 id = 5； 现在需要 从 execute_map_plan_中找出所有的路径 ，起始点 id 到 目标点 id 的所有路径， 每条路径都按查找顺序依次放在一个集合中，然后输出路径长度最短的路径。 c++代码实现： 起始点 id = 0，目标点 id = 5，execute_map_plan_中找出所有能到达的路径 ，那么输出的路径 例如：0->1->5，0->2->3->4->5，这些路径都是起始点 id = 0，目标点 id = 5的路径，但是路径长度最短的路径为：0->5，所以输出的路径为：0->5。 例如：起始点 id = 0，目标点 id = 5，execute_map_plan_中找出所有能到达的路径 ，那么输出的路径为：0->1->5，0->2->3->4->5，0->2->3->5，0->3->4->5，0->3->5，0->4->5，0->5，这些路径都是起始点 id = 0，目标点 id = 5的路径，但是路径长度最短的路径为：0->5，所以输出的路径为：0->5。 起始点 id = 0，目标点 id = 5，execute_map_plan_中找出所有能到达的路径 ，那么输出的路径为：0->1->5 例如：起始点 id = 0，目标点 id = 5，execute_map_plan_中找出所有能到达的路径 ，那么输出的路径为：0->1->5，0->2->3->4->5，0->2->3->5，0->3->4->5，0->3->5，0->4->5，0->5，这些路径都是起始点 id = 0，目标点 id = 5的路径，但是路径长度最短的路径为：0->5，所以输出的路径为：0->5。 ================================