From c8cd3c29f5a7ae2f78ade46faadd541d1fd5f8ea Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Sun, 12 Jul 2026 19:44:23 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=96=B0=E7=9A=84pl?= =?UTF-8?q?anner=EF=BC=8C=E6=9C=AA=E5=AE=9E=E9=99=85=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=E8=BF=87=EF=BC=8C=E9=9C=80=E8=BF=9B=E8=A1=8C=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/obs.zip | Bin 0 -> 7377 bytes src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md | 500 ++++++++++++++ src/planner/CMakeLists.txt | 68 +- src/planner/README.md | 116 ++++ src/planner/config/grid_astar_theta.yaml | 64 ++ src/planner/config/planner.yaml | 631 +++++++----------- src/planner/config/planner.yaml.bak- | 232 ------- .../config/planner.yaml.bak-20260623-160129 | 232 ------- src/planner/config/pure_pursuit_tracker.yaml | 31 + src/planner/config/topology_graph.yaml | 38 ++ src/planner/config/topology_pure_pursuit.yaml | 44 ++ src/planner/launch/grid_astar_theta.launch.py | 43 ++ .../grid_astar_theta_with_tracker.launch.py | 59 ++ src/planner/launch/planner.launch.py | 154 ++--- .../launch/pure_pursuit_tracker.launch.py | 45 ++ .../launch/topology_pure_pursuit.launch.py | 58 ++ src/planner/package.xml | 38 +- src/planner/scripts/grid_astar_theta_node.py | 475 +++++++++++++ .../scripts/topology_pure_pursuit_node.py | 629 +++++++++++++++++ src/planner/src/grid_astar_theta_node.cpp | 11 + src/planner/src/planner_version.cpp | 22 +- 21 files changed, 2505 insertions(+), 985 deletions(-) create mode 100644 src/obs.zip create mode 100644 src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md create mode 100644 src/planner/README.md create mode 100644 src/planner/config/grid_astar_theta.yaml delete mode 100644 src/planner/config/planner.yaml.bak- delete mode 100644 src/planner/config/planner.yaml.bak-20260623-160129 create mode 100644 src/planner/config/pure_pursuit_tracker.yaml create mode 100644 src/planner/config/topology_graph.yaml create mode 100644 src/planner/config/topology_pure_pursuit.yaml create mode 100644 src/planner/launch/grid_astar_theta.launch.py create mode 100644 src/planner/launch/grid_astar_theta_with_tracker.launch.py create mode 100644 src/planner/launch/pure_pursuit_tracker.launch.py create mode 100644 src/planner/launch/topology_pure_pursuit.launch.py create mode 100644 src/planner/scripts/grid_astar_theta_node.py create mode 100644 src/planner/scripts/topology_pure_pursuit_node.py create mode 100644 src/planner/src/grid_astar_theta_node.cpp diff --git a/src/obs.zip b/src/obs.zip new file mode 100644 index 0000000000000000000000000000000000000000..c03beeee0eeaded3293b802a456b853abf2018dd GIT binary patch literal 7377 zcmb7J1z42Z)*fJn2I-dW21%t6Bppg5g`p&d5Rh)^kdQ8s4r!1aQfWb?5s;Mb6a@a^ zc>bfugXi8mGy9u(o_XK3zP0w+Yrk7X5ebM6xO&(-Zs`5-;m->$fC6A^%CpJQ6d2Rh=DlJm(rQCjLYwhtjSiqR$_*H;n?GIux-(# zR1vF}?v2qn%GWF@)U2)BDFf<5FX2yY*&przSNh$pnFIG^2BExRZ{x$>7urEBpZykz zySHRD>l?2X3Lwv6u&Q(U!f?`vyqUPd5}kUX!yqh^;mjhdk!ii0NmONxS!8YTSUvR{ zALm6ywQ5DGX;gJa)>$3%1BR7e?W2eX6-IahjYSmg8r)O-Ut%L_z>_GxESoLRTEO~C z*v!(z4KhM@EZbFb%f=S(x~`*V!kv-tPPq93n<;2{U|AMQL(H!f?W5Y-bM+LEb#c-b z*3renlUHRC%VqVHWc!~m`YzY9AS{m!7q|Ii*^QB!h+;X_)Yr??(!S9h#b4DgM^Vwy zNCPqsh9ZurUbYLl=XCQcb7{xO3Kk_+2z4je`U;*{M6x2FFiD1K3k^Rm8|!bl$8>0b zl7lLIiu4(UEyJTT8d8%$1RZ-5A?$dB`(!>bJW!sx{+ju+{KN1nWJC zqY&~$KeAllr<)Vb>SX%FLl$|KB%}Q!taTyUDJo!*yN#$FLCFmwLJcih1N;(36r)qdb}+@Ql?Kk)i*!*qY3VspYY0wnFHATm&10p=s$A;n0KWQG&+ z+JJC8N9sd>R5?*@s}P<@ni57z(#NV651|y^R#EpAKUWKSbS?R{ebn(AtFa6`ha@Zx zRYy>3Yxfi=to}yKo#%IZJb3X2V`rsGIzgOkl@`OH#4ww@%8|{cfDC6+<&VXS)1-v# zG-a*=1hMXSAyK$sb;{=<4s7GMlT;+nomc&#QRis*5XV86k-led-C&MHDA2J;vvwbT z`CVrp_SJKtM+A&4(LxHRJy17T=U(yp4FS1SMyx1a8uY{<7PM|k>cLp|5|oC|0p-3( zcE$eUruVZ3QOw0?@5?!L-+xBUOS5|SE_hhSh$z>iN00F?ed^&ErirpTUIueFb_gAH zVicd0CfZ0Qv{+<%=Vm!V8B*#lEM*yHM%@n(kC5MBOwA|v+a6`O- ze&fY^Wg8NWGj%mu2OqziX?vBUQ_(;LKp2G~1YjfFC_jmnxcX*tkt|8KJy7U z99vUg0Xe!pNk_E29x6%FU8%eOJ~e$!Gw&|(7m=vSAjD&pdCJf!q$I0WKo@5s zM*b%3xT_|c*@btW229$=|A@2TJd-?FW^q7-?-=L)M-WymxP@s)TT;X?=N4|v{ zHu2TzZ2M)+qp+95Vgp{X4+Xf{YqPX*y5ttLuxA;z)D-gTfRD}$vA!tKSQ;9ax>-b} zl2OUp31w$YG5K{o+AF%9|t2Mazzm9PM@rYZ+5Ij@fo%3{R#g{OsHc%=v|c?GpJ&rK7}vxp*nen3K+ ze%Ng)oxa3Ya3`8lO1x%XoA*i3@EzxL`I-Bl9w#SaGBE23GAyq&o$XW>IcJzW_g$Kj zBb#^*YOt$|ysxBshef*g|&8*^z4eVc`v;{4tpUeES;3r4VqT&-i@ciBdn$Jes} zYg%5%#4_^K7<*aZh`hSj8hvIX;cIRoAiVqp)1jN-V+OBq57L(+?no4NaLJM79v*BV zvk%>`!U1Kqu!y`+kw#&wf=pIXAez*YlGd6D5>x-^O`6d}K(bPLiD7n2(2)(eHRR#Q zOZN&g8K{j8C#KpSX(duruH}eCc46rf{o+6&T@(v4YiHBVJ-ZB$JTJb=IX$@AxYtT^ zXDVrtmF5BMa=BU%FdR7o@o|9`L8^!_KPXTLsr!?sZ>rswFbDN}2rH7>bkVKpPy4t> zLt`9z;u*3H1>B4Trf!4m51DG(c)6-Gp~^xv5uO(D6UxM65kVDM>iTX zE9E9ylclT*61P_Z1?!#ig+um4j4nANjO|CcQi&!WgSc3WS#Q{a{X!o>Y4CUV9XC_n`Ma-Oa zLla&6ZqV!z_ZzEeiK?o6Tps1NBff#-b|VaJT|@AexIL}v4RZx=oV0OIpsP?2g=JU- znJsWx^XP=;jd3hsJ;hEgbLxJ)wa8R+gSW>9<#h9T3u)yrR^~1VCZI6>@b*hoIxSk0 zn=q{%jPb|dk>b)19Exs9F2NJ6R$9xn^I|N)-FN$z*4oT&`OH?M>_@7X`?iWZueBKE z$=DRdbX7^cqGN)s5jP~Jz&FQNWiOl7)0;iEL_;z}pa$e-(=xMCA=_#e z5mvXUKsV>Sui;dzELAhE)X$ipvGK`TFf&@SeW^Qri&fj1;DLWzj!a#SwNokg{jW*# z&L8k&$;YuaEHS3%-n^(XGP1JIshA3j=@_=HjB2u~n@j7cu82QzW3#GsplsBTb!Xp{e&;_Ux+e=l!BUPp|5cS zmaE zp$jmkt)X3jNv^*ZVGwjPdJaXII85U%l-IC-EcpMQS60->2}^usRK!PYjyEdL!q%(lp@{BofTBQufs`vU|yQ)K~T#zdE)N$3mtnP_$eeUCb z53X}KCHsiSl9@j9M(JkX=3MSlNm=t#n(eZbySL=crJh%r81Ne?R_MyzRE|;0%ag*a zcMGe;USQ}%WY!3mz$NXcRz>9MEl1<^8#fc!2?kFfs}>H}vlSH}2kC2atC!vph-|Y$jMg!`Qb;{CZv`x3zRf># zq_$+x{2GLrJ$aXYXERNQ*vJB)7V;*hDbwz3Wox?qnZ5Jtd}0{S<5oYw)@!7%;U z@|e7ct%)pk%dn|}>K872r5gFYW!alWgxSg%77ie_CRksi0y#UhU@I4-NrKVQ=C+O~ z#9b=l_AQS{bH(Yx*-3Nd`DnELf-BM4%G4^~!=d+g3X`?ytWM_7c>hvEUAzsi%K)h=K8r2yKm;fJT~ zFhSirgLTMlR)n<&Rrb|xLqQ1y!JL+7*8xQG<8GsXg16i=>yQAy%mIHnL))h0lpEDi5 zy-$8F05j^>13PL<59YqeD}*8ye@VZJ{G0Yk1yJdpze;Ky$Giu&MiKY#&F%MKrm|A|`C-X7xmpZM80`2sz%X2%qPWfoJBYCr zMzbjUZ7IA~#4i*oFlQ?(ZR*X^+c10-9m^bN`ri2Kd>ws z{q0x8T#&dLil}6^H@J5>Jhd@4{~u;tdz*SG)e?k0J2jcwB#A)aHR~}*s{zC`~ zh#s(c#*Nvr?TVgz6HwsSZu6pcQ)m-Z$jaL)Az&9}oV#~~;XV5C>7++6!SeCf%CycP zkAu{Nc>C-D^Th0@2icZRp2Mwjxe-utq4<@jb+8OUC`$SBqbRfGTigzl-WW4_0c=R3 zG3JVd>4$4kScYO5tr$}oD;#;A7~%C5;j$E9OX<{y3J~YSbY*6_V>RqzZL#rf&~kS4nHz!8%BQYBiUboU^Kg$Fx~y z#m$&6=ncXYm#6s}5mXcr5Gj#`r}WEFP!rlwfE;x{Ie(c*&PquYdF7m2yp1DW&B^<`DcIdvpc=Ng9iVt zTlnv2{@x4xOe2f;8xQ`U=BLi!zvKG*lK+FN%D4AAuD>+^|Hkk=y?%`c3E<~=tiu!K z*Kl+FwS@c|`u7aGLjU^$^27d3Us1x}qyNhK^K$ZUy#I|?wM5tO{#8r=4gI@U|E#1x lyvp`PC;mP9-|8xy=$e|Uq6k91sz{jdXDtu_K>7pw{{VmV5$gZ| literal 0 HcmV?d00001 diff --git a/src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md b/src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md new file mode 100644 index 0000000..9e943a3 --- /dev/null +++ b/src/planner/ACKERMANN_HYBRID_ASTAR_PLAN.md @@ -0,0 +1,500 @@ +# 轻量阿克曼 Hybrid A* 路径规划方案说明 + +## 1. 方案目标 + +本方案用于阿克曼底盘小车的轻量路径规划，重点满足以下要求： + +- 考虑小车最小转弯半径。 +- 支持前进、倒车和转向。 +- 支持运动过程中动态添加锥桶障碍物。 +- 支持锥桶障碍物累计保留和去重。 +- 使用融合后的定位和代价地图，不直接处理雷达、相机、IMU、轮式里程计原始数据。 +- 输出仍保持 ROS 标准 `/plan nav_msgs/Path`，避免引入复杂自定义消息。 + +当前实现是在 `planner` 包已有代码基础上升级完成的，核心规划器仍使用原可执行文件名 `grid_astar_theta_node`，但内部已经从二维 A*/Theta* 改为轻量阿克曼 Hybrid A*/State Lattice。 + +## 2. 总体架构 + +系统由两个主要节点组成： + +| 节点 | 文件 | 作用 | +|---|---|---| +| `grid_astar_theta_planner` | `src/planner/src/grid_astar_theta_node.cpp` | 轻量 Hybrid A* 路径规划，融合 costmap 和锥桶障碍物，输出 `/plan` | +| `topology_pure_pursuit` | `src/planner/scripts/topology_pure_pursuit_node.py` | 跟踪 `/plan`，根据路径 yaw 判断前进或倒车，输出 `/cmd_vel` | + +典型数据流： + +```text +/odom +/goal_pose +/global_costmap/costmap +/random_obstacles + | + v +grid_astar_theta_planner + | + v +/plan + | + v +topology_pure_pursuit + | + v +/cmd_vel + | + v +cmd_vel_to_ackermann_drive.py + | + v +/ackermann_cmd +``` + +## 3. 输入与输出 + +### 3.1 规划器输入 + +| Topic | 类型 | 说明 | +|---|---|---| +| `/odom` | `nav_msgs/Odometry` | 小车当前融合定位，至少需要 `x, y, yaw` | +| `/goal_pose` | `geometry_msgs/PoseStamped` | 目标位姿，必须包含 `x, y, yaw` | +| `/global_costmap/costmap` | `nav_msgs/OccupancyGrid` | 全局代价地图 | +| `/random_obstacles` | `std_msgs/Float32MultiArray` | 锥桶障碍物，格式 `[x, y, radius, ...]` | +| `/clear_random_obstacles` | `std_msgs/Bool` | 清空已累计锥桶 | + +所有坐标默认工作在 `map` 坐标系下。如果 `/odom` 或 `/goal_pose` 的 `frame_id` 不是 `map`，则必须存在 TF 变换。 + +### 3.2 规划器输出 + +| Topic | 类型 | 说明 | +|---|---|---| +| `/plan` | `nav_msgs/Path` | 规划结果 | +| `/ackermann_lattice_status` | `std_msgs/String` | 规划状态和失败原因 | + +`/plan` 的每个 Pose 有特殊语义： + +- `pose.position.x/y` 表示轨迹点位置。 +- `pose.orientation.yaw` 表示小车车身朝向。 +- 如果路径点前进方向与车身 yaw 相反，则该段表示倒车。 + +## 4. 代价地图处理 + +规划器订阅 `/global_costmap/costmap`，类型为 `nav_msgs/OccupancyGrid`。 + +默认规则： + +| costmap 值 | 解释 | +|---|---| +| `>= 50` | 障碍物 | +| `-1` | 未知区域，默认视为障碍 | +| `< 50` 且非 `-1` | 可通行 | + +相关参数： + +```yaml +costmap_topic: /global_costmap/costmap +costmap_occupied_threshold: 50 +unknown_is_obstacle: true +outside_costmap_is_obstacle: true +``` + +当前规划器内部搜索分辨率默认是 `0.10m`。如果收到 costmap，且 `fit_map_to_costmap: true`，规划器会用 costmap 的物理边界更新规划范围。 + +需要注意： + +- costmap 的实际像素宽高以运行时 `OccupancyGrid.info.width/height` 为准。 +- 静态地图 `my_map.pgm` 是 `199 x 266` 像素，分辨率 `0.05m/像素`。 +- 静态地图 `zhihui.pgm` 是 `109 x 115` 像素，分辨率 `0.05m/像素`。 + +运行时查看 costmap 信息： + +```bash +ros2 topic echo --once /global_costmap/costmap --field info +``` + +## 5. 锥桶障碍物处理 + +锥桶输入 topic： + +```text +/random_obstacles +``` + +消息类型： + +```text +std_msgs/Float32MultiArray +``` + +数据格式： + +```text +[x, y, radius, x, y, radius, ...] +``` + +示例： + +```bash +ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \ +"{data: [1.2, 0.6, 0.15]}" +``` + +### 5.1 增量添加 + +当前默认开启： + +```yaml +accumulate_obstacles: true +``` + +这表示每次发布新锥桶后，规划器不会清空旧锥桶，而是将新锥桶合并到已有锥桶集合中。 + +例如先发布： + +```bash +ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \ +"{data: [1.2, 0.6, 0.15]}" +``` + +再发布： + +```bash +ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \ +"{data: [2.0, 1.1, 0.15]}" +``` + +规划器内部会保留两个锥桶。 + +### 5.2 去重规则 + +默认参数： + +```yaml +obstacle_merge_distance: 0.20 +``` + +如果新锥桶中心与已有锥桶中心距离小于 `0.20m`，认为是同一个锥桶： + +- 更新锥桶中心位置。 +- 半径取新旧较大值。 + +### 5.3 清空锥桶 + +如果误检或需要重新开始累计，可以发布： + +```bash +ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}" +``` + +## 6. 障碍物膨胀 + +锥桶障碍物不会只按原始半径参与碰撞检测，而是会进行安全膨胀。 + +膨胀半径计算： + +```text +inflated_radius = + max(obstacle_radius, unknown_obstacle_radius) + + robot_radius + + localization_error + + latency_margin + + safety_margin +``` + +默认参数： + +```yaml +default_obstacle_radius: 0.15 +unknown_obstacle_radius: 0.15 +robot_radius: 0.14 +localization_error: 0.10 +latency_margin: 0.05 +safety_margin: 0.05 +``` + +按默认值，一个半径 `0.15m` 的锥桶最终膨胀半径约为： + +```text +0.15 + 0.14 + 0.10 + 0.05 + 0.05 = 0.49m +``` + +这个值偏保守，适合定位误差较大或控制延迟明显的情况。如果实际通道较窄，可以降低 `localization_error` 或 `safety_margin`。 + +## 7. Hybrid A* 搜索模型 + +规划器状态为： + +```text +x, y, yaw, gear +``` + +其中： + +- `x, y` 是位置。 +- `yaw` 是车身朝向。 +- `gear` 是挡位方向，包含前进和倒车。 + +### 7.1 状态离散 + +默认参数： + +```yaml +resolution: 0.10 +yaw_bins: 72 +``` + +含义： + +- 平面位置按 `0.10m` 分辨率离散。 +- yaw 被离散成 72 份，即每份约 5 度。 + +### 7.2 运动原语 + +每次扩展使用 6 类动作： + +| gear | steering | 含义 | +|---|---|---| +| 前进 | 直行 | 向前直行 | +| 前进 | 左转 | 向前左转 | +| 前进 | 右转 | 向前右转 | +| 倒车 | 直行 | 向后直行 | +| 倒车 | 左转 | 向后左转 | +| 倒车 | 右转 | 向后右转 | + +默认运动原语长度： + +```yaml +primitive_length: 0.12 +``` + +碰撞检测采样间隔： + +```yaml +collision_sample_step: 0.03 +``` + +也就是说每条原语会被分成多个点做碰撞检测，而不是只检测终点。 + +### 7.3 最小转弯半径 + +默认： + +```yaml +min_turning_radius: 0.40 +``` + +转弯原语按该半径积分生成，规划阶段会保证轨迹曲率不小于这个半径限制。 + +## 8. 目标判定 + +目标输入为 `/goal_pose`，必须包含 `x, y, yaw`。 + +默认到达条件： + +```yaml +goal_xy_tolerance: 0.20 +goal_yaw_tolerance: 0.174533 +``` + +含义： + +- 位置误差小于 `0.20m`。 +- 朝向误差小于约 10 度。 + +如果目标只给 `x, y` 而 yaw 没有意义，那么最终车头方向可能不符合预期。因此对于阿克曼小车，建议目标始终提供合理 yaw。 + +## 9. 代价函数 + +搜索过程中使用以下代价项： + +```yaml +reverse_penalty: 1.3 +turn_penalty: 0.05 +gear_change_penalty: 0.20 +steering_change_penalty: 0.02 +``` + +含义： + +| 参数 | 作用 | +|---|---| +| `reverse_penalty` | 倒车惩罚，越大越不愿意倒车 | +| `turn_penalty` | 转弯惩罚，减少不必要转向 | +| `gear_change_penalty` | 前进/倒车切换惩罚，减少频繁换挡 | +| `steering_change_penalty` | 转向变化惩罚，让路径更稳定 | + +当前默认允许倒车，但不会无理由倒车。只有倒车明显降低路径代价或满足空间约束时，才更容易出现倒车段。 + +## 10. 重规划机制 + +重规划触发条件： + +- 收到新的 `/goal_pose`。 +- 收到新的 `/global_costmap/costmap`。 +- 收到新的 `/random_obstacles`。 +- 收到清空锥桶命令。 + +新目标会立即规划。 + +障碍物和 costmap 更新会通过定时器限频： + +```yaml +replan_rate: 2.0 +``` + +表示最多 2Hz 重规划，避免障碍物高频更新导致 CPU 被规划器占满。 + +单次规划时间预算： + +```yaml +planning_timeout_ms: 100.0 +``` + +如果超过该时间还没有找到路径，则本次规划失败。 + +失败策略： + +- 如果旧路径仍然没有碰撞，则继续使用旧路径。 +- 如果旧路径已被障碍物挡住，则发布空 `/plan`，跟踪器停车。 + +## 11. 跟踪器与倒车执行 + +跟踪器读取 `/plan`，并根据路径点 yaw 判断当前段是前进还是倒车。 + +判断逻辑： + +```text +如果 路径段方向 与 车身 yaw 方向相反，则认为是倒车段 +``` + +默认速度： + +```yaml +linear_speed: 0.30 +reverse_speed: 0.12 +``` + +前进段： + +```text +/cmd_vel.linear.x > 0 +``` + +倒车段： + +```text +/cmd_vel.linear.x < 0 +``` + +之后由现有 `cmd_vel_to_ackermann_drive.py` 桥接到 `/ackermann_cmd`。 + +## 12. 启动方式 + +构建： + +```bash +colcon build --packages-select planner +source install/setup.bash +``` + +启动规划器和跟踪器： + +```bash +ros2 launch planner grid_astar_theta_with_tracker.launch.py +``` + +发布目标： + +```bash +ros2 topic pub -1 /goal_pose geometry_msgs/msg/PoseStamped \ +"{header: {frame_id: map}, pose: {position: {x: 3.0, y: 1.5, z: 0.0}, orientation: {w: 1.0}}}" +``` + +发布锥桶： + +```bash +ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \ +"{data: [1.2, 0.6, 0.15]}" +``` + +清空锥桶： + +```bash +ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}" +``` + +查看规划输出： + +```bash +ros2 topic echo /plan +``` + +查看状态： + +```bash +ros2 topic echo /ackermann_lattice_status +``` + +## 13. 当前方案优点 + +- 轻量，主要逻辑是 C++ 实现。 +- 不依赖高频雷达融合或复杂局部规划器。 +- 规划阶段考虑阿克曼最小转弯半径。 +- 支持倒车和换挡。 +- 支持运动过程中动态添加锥桶。 +- 支持锥桶累计、去重和清空。 +- 仍使用标准 `/plan nav_msgs/Path` 输出，接口简单。 + +## 14. 当前限制 + +- `/plan` 本身没有显式 gear 字段，倒车语义依赖 Pose yaw 约定。 +- 目前没有单独实现局部地图或局部避障器。 +- costmap 的动态障碍处理依赖上游是否正确发布 `/global_costmap/costmap`。 +- Hybrid A* 是轻量版本，不包含复杂解析扩展或高级平滑器。 +- 如果地图非常大，`100ms` 超时可能导致复杂场景找不到路径，需要调整分辨率、搜索范围或超时时间。 + +## 15. 推荐调参顺序 + +建议按以下顺序调参： + +1. `min_turning_radius` +2. `robot_radius` +3. `localization_error` +4. `safety_margin` +5. `obstacle_merge_distance` +6. `primitive_length` +7. `planning_timeout_ms` +8. `reverse_penalty` +9. `lookahead_distance` +10. `reverse_speed` + +如果小车过于保守、容易找不到路，优先降低： + +```yaml +localization_error +safety_margin +unknown_is_obstacle +``` + +如果小车离障碍物太近，优先增加： + +```yaml +robot_radius +safety_margin +localization_error +``` + +如果规划太慢，优先调整： + +```yaml +resolution +planning_timeout_ms +map_min_x/map_max_x/map_min_y/map_max_y +``` + +## 16. 总结 + +这套方案本质上是一个面向低算力阿克曼小车的轻量 Hybrid A* 规划框架。它不处理底层传感器融合，只消费已经融合好的定位、代价地图和锥桶全局坐标。 + +相比普通二维 A*，它在规划阶段加入了车身朝向、最小转弯半径、前进/倒车和换挡代价，因此输出路径更符合阿克曼底盘运动约束。 + +相比完整 Nav2 Hybrid A* 或 MPPI，它更轻量、接口更简单、可控性更强，适合竞赛场景中地图规模较小、障碍物数量有限、需要快速重规划的任务。 diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index 443d085..eec7524 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -1,20 +1,45 @@ -cmake_minimum_required(VERSION 3.5) -project(planner) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) -endif() - +cmake_minimum_required(VERSION 3.5) +project(planner) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) # Dummy executable to satisfy ament_cmake + install of launch/config add_executable(planner_version src/planner_version.cpp) +add_executable(grid_astar_theta_node src/grid_astar_theta_node.cpp) +ament_target_dependencies(grid_astar_theta_node + geometry_msgs + nav_msgs + rclcpp + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros +) -install(TARGETS planner_version +install(TARGETS + planner_version + grid_astar_theta_node + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/topology_pure_pursuit_node.py + scripts/grid_astar_theta_node.py DESTINATION lib/${PROJECT_NAME} ) @@ -23,11 +48,16 @@ install( DESTINATION share/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() +install(FILES + README.md + DESTINATION share/${PROJECT_NAME} +) -ament_package() +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/planner/README.md b/src/planner/README.md new file mode 100644 index 0000000..699747a --- /dev/null +++ b/src/planner/README.md @@ -0,0 +1,116 @@ +# Lightweight Planning + +This package keeps the existing Nav2 minimal planner launch, and adds a lighter +fixed-field option: + +1. Use a sparse topology graph for fixed venues. +2. Publish a densified `nav_msgs/Path`. +3. Track it with Pure Pursuit at a low, deterministic control cost. + +The node is intended for cases where localization noise and runtime delay make +high-batch MPPI difficult to tune on the robot. + +For random cone obstacles, the package includes a lightweight Ackermann Hybrid +A*/state-lattice planner implemented in C++. It consumes the fused robot pose, +target pose, OccupancyGrid costmap, and optional cone list, then publishes +`/plan` for the tracker. The executable name is kept as +`grid_astar_theta_node` for launch compatibility, but the planner is no longer +2D A*/Theta*. + +## Run + +```bash +colcon build --packages-select planner +source install/setup.bash +ros2 launch planner topology_pure_pursuit.launch.py +``` + +Send a graph-node target: + +```bash +ros2 topic pub -1 /topology_goal std_msgs/msg/String "{data: bed_b}" +``` + +Or send `/goal_pose` near a graph node; the nearest node is selected. + +## Random Obstacles / Ackermann Hybrid A* + +Run the grid planner and tracker together: + +```bash +ros2 launch planner grid_astar_theta_with_tracker.launch.py +``` + +Publish detected cone obstacles as a flat `Float32MultiArray`: + +```bash +ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \ + "{data: [1.2, 0.6, 0.15, 2.1, 1.4, 0.20]}" +``` + +By default, `/random_obstacles` is incremental. New cones are merged into the +stored cone set, and old cones remain. Detections within +`obstacle_merge_distance` are treated as the same cone and update its position. +Clear stored cones when needed: + +```bash +ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}" +``` + +Send a target: + +```bash +ros2 topic pub -1 /goal_pose geometry_msgs/msg/PoseStamped \ + "{header: {frame_id: map}, pose: {position: {x: 3.0, y: 1.5}, orientation: {w: 1.0}}}" +``` + +The planner also subscribes to `/global_costmap/costmap` as +`nav_msgs/OccupancyGrid`. Occupied cells (`>= 50`) and unknown cells (`-1`) are +blocked by default. + +The planner publishes `/plan` as `nav_msgs/Path`; each pose orientation is the +vehicle body yaw. If the segment direction is opposite that yaw, the tracker +treats that segment as reverse and publishes negative `/cmd_vel.linear.x`. + +## Files + +- `config/topology_graph.yaml`: fixed-field topology graph. Replace the sample + coordinates with measured centerline coordinates in the map frame. +- `config/topology_pure_pursuit.yaml`: tracking, delay compensation, and topic + parameters. +- `launch/topology_pure_pursuit.launch.py`: standalone launch for the topology + planner and Pure Pursuit tracker. +- `config/grid_astar_theta.yaml`: costmap fusion, cone inflation, and Ackermann + Hybrid A* search parameters. +- `launch/grid_astar_theta_with_tracker.launch.py`: random-obstacle Ackermann + planner plus yaw-aware tracker. +- `src/grid_astar_theta_node.cpp`: OccupancyGrid fusion, cone inflation, + Hybrid A* search, and `/plan` publishing. +- `scripts/grid_astar_theta_node.py`: Python reference version kept for quick + experiments; launch files use the C++ executable by default. +- `scripts/topology_pure_pursuit_node.py`: Dijkstra graph search, external + `/plan` tracking, yaw-aware reverse handling, TF-aware pose handling, and + latency compensation. + +## Operating Notes + +- Do not run this node's `/cmd_vel` output together with Nav2 + `controller_server`; choose one command publisher. +- If the graph is in `map` and odometry is in `odom`, keep TF available. The node + transforms odometry poses into `map_frame`. +- Increase `nearest_node_max_distance`, `goal_tolerance`, and graph clearance + when localization is noisy. +- Tune `lookahead_distance`, `min_turning_radius`, and `reverse_speed` first for + Ackermann corner and reverse behavior. +- If the base expects Ackermann commands, run the existing + `cmd_vel_to_ackermann_drive.py` bridge and configure the base driver + accordingly. + +## Planner Semantics + +- Minimum turning radius defaults to `0.40 m`. +- `/goal_pose` uses `x`, `y`, and `yaw`; yaw tolerance defaults to 10 degrees. +- Replanning is immediate on new goals and limited to 2 Hz for costmap or + accumulated cone updates. +- If new planning fails, the last collision-free path is kept; if it is no + longer safe, an empty `/plan` is published so the tracker stops. diff --git a/src/planner/config/grid_astar_theta.yaml b/src/planner/config/grid_astar_theta.yaml new file mode 100644 index 0000000..d8d32cc --- /dev/null +++ b/src/planner/config/grid_astar_theta.yaml @@ -0,0 +1,64 @@ +grid_astar_theta_planner: + ros__parameters: + use_sim_time: false + + # Inputs: + # - /odom gives the fused current robot pose. + # - /goal_pose gives the target pose (x, y, yaw). + # - /global_costmap/costmap is nav_msgs/OccupancyGrid. + # - /random_obstacles is std_msgs/Float32MultiArray. + # obstacle_format=xyr means [x, y, radius, x, y, radius, ...]. + map_frame: map + odom_topic: /odom + goal_pose_topic: /goal_pose + obstacles_topic: /random_obstacles + clear_obstacles_topic: /clear_random_obstacles + costmap_topic: /global_costmap/costmap + plan_topic: /plan + status_topic: /ackermann_lattice_status + + # Fallback map bounds in map_frame. When a costmap arrives in map frame, + # bounds are taken from that OccupancyGrid by default. + map_min_x: -5.0 + map_min_y: -5.0 + map_max_x: 5.0 + map_max_y: 5.0 + resolution: 0.10 + fit_map_to_costmap: true + + # OccupancyGrid fusion. Unknown is blocked by default for safety. + costmap_occupied_threshold: 50 + unknown_is_obstacle: true + outside_costmap_is_obstacle: true + + # Random circular cone obstacles. Inflation is: + # max(obstacle_radius, unknown_obstacle_radius) + # + robot_radius + localization_error + latency_margin + safety_margin. + obstacle_format: xyr + default_obstacle_radius: 0.15 + unknown_obstacle_radius: 0.15 + accumulate_obstacles: true + obstacle_merge_distance: 0.20 + robot_radius: 0.14 + localization_error: 0.10 + latency_margin: 0.05 + safety_margin: 0.05 + + # Lightweight Hybrid A*/state-lattice search. + min_turning_radius: 0.40 + yaw_bins: 72 + primitive_length: 0.12 + collision_sample_step: 0.03 + goal_xy_tolerance: 0.20 + goal_yaw_tolerance: 0.174533 + reverse_penalty: 1.3 + turn_penalty: 0.05 + gear_change_penalty: 0.20 + steering_change_penalty: 0.02 + plan_on_obstacle_update: true + max_iterations: 50000 + planning_timeout_ms: 100.0 + replan_rate: 2.0 + + max_odom_age: 0.80 + tf_timeout: 0.05 diff --git a/src/planner/config/planner.yaml b/src/planner/config/planner.yaml index 3bd2677..3b2b83b 100644 --- a/src/planner/config/planner.yaml +++ b/src/planner/config/planner.yaml @@ -1,399 +1,232 @@ -# ============================================================================ -# planner.yaml - Nav2 minimal global planning stack -# 节点: slam_toolbox + planner_server + global_costmap + local_costmap -# 用途: 启动建图、全局规划、全局代价地图和局部代价地图所需的 ROS2 参数。 -# 说明: 本文件只配置参数；实际由 launch 文件把这些参数加载到对应节点。 -# ============================================================================ - -# --------------------------------------------------------------------------- -# slam_toolbox - online_async 实时建图 -# 主要作用: -# 1. 订阅激光 /scan 和里程计 TF。 -# 2. 生成 /map 占用栅格地图。 -# 3. 发布 map -> odom 变换，使 map 坐标系和机器人 odom/base_link 连接起来。 -# --------------------------------------------------------------------------- -slam_toolbox: - ros__parameters: - # 是否使用仿真时间 /clock。真实车通常为 False；Gazebo/rosbag 回放通常为 True。 - use_sim_time: False - - # Solver - # 后端图优化求解器插件。CeresSolver 用 Ceres 做位姿图优化，是 slam_toolbox 常用选择。 - solver_plugin: solver_plugins::CeresSolver - # Ceres 线性求解器。SPARSE_NORMAL_CHOLESKY 适合稀疏 SLAM 图，速度和精度较均衡。 - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - # Ceres 预条件器。SCHUR_JACOBI 常用于 bundle/pose graph 类问题，加速迭代收敛。 - ceres_preconditioner: SCHUR_JACOBI - # 信赖域策略。LEVENBERG_MARQUARDT 稳定性好，适合非线性最小二乘优化。 - ceres_trust_strategy: LEVENBERG_MARQUARDT - # dogleg 策略类型；仅在 dogleg 信赖域策略启用时影响优化路径，这里保留默认传统形式。 - ceres_dogleg_type: TRADITIONAL_DOGLEG - # 鲁棒核函数。None 表示不额外压制离群约束；激光匹配质量差时可考虑鲁棒 loss。 - ceres_loss_function: None - - # ROS base - # 里程计坐标系名。slam_toolbox 需要 TF 中存在 odom -> base_link 或等价链路。 - odom_frame: odom - # 地图坐标系名。/map 消息和 map -> odom TF 都以这个坐标系作为全局参考。 - map_frame: map - # 机器人车体坐标系名。用于从 TF 读取机器人当前在 odom/map 下的位姿。 - base_frame: base_link - # 激光雷达话题。slam_toolbox 从这里取 LaserScan 做 scan matching 和建图。 - scan_topic: /scan - # SLAM 工作模式。mapping 表示在线建图；localization 表示基于已有地图定位。 - mode: mapping - # 是否启用地图保存相关服务/能力，便于后续保存当前构建出的地图。 - use_map_saver: true - - # Debug & performance - # 是否输出详细调试日志。排查匹配/闭环问题时可开；平时关闭减少日志量。 - debug_logging: false - # 激光降采样倍率。1 表示每帧都处理；增大可降负载但地图更新更慢。 - throttle_scans: 2 - # 发布 map -> odom TF 的周期，单位秒。0.02 表示约 50Hz，越小 TF 越实时但 CPU 占用更高。 - transform_publish_period: 0.02 - # /map 地图更新周期，单位秒。数值越小地图刷新越频繁，但计算/网络开销更高。 - map_update_interval: 1.0 - # 地图分辨率，单位 m/cell。0.05 表示每个栅格 5cm；越小越精细但地图更大。 - resolution: 0.05 - # 接受的最小激光距离，小于该距离的点会被过滤，避免近距离噪声/车体反射。 - min_laser_range: 0.15 - # 接受的最大激光距离，超过该距离的点会被过滤，避免远距离弱回波干扰。 - max_laser_range: 20.0 - # 两次处理激光之间的最小时间间隔，单位秒；用于限制处理频率。 - minimum_time_interval: 0.5 - # 查询 TF 的超时时间，单位秒；TF 延迟超过该值会导致当前帧无法处理。 - transform_timeout: 0.2 - # TF 缓冲时间长度，单位秒；越大越能容忍延迟数据，但内存占用增加。 - tf_buffer_duration: 10.0 - # slam_toolbox 内部线程栈大小；地图/图优化规模大时需要足够栈空间。 - stack_size_to_use: 40000000 - # 是否启用交互模式，允许通过 RViz/服务交互式调整图节点或执行手动操作。 - enable_interactive_mode: true - - # Mapping - # 是否使用 scan matching。开启后根据激光匹配修正里程计漂移，是建图核心能力。 - use_scan_matching: true - # 是否使用激光点云重心辅助匹配，可改善局部匹配初值稳定性。 - use_scan_barycenter: true - # 机器人至少移动多少米才添加/处理新的位姿节点；越小图更密，越大负载更低。 - minimum_travel_distance: 0.5 - # 机器人至少转动多少弧度才添加/处理新的位姿节点；越小对转弯更敏感。 - minimum_travel_heading: 0.1 - # scan buffer 中保留的扫描帧数量，用于局部匹配和链路建立。 - scan_buffer_size: 10 - # scan buffer 中扫描之间允许的最大距离，超过该距离的历史帧不再用于匹配。 - scan_buffer_maximum_scan_distance: 5.0 - # 精匹配响应最低阈值；低于该值的局部链路匹配会被认为不可靠。 - link_match_minimum_response_fine: 0.1 - # 建立相邻 scan 链路时允许的最大空间距离，超过则不尝试连接。 - link_scan_maximum_distance: 1.5 - - # Loop closure - # 是否启用闭环检测。开启后回到旧区域时可修正累计漂移。 - do_loop_closing: false - # 闭环候选链最小长度；较大可减少误闭环，较小更容易检测短回环。 - loop_match_minimum_chain_size: 10 - # 粗匹配允许的最大方差；越小越严格，误闭环少但可能漏闭环。 - loop_match_maximum_variance_coarse: 3.0 - # 闭环粗匹配最低响应阈值；候选低于该值会被丢弃。 - loop_match_minimum_response_coarse: 0.35 - # 闭环精匹配最低响应阈值；越高越保守，闭环质量更可靠。 - loop_match_minimum_response_fine: 0.45 - # 搜索闭环候选的最大距离，单位米；越大越容易找回环但计算量和误匹配风险增加。 - loop_search_maximum_distance: 3.0 - - # Scan matching - # 局部 scan matching 搜索窗口尺寸，单位米；越大越能容忍初值误差但更耗时。 - correlation_search_space_dimension: 0.5 - # 局部搜索分辨率，单位米；越小搜索更精细但计算更多。 - correlation_search_space_resolution: 0.01 - # 相关性搜索的 smear 标准差，用于让匹配响应更平滑，提升抗噪性。 - correlation_search_space_smear_deviation: 0.1 - # 闭环搜索窗口尺寸，单位米；通常比局部匹配更大以覆盖漂移。 - loop_search_space_dimension: 8.0 - # 闭环搜索分辨率，单位米；控制闭环候选搜索精度和计算量。 - loop_search_space_resolution: 0.05 - # 闭环搜索 smear 标准差，用于平滑闭环匹配响应。 - loop_search_space_smear_deviation: 0.03 - - # Matcher params - # 位移方差惩罚。越大越不愿接受与预测位移差异大的匹配结果。 - distance_variance_penalty: 0.5 - # 角度方差惩罚。越大越不愿接受与预测角度差异大的匹配结果。 - angle_variance_penalty: 1.0 - # 精搜索角度步进，单位弧度；越小角度搜索更细但更慢。 - fine_search_angle_offset: 0.00349 - # 粗搜索角度范围/步进相关参数，单位弧度；用于大范围初始角度搜索。 - coarse_search_angle_offset: 0.349 - # 粗搜索角分辨率，单位弧度；越小越精细但计算量更高。 - coarse_angle_resolution: 0.0349 - # 最小角度惩罚因子，限制角度惩罚不要过低，避免过度相信异常角度匹配。 - minimum_angle_penalty: 0.9 - # 最小距离惩罚因子，限制距离惩罚不要过低，避免过度相信异常位移匹配。 - minimum_distance_penalty: 0.5 - # 是否扩展匹配响应区域，有助于在响应较弱时找到可接受匹配。 - use_response_expansion: true - # 激光穿过栅格的最小次数阈值，用于空闲/占用证据更新。 - min_pass_through: 2 - # 占用概率阈值。超过该阈值的栅格更倾向标为障碍。 - occupancy_threshold: 0.1 - # 输入 scan 队列大小；处理跟不上时队列过小会丢帧，过大可能增加延迟。 - scan_queue_size: 20 - -# --------------------------------------------------------------------------- -# planner_server - SmacPlannerHybrid 全局路径规划 -# 主要作用: -# 1. 接收起点/终点和全局代价地图。 -# 2. 使用 Hybrid-A* / Reeds-Shepp 运动模型生成符合车体转弯约束的 /plan。 -# --------------------------------------------------------------------------- -planner_server: - ros__parameters: - # 规划插件列表。每个名字对应下方同名配置块；这里仅启用 GridBased。 - planner_plugins: - - GridBased - # 是否使用仿真时间。应与其它 Nav2 节点保持一致。 - use_sim_time: False - GridBased: - # 插件类型。SmacPlannerHybrid 适合非完整约束车辆，能考虑转弯半径和倒车。 - plugin: nav2_smac_planner/SmacPlannerHybrid - # 是否对 costmap 降采样后规划。False 表示用原始分辨率，路径更细但计算更多。 - downsample_costmap: False - # 降采样倍率；只有 downsample_costmap=True 时生效。 - downsampling_factor: 1 - # 允许规划终点距离目标的容差，单位米；目标附近不可达时可在容差内结束。 - tolerance: 0.25 - # 是否允许路径经过未知区域。True 可穿过未探索区域，False 更保守。 - allow_unknown: True - # 最大搜索迭代次数；越大越不容易提前失败，但规划耗时上限更高。 - max_iterations: 100000 - # 接近目标阶段的最大迭代次数，用于目标附近精细搜索。 - max_on_approach_iterations: 1000 - # 单次规划最大耗时，单位秒；超时后规划失败或返回当前可行结果。 - max_planning_time: 5.0 - # 搜索运动模型。REEDS_SHEPP 支持前进和倒车，适合可倒车小车。 - motion_model_for_search: REEDS_SHEPP - # 朝向离散桶数量。72 表示 5 度一个方向；越大方向更细但搜索量更大。 - angle_quantization_bins: 72 - # 解析扩展触发比例，用于尝试直接连接目标，提高接近目标时的效率。 - analytic_expansion_ratio: 2.0 - # 解析扩展最大长度，单位米；太长可能穿障碍，太短接近目标效率降低。 - analytic_expansion_max_length: 3.0 - # 最小转弯半径，单位米。应接近实车运动学能力；过小会生成车转不过的路径。 - minimum_turning_radius: 0.35 - # 倒车惩罚。大于 1 会减少倒车段；值越大越偏好前进。 - reverse_penalty: 1.3 - # 前进/倒车切换惩罚。增大后路径会减少换向次数。 - change_penalty: 0.0 - # 非直线运动惩罚。增大后更偏好直线路径，可能牺牲可达性。 - non_straight_penalty: 0.0 - # 代价地图代价惩罚。越大越远离障碍/高代价区域，但可能绕路更多。 - cost_penalty: 5.0 - # 回溯惩罚。用于影响搜索顺序，通常小正数可提升搜索效率。 - retrospective_penalty: 0.015 - # 运动启发查表范围，单位米；越大预计算更多，可能提升规划质量但占内存。 - lookup_table_size: 5.0 - # 是否缓存障碍启发。静态环境可开以加速重复规划；动态环境关闭更安全。 - cache_obstacle_heuristic: False - # 是否发布搜索扩展可视化。调试规划失败时可开，平时关闭减少开销。 - viz_expansions: False - # 是否对生成路径做平滑。True 通常让路径更适合控制器跟踪。 - smooth_path: True - smoother: - # 平滑器最大迭代次数；越大越可能收敛但耗时更长。 - max_iterations: 1000 - # 平滑权重。越大路径越平滑，但可能偏离原始安全路径。 - w_smooth: 0.4 - # 数据保持权重。越大越贴近原始规划路径，越不容易被平滑拉偏。 - w_data: 0.2 - # 平滑收敛阈值；变化小于该值时停止迭代。 - tolerance: 1.0e-10 - # 是否进行多轮细化平滑，改善最终路径质量。 - do_refinement: True - # 细化轮数；轮数越多路径越平滑但耗时增加。 - refinement_num: 4 - -planner_server_rclcpp_node: - ros__parameters: - # planner_server 辅助 rclcpp 节点的时间源设置，应与 planner_server 保持一致。 - use_sim_time: False - -# --------------------------------------------------------------------------- -# global_costmap - 全局代价地图 -# 主要作用: -# 1. 在 map 坐标系下融合静态地图和激光障碍。 -# 2. 给 planner_server 提供从起点到目标点的全局规划代价。 -# --------------------------------------------------------------------------- -global_costmap: - global_costmap: - ros__parameters: - # 是否使用仿真时间。应与 SLAM、planner_server 和其它 Nav2 节点一致。 - use_sim_time: False - # TF 允许的时间容差，单位秒；传感器/TF 稍有延迟时可避免频繁报错。 - transform_tolerance: 2.0 - # 代价地图更新频率，单位 Hz；越高越实时但 CPU 占用更高。 - update_frequency: 1.0 - # 代价地图发布频率，单位 Hz；影响 RViz/Foxglove 可视化刷新和下游接收频率。 - publish_frequency: 1.0 - # 全局代价地图所在坐标系。全局规划通常使用 map。 - global_frame: map - # 机器人基坐标系，用于从 TF 获取机器人在 costmap 中的位置。 - robot_base_frame: base_link - # 机器人二维足迹，多边形点按 base_link 坐标给出，单位米；用于碰撞检测。 - footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' - # 足迹外扩安全边距，单位米；越大规划离障碍越远但可通行空间变窄。 - footprint_padding: 0.02 - # 代价地图分辨率，单位 m/cell；应与地图精度和计算能力匹配。 - resolution: 0.05 - # 是否保留未知空间。True 时未知区域保持 unknown，配合 allow_unknown 决定能否规划穿过。 - track_unknown_space: True - # 启用的 costmap layer 顺序。静态层提供地图，障碍层叠加实时障碍，膨胀层扩展安全距离。 - plugins: - - static_layer - - obstacle_layer - - inflation_layer - obstacle_layer: - # 障碍层插件类型，负责把 LaserScan/PointCloud 转为障碍和清障信息。 - plugin: nav2_costmap_2d::ObstacleLayer - # 是否启用障碍层。False 时该层不参与代价地图。 - enabled: True - # 观测源名称列表；这里定义一个名为 scan 的传感器源。 - observation_sources: scan - scan: - # 激光雷达话题名。障碍层从这里读取 LaserScan。 - topic: /scan - # 处理障碍的最大高度，单位米；LaserScan 通常按 2D 使用，此参数保留兼容层配置。 - max_obstacle_height: 2.0 - # 是否用激光射线清除自由空间。True 会把射线经过区域标为空闲。 - clearing: True - # 是否用激光命中点标记障碍。True 会把终点附近标为障碍。 - marking: True - # 传感器数据类型。LaserScan 表示订阅 sensor_msgs/msg/LaserScan。 - data_type: LaserScan - # 清障射线最大距离，单位米；越大清除范围越远。 - raytrace_max_range: 3.0 - # 清障射线最小距离，单位米；小于该距离不参与清障。 - raytrace_min_range: 0.0 - # 标记障碍最大距离，单位米；超过该距离的障碍点不写入 costmap。 - obstacle_max_range: 2.5 - # 标记障碍最小距离，单位米；小于该距离的点不写入 costmap。 - obstacle_min_range: 0.0 - static_layer: - # 静态地图层插件类型，订阅 /map 并把占用栅格转成全局代价。 - plugin: nav2_costmap_2d::StaticLayer - # 是否用 transient local QoS 订阅地图。True 可收到已经发布过的 latched /map。 - map_subscribe_transient_local: True - inflation_layer: - # 膨胀层插件类型，把障碍周围一定范围扩展为逐渐降低的代价。 - plugin: nav2_costmap_2d::InflationLayer - # 膨胀代价衰减系数。越大衰减越快，障碍影响范围内高代价区域更窄。 - cost_scaling_factor: 3.0 - # 膨胀半径，单位米。障碍周围该半径内会增加代价。 - inflation_radius: 0.2 - # 是否每次发布完整 costmap。True 简化可视化/调试，但带宽占用更高。 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - # global_costmap lifecycle client 的时间源设置，应与 global_costmap 保持一致。 - use_sim_time: False - global_costmap_rclcpp_node: - ros__parameters: - # global_costmap 内部 rclcpp 节点的时间源设置，应与 global_costmap 保持一致。 - use_sim_time: False - -# --------------------------------------------------------------------------- -# local_costmap - 局部代价地图 -# 主要作用: -# 1. 在 odom 坐标系下围绕机器人滚动更新附近障碍。 -# 2. 给局部控制器/避障模块提供短距离实时环境代价。 -# --------------------------------------------------------------------------- -local_costmap: - local_costmap: - ros__parameters: - # 局部代价地图更新频率，单位 Hz；越高避障越实时但 CPU 占用更高。 - update_frequency: 1.0 - # 局部代价地图发布频率，单位 Hz；影响可视化和下游节点接收频率。 - publish_frequency: 2.0 - # TF 查询容差，单位秒；允许 odom/base_link TF 存在小幅延迟。 - transform_tolerance: 2.0 - # 局部地图所在坐标系。局部 costmap 通常用 odom，避免 map 闭环跳变影响局部控制。 - global_frame: odom - # 机器人基坐标系，用于把机器人足迹放到局部代价地图中。 - robot_base_frame: base_link - # 是否使用仿真时间。应与其它 Nav2 节点一致。 - use_sim_time: False - # 是否启用滚动窗口。True 表示地图窗口跟随机器人移动。 - rolling_window: True - # 局部窗口宽度，单位米；越大能看到更远障碍但计算更多。 - width: 3 - # 局部窗口高度，单位米；越大能看到更远障碍但计算更多。 - height: 3 - # 局部代价地图分辨率，单位 m/cell；越小越精细但计算更多。 - resolution: 0.05 - # 机器人二维足迹，单位米；用于局部碰撞检测。 - footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' - # 足迹外扩安全边距，单位米；越大越保守。 - footprint_padding: 0.02 - # 启用的局部 costmap layer。当前只启用 voxel_layer 和 inflation_layer。 - plugins: - - voxel_layer - - inflation_layer - inflation_layer: - # 局部膨胀层插件类型，负责给附近障碍周围增加安全代价。 - plugin: nav2_costmap_2d::InflationLayer - # 膨胀代价衰减系数。越大衰减越快，机器人更贴近障碍。 - cost_scaling_factor: 3.0 - # 膨胀半径，单位米；建议不小于机器人定位误差和控制误差。 - inflation_radius: 0.2 - voxel_layer: - # 体素层插件类型，可维护 3D 障碍体素并投影到 2D costmap。 - plugin: nav2_costmap_2d::VoxelLayer - # 是否启用体素层。False 时不会把 scan 写入该层。 - enabled: True - # 是否发布体素地图，便于 RViz/Foxglove 调试障碍高度栅格。 - publish_voxel_map: True - # 体素网格 z 方向起点，单位米；通常从地面或激光参考高度附近开始。 - origin_z: 0.0 - # 单个体素 z 方向分辨率，单位米。 - z_resolution: 0.05 - # z 方向体素层数；与 z_resolution 相乘决定可表达的高度范围。 - z_voxels: 16 - # 最大障碍高度，单位米；高于该值的数据不作为障碍处理。 - max_obstacle_height: 2.0 - # 标记障碍所需的最小体素命中阈值。0 表示非常敏感，容易标障碍。 - mark_threshold: 0 - # 观测源名称列表；这里使用 scan。 - observation_sources: scan - scan: - # 激光雷达话题名。体素层从这里读取 LaserScan。 - topic: /scan - # 该观测源接受的最大障碍高度，单位米。 - max_obstacle_height: 2.0 - # 是否用激光射线清除自由空间。 - clearing: True - # 是否用激光命中点标记障碍。 - marking: True - # 传感器数据类型。LaserScan 表示 2D 激光。 - data_type: LaserScan - # 清障射线最大距离，单位米。 - raytrace_max_range: 3.0 - # 清障射线最小距离，单位米。 - raytrace_min_range: 0.0 - # 标记障碍最大距离，单位米。 - obstacle_max_range: 2.5 - # 标记障碍最小距离，单位米。 - obstacle_min_range: 0.0 - static_layer: - # 注意: 当前 local_costmap.plugins 未包含 static_layer，因此这个配置块不会生效。 - # 若以后把 static_layer 加入 plugins，该参数表示用 transient local QoS 订阅 /map。 - map_subscribe_transient_local: True - # 是否每次发布完整局部 costmap。True 便于可视化，但占用更多带宽。 - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - # local_costmap lifecycle client 的时间源设置，应与 local_costmap 保持一致。 - use_sim_time: False - local_costmap_rclcpp_node: - ros__parameters: - # local_costmap 内部 rclcpp 节点的时间源设置，应与 local_costmap 保持一致。 - use_sim_time: False +# ============================================================================ +# planner.yaml — Nav2 minimal global planning stack +# 节点: slam_toolbox + planner_server + global_costmap + local_costmap +# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁 +# ============================================================================ + +# --------------------------------------------------------------------------- +# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform +# 来源: slam_toolbox_mapping.yaml +# --------------------------------------------------------------------------- +slam_toolbox: + ros__parameters: + use_sim_time: False + + # Solver + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS base + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: mapping + use_map_saver: true + + # Debug & performance + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 + map_update_interval: 3.0 + resolution: 0.05 + min_laser_range: 0.15 + max_laser_range: 20.0 + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 + enable_interactive_mode: true + + # Mapping + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + + # Loop closure + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + loop_search_maximum_distance: 3.0 + + # Scan matching + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Matcher params + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 + scan_queue_size: 20 + +# --------------------------------------------------------------------------- +# planner_server — SmacPlannerHybrid 全局路径规划 → /plan +# 来源: gc_navigation_slam.yaml planner_server section +# --------------------------------------------------------------------------- +planner_server: + ros__parameters: + planner_plugins: + - GridBased + use_sim_time: False + GridBased: + plugin: nav2_smac_planner/SmacPlannerHybrid + downsample_costmap: False + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: True + max_iterations: 100000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: REEDS_SHEPP + angle_quantization_bins: 72 + analytic_expansion_ratio: 2.0 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.35 + reverse_penalty: 1.3 + change_penalty: 0.0 + non_straight_penalty: 0.0 + cost_penalty: 5.0 + retrospective_penalty: 0.015 + lookup_table_size: 5.0 + cache_obstacle_heuristic: False + viz_expansions: False + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.4 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: True + refinement_num: 4 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +# --------------------------------------------------------------------------- +# global_costmap — 全局代价地图（订阅 /scan + /map） +# 来源: gc_navigation_slam.yaml global_costmap section +# --------------------------------------------------------------------------- +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: False + transform_tolerance: 2.0 + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' + footprint_padding: 0.02 + resolution: 0.05 + track_unknown_space: True + plugins: + - static_layer + - obstacle_layer + - inflation_layer + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: nav2_costmap_2d::StaticLayer + map_subscribe_transient_local: True + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + cost_scaling_factor: 3.0 + inflation_radius: 0.2 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +# --------------------------------------------------------------------------- +# local_costmap — 局部代价地图（obstacle_detector 可注入） +# 来源: gc_navigation_slam.yaml local_costmap section +# --------------------------------------------------------------------------- +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 2.0 + transform_tolerance: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: False + rolling_window: True + width: 3 + height: 3 + resolution: 0.05 + footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' + footprint_padding: 0.02 + plugins: + - voxel_layer + - inflation_layer + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + cost_scaling_factor: 3.0 + inflation_radius: 0.2 + voxel_layer: + plugin: nav2_costmap_2d::VoxelLayer + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False diff --git a/src/planner/config/planner.yaml.bak- b/src/planner/config/planner.yaml.bak- deleted file mode 100644 index 31c2ffd..0000000 --- a/src/planner/config/planner.yaml.bak- +++ /dev/null @@ -1,232 +0,0 @@ -# ============================================================================ -# planner.yaml — Nav2 minimal global planning stack -# 节点: slam_toolbox + planner_server + global_costmap + local_costmap -# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁 -# ============================================================================ - -# --------------------------------------------------------------------------- -# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform -# 来源: slam_toolbox_mapping.yaml -# --------------------------------------------------------------------------- -slam_toolbox: - ros__parameters: - use_sim_time: False - - # Solver - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ROS base - odom_frame: odom - map_frame: map - base_frame: base_link - scan_topic: /scan - mode: mapping - use_map_saver: true - - # Debug & performance - debug_logging: false - throttle_scans: 1 - transform_publish_period: 0.02 - map_update_interval: 3.0 - resolution: 0.05 - min_laser_range: 0.15 - max_laser_range: 20.0 - minimum_time_interval: 0.5 - transform_timeout: 0.2 - tf_buffer_duration: 30.0 - stack_size_to_use: 40000000 - enable_interactive_mode: true - - # Mapping - use_scan_matching: true - use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.1 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - - # Loop closure - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - loop_search_maximum_distance: 3.0 - - # Scan matching - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - - # Matcher params - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true - min_pass_through: 2 - occupancy_threshold: 0.1 - scan_queue_size: 20 - -# --------------------------------------------------------------------------- -# planner_server — SmacPlannerHybrid 全局路径规划 → /plan -# 来源: gc_navigation_slam.yaml planner_server section -# --------------------------------------------------------------------------- -planner_server: - ros__parameters: - planner_plugins: - - GridBased - use_sim_time: False - GridBased: - plugin: nav2_smac_planner/SmacPlannerHybrid - downsample_costmap: False - downsampling_factor: 1 - tolerance: 0.25 - allow_unknown: True - max_iterations: 100000 - max_on_approach_iterations: 1000 - max_planning_time: 5.0 - motion_model_for_search: REEDS_SHEPP - angle_quantization_bins: 72 - analytic_expansion_ratio: 2.0 - analytic_expansion_max_length: 3.0 - minimum_turning_radius: 0.35 - reverse_penalty: 1.3 - change_penalty: 0.0 - non_straight_penalty: 0.0 - cost_penalty: 5.0 - retrospective_penalty: 0.015 - lookup_table_size: 5.0 - cache_obstacle_heuristic: False - viz_expansions: False - smooth_path: True - smoother: - max_iterations: 1000 - w_smooth: 0.4 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: True - refinement_num: 4 - -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: False - -# --------------------------------------------------------------------------- -# global_costmap — 全局代价地图（订阅 /scan + /map） -# 来源: gc_navigation_slam.yaml global_costmap section -# --------------------------------------------------------------------------- -global_costmap: - global_costmap: - ros__parameters: - use_sim_time: False - transform_tolerance: 2.0 - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' - footprint_padding: 0.02 - resolution: 0.05 - track_unknown_space: True - plugins: - - static_layer - - obstacle_layer - - inflation_layer - obstacle_layer: - plugin: nav2_costmap_2d::ObstacleLayer - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: LaserScan - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: nav2_costmap_2d::StaticLayer - map_subscribe_transient_local: True - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - cost_scaling_factor: 3.0 - inflation_radius: 0.2 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: False - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: False - -# --------------------------------------------------------------------------- -# local_costmap — 局部代价地图（obstacle_detector 可注入） -# 来源: gc_navigation_slam.yaml local_costmap section -# --------------------------------------------------------------------------- -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 2.0 - transform_tolerance: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: False - rolling_window: True - width: 3 - height: 3 - resolution: 0.05 - footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' - footprint_padding: 0.02 - plugins: - - voxel_layer - - inflation_layer - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - cost_scaling_factor: 3.0 - inflation_radius: 0.2 - voxel_layer: - plugin: nav2_costmap_2d::VoxelLayer - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: LaserScan - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: False - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: False diff --git a/src/planner/config/planner.yaml.bak-20260623-160129 b/src/planner/config/planner.yaml.bak-20260623-160129 deleted file mode 100644 index 31c2ffd..0000000 --- a/src/planner/config/planner.yaml.bak-20260623-160129 +++ /dev/null @@ -1,232 +0,0 @@ -# ============================================================================ -# planner.yaml — Nav2 minimal global planning stack -# 节点: slam_toolbox + planner_server + global_costmap + local_costmap -# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁 -# ============================================================================ - -# --------------------------------------------------------------------------- -# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform -# 来源: slam_toolbox_mapping.yaml -# --------------------------------------------------------------------------- -slam_toolbox: - ros__parameters: - use_sim_time: False - - # Solver - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ROS base - odom_frame: odom - map_frame: map - base_frame: base_link - scan_topic: /scan - mode: mapping - use_map_saver: true - - # Debug & performance - debug_logging: false - throttle_scans: 1 - transform_publish_period: 0.02 - map_update_interval: 3.0 - resolution: 0.05 - min_laser_range: 0.15 - max_laser_range: 20.0 - minimum_time_interval: 0.5 - transform_timeout: 0.2 - tf_buffer_duration: 30.0 - stack_size_to_use: 40000000 - enable_interactive_mode: true - - # Mapping - use_scan_matching: true - use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.1 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - - # Loop closure - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - loop_search_maximum_distance: 3.0 - - # Scan matching - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - - # Matcher params - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true - min_pass_through: 2 - occupancy_threshold: 0.1 - scan_queue_size: 20 - -# --------------------------------------------------------------------------- -# planner_server — SmacPlannerHybrid 全局路径规划 → /plan -# 来源: gc_navigation_slam.yaml planner_server section -# --------------------------------------------------------------------------- -planner_server: - ros__parameters: - planner_plugins: - - GridBased - use_sim_time: False - GridBased: - plugin: nav2_smac_planner/SmacPlannerHybrid - downsample_costmap: False - downsampling_factor: 1 - tolerance: 0.25 - allow_unknown: True - max_iterations: 100000 - max_on_approach_iterations: 1000 - max_planning_time: 5.0 - motion_model_for_search: REEDS_SHEPP - angle_quantization_bins: 72 - analytic_expansion_ratio: 2.0 - analytic_expansion_max_length: 3.0 - minimum_turning_radius: 0.35 - reverse_penalty: 1.3 - change_penalty: 0.0 - non_straight_penalty: 0.0 - cost_penalty: 5.0 - retrospective_penalty: 0.015 - lookup_table_size: 5.0 - cache_obstacle_heuristic: False - viz_expansions: False - smooth_path: True - smoother: - max_iterations: 1000 - w_smooth: 0.4 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: True - refinement_num: 4 - -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: False - -# --------------------------------------------------------------------------- -# global_costmap — 全局代价地图（订阅 /scan + /map） -# 来源: gc_navigation_slam.yaml global_costmap section -# --------------------------------------------------------------------------- -global_costmap: - global_costmap: - ros__parameters: - use_sim_time: False - transform_tolerance: 2.0 - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' - footprint_padding: 0.02 - resolution: 0.05 - track_unknown_space: True - plugins: - - static_layer - - obstacle_layer - - inflation_layer - obstacle_layer: - plugin: nav2_costmap_2d::ObstacleLayer - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: LaserScan - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: nav2_costmap_2d::StaticLayer - map_subscribe_transient_local: True - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - cost_scaling_factor: 3.0 - inflation_radius: 0.2 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: False - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: False - -# --------------------------------------------------------------------------- -# local_costmap — 局部代价地图（obstacle_detector 可注入） -# 来源: gc_navigation_slam.yaml local_costmap section -# --------------------------------------------------------------------------- -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 2.0 - transform_tolerance: 2.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: False - rolling_window: True - width: 3 - height: 3 - resolution: 0.05 - footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]' - footprint_padding: 0.02 - plugins: - - voxel_layer - - inflation_layer - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - cost_scaling_factor: 3.0 - inflation_radius: 0.2 - voxel_layer: - plugin: nav2_costmap_2d::VoxelLayer - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: LaserScan - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: False - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: False diff --git a/src/planner/config/pure_pursuit_tracker.yaml b/src/planner/config/pure_pursuit_tracker.yaml new file mode 100644 index 0000000..c719a56 --- /dev/null +++ b/src/planner/config/pure_pursuit_tracker.yaml @@ -0,0 +1,31 @@ +topology_pure_pursuit: + ros__parameters: + use_sim_time: false + + # Tracker-only mode: consume /plan generated by a planner node. + enable_topology_planning: false + accept_external_plan: true + publish_cmd_vel: true + stop_without_plan: true + + map_frame: map + odom_topic: /odom + external_plan_topic: /plan + cmd_vel_topic: /cmd_vel + status_topic: /pure_pursuit_status + + control_rate: 20.0 + lookahead_distance: 0.55 + goal_tolerance: 0.20 + linear_speed: 0.30 + min_linear_speed: 0.08 + reverse_speed: 0.12 + slowdown_distance: 0.80 + curvature_slowdown_gain: 0.18 + max_angular_speed: 1.5 + min_turning_radius: 0.40 + + latency_compensation: true + max_latency_compensation: 0.25 + max_odom_age: 0.80 + tf_timeout: 0.05 diff --git a/src/planner/config/topology_graph.yaml b/src/planner/config/topology_graph.yaml new file mode 100644 index 0000000..c621baa --- /dev/null +++ b/src/planner/config/topology_graph.yaml @@ -0,0 +1,38 @@ +# Fixed-field topology graph for the lightweight planner. +# +# Replace the sample coordinates with measured corridor-center coordinates from +# your map. Send a target by node name: +# ros2 topic pub -1 /topology_goal std_msgs/msg/String "{data: bed_b}" +# +# Or send /goal_pose near a node; the nearest node is selected. + +default_start: start +default_goal: bed_b + +nodes: + start: + x: 0.54 + y: 0.20 + corridor_a: + x: 1.50 + y: 0.20 + corridor_b: + x: 2.50 + y: 0.80 + bed_a: + x: 3.20 + y: 1.60 + bed_b: + x: 2.54 + y: 2.50 + return_point: + x: 0.54 + y: 0.20 + +edges: + - [start, corridor_a] + - [corridor_a, corridor_b] + - [corridor_b, bed_a] + - [corridor_b, bed_b] + - [bed_a, bed_b] + - [start, return_point] diff --git a/src/planner/config/topology_pure_pursuit.yaml b/src/planner/config/topology_pure_pursuit.yaml new file mode 100644 index 0000000..95f17a5 --- /dev/null +++ b/src/planner/config/topology_pure_pursuit.yaml @@ -0,0 +1,44 @@ +topology_pure_pursuit: + ros__parameters: + use_sim_time: false + + # Graph file is normally overridden by the launch file after installation. + graph_file: "" + + # Topic interface. Run this node instead of Nav2 controller_server to avoid + # multiple publishers fighting over /cmd_vel. + # The graph coordinates live in this frame. The node transforms odom poses + # into this frame through TF when odom.header.frame_id differs. + map_frame: map + odom_topic: /odom + goal_pose_topic: /goal_pose + goal_node_topic: /topology_goal + plan_topic: /plan + cmd_vel_topic: /cmd_vel + status_topic: /topology_status + publish_cmd_vel: true + stop_without_plan: true + + # Planner. Keep the graph sparse and place nodes near lane or corridor + # centers so localization error still leaves a useful safety margin. + autoplan_default_goal: false + path_resolution: 0.10 + nearest_node_max_distance: 2.0 + + # Pure Pursuit tracking. The defaults are conservative for the origincar + # 0.143 m wheelbase and current /cmd_vel -> Ackermann conversion. + control_rate: 20.0 + lookahead_distance: 0.55 + goal_tolerance: 0.25 + linear_speed: 0.30 + min_linear_speed: 0.08 + slowdown_distance: 0.80 + curvature_slowdown_gain: 0.18 + max_angular_speed: 1.5 + min_turning_radius: 0.35 + + # Delay handling. Stamped odom is projected forward by at most this horizon. + latency_compensation: true + max_latency_compensation: 0.25 + max_odom_age: 0.80 + tf_timeout: 0.05 diff --git a/src/planner/launch/grid_astar_theta.launch.py b/src/planner/launch/grid_astar_theta.launch.py new file mode 100644 index 0000000..8384c8d --- /dev/null +++ b/src/planner/launch/grid_astar_theta.launch.py @@ -0,0 +1,43 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("planner") + params_file = LaunchConfiguration( + "params_file", + default=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"), + ) + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + + planner_node = Node( + package="planner", + executable="grid_astar_theta_node", + name="grid_astar_theta_planner", + output="screen", + parameters=[ + params_file, + {"use_sim_time": use_sim_time}, + ], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "params_file", + default_value=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"), + description="Parameters for lightweight Ackermann Hybrid A* planner.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock.", + ), + planner_node, + ] + ) diff --git a/src/planner/launch/grid_astar_theta_with_tracker.launch.py b/src/planner/launch/grid_astar_theta_with_tracker.launch.py new file mode 100644 index 0000000..c297ff7 --- /dev/null +++ b/src/planner/launch/grid_astar_theta_with_tracker.launch.py @@ -0,0 +1,59 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("planner") + planner_params = LaunchConfiguration( + "planner_params", + default=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"), + ) + tracker_params = LaunchConfiguration( + "tracker_params", + default=os.path.join(pkg_dir, "config", "pure_pursuit_tracker.yaml"), + ) + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + + planner_node = Node( + package="planner", + executable="grid_astar_theta_node", + name="grid_astar_theta_planner", + output="screen", + parameters=[planner_params, {"use_sim_time": use_sim_time}], + ) + tracker_node = Node( + package="planner", + executable="topology_pure_pursuit_node.py", + name="topology_pure_pursuit", + output="screen", + parameters=[tracker_params, {"use_sim_time": use_sim_time}], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "planner_params", + default_value=os.path.join(pkg_dir, "config", "grid_astar_theta.yaml"), + description="Parameters for lightweight Ackermann Hybrid A* planner.", + ), + DeclareLaunchArgument( + "tracker_params", + default_value=os.path.join( + pkg_dir, "config", "pure_pursuit_tracker.yaml" + ), + description="Parameters for yaw-aware tracker-only mode.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock.", + ), + planner_node, + tracker_node, + ] + ) diff --git a/src/planner/launch/planner.launch.py b/src/planner/launch/planner.launch.py index aa7669f..bfedd16 100644 --- a/src/planner/launch/planner.launch.py +++ b/src/planner/launch/planner.launch.py @@ -1,77 +1,77 @@ -# ============================================================================ -# planner.launch.py -# Nav2 最小全局规划模块 -# 启动: slam_toolbox + planner_server + lifecycle_manager -# costmap 由 planner_server 内部实例化，不需要单独启动 -# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother -# -# 参考: gc_nav2_with_slam_online_real.launch.py -# ============================================================================ - -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - """启动 Nav2 最小规划栈：slam + planner""" - ld = LaunchDescription() - - # === 包路径 === - pkg_dir = get_package_share_directory('planner') - - # === 参数 === - use_sim_time = LaunchConfiguration('use_sim_time', default='false') - params_file = LaunchConfiguration( - 'params_file', - default=os.path.join(pkg_dir, 'config', 'planner.yaml')) - - declare_params = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'), - description='Path to the planner parameters file') - ld.add_action(declare_params) - - # ===================================================================== - # 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom - # ===================================================================== - slam_toolbox_node = Node( - package='slam_toolbox', - executable='async_slam_toolbox_node', - name='slam_toolbox', - output='screen', - parameters=[params_file, - {'use_sim_time': use_sim_time}], - ) - ld.add_action(slam_toolbox_node) - - # ===================================================================== - # 2. planner_server — SmacPlannerHybrid，ComputePathToPose action - # planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数） - # ===================================================================== - planner_server_node = Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[params_file], - ) - ld.add_action(planner_server_node) - - # ===================================================================== - # 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点) - # ===================================================================== - lifecycle_manager_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_planner', - output='screen', - parameters=[{'autostart': True}, - {'node_names': ['planner_server']}], - ) - ld.add_action(lifecycle_manager_node) - - return ld +# ============================================================================ +# planner.launch.py +# Nav2 最小全局规划模块 +# 启动: slam_toolbox + planner_server + lifecycle_manager +# costmap 由 planner_server 内部实例化，不需要单独启动 +# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother +# +# 参考: gc_nav2_with_slam_online_real.launch.py +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + """启动 Nav2 最小规划栈：slam + planner""" + ld = LaunchDescription() + + # === 包路径 === + pkg_dir = get_package_share_directory('planner') + + # === 参数 === + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + params_file = LaunchConfiguration( + 'params_file', + default=os.path.join(pkg_dir, 'config', 'planner.yaml')) + + declare_params = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'), + description='Path to the planner parameters file') + ld.add_action(declare_params) + + # ===================================================================== + # 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom + # ===================================================================== + slam_toolbox_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[params_file, + {'use_sim_time': use_sim_time}], + ) + ld.add_action(slam_toolbox_node) + + # ===================================================================== + # 2. planner_server — SmacPlannerHybrid，ComputePathToPose action + # planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数） + # ===================================================================== + planner_server_node = Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[params_file], + ) + ld.add_action(planner_server_node) + + # ===================================================================== + # 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点) + # ===================================================================== + lifecycle_manager_node = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_planner', + output='screen', + parameters=[{'autostart': True}, + {'node_names': ['planner_server']}], + ) + ld.add_action(lifecycle_manager_node) + + return ld diff --git a/src/planner/launch/pure_pursuit_tracker.launch.py b/src/planner/launch/pure_pursuit_tracker.launch.py new file mode 100644 index 0000000..c1478a1 --- /dev/null +++ b/src/planner/launch/pure_pursuit_tracker.launch.py @@ -0,0 +1,45 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("planner") + params_file = LaunchConfiguration( + "params_file", + default=os.path.join(pkg_dir, "config", "pure_pursuit_tracker.yaml"), + ) + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + + tracker_node = Node( + package="planner", + executable="topology_pure_pursuit_node.py", + name="topology_pure_pursuit", + output="screen", + parameters=[ + params_file, + {"use_sim_time": use_sim_time}, + ], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "params_file", + default_value=os.path.join( + pkg_dir, "config", "pure_pursuit_tracker.yaml" + ), + description="Parameters for Pure Pursuit tracker-only mode.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock.", + ), + tracker_node, + ] + ) diff --git a/src/planner/launch/topology_pure_pursuit.launch.py b/src/planner/launch/topology_pure_pursuit.launch.py new file mode 100644 index 0000000..5ff0aa0 --- /dev/null +++ b/src/planner/launch/topology_pure_pursuit.launch.py @@ -0,0 +1,58 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("planner") + + params_file = LaunchConfiguration( + "params_file", + default=os.path.join(pkg_dir, "config", "topology_pure_pursuit.yaml"), + ) + graph_file = LaunchConfiguration( + "graph_file", + default=os.path.join(pkg_dir, "config", "topology_graph.yaml"), + ) + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + + topology_node = Node( + package="planner", + executable="topology_pure_pursuit_node.py", + name="topology_pure_pursuit", + output="screen", + parameters=[ + params_file, + { + "graph_file": graph_file, + "use_sim_time": use_sim_time, + }, + ], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "params_file", + default_value=os.path.join( + pkg_dir, "config", "topology_pure_pursuit.yaml" + ), + description="Parameters for topology Pure Pursuit.", + ), + DeclareLaunchArgument( + "graph_file", + default_value=os.path.join(pkg_dir, "config", "topology_graph.yaml"), + description="Topology graph YAML.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation clock.", + ), + topology_node, + ] + ) diff --git a/src/planner/package.xml b/src/planner/package.xml index bf4169c..ce7f983 100644 --- a/src/planner/package.xml +++ b/src/planner/package.xml @@ -1,26 +1,34 @@ - - - - planner - 0.0.0 - Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager - sunrise - TODO: License declaration - + + + + planner + 0.0.0 + Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager + sunrise + TODO: License declaration + ament_cmake rclcpp + geometry_msgs + nav_msgs + std_msgs slam_toolbox nav2_planner nav2_costmap_2d nav2_lifecycle_manager nav2_common nav2_util + tf2 + tf2_geometry_msgs + tf2_ros + rclpy + python3-yaml ament_lint_auto - ament_lint_common - - - ament_cmake - - + ament_lint_common + + + ament_cmake + + diff --git a/src/planner/scripts/grid_astar_theta_node.py b/src/planner/scripts/grid_astar_theta_node.py new file mode 100644 index 0000000..2cf0fe9 --- /dev/null +++ b/src/planner/scripts/grid_astar_theta_node.py @@ -0,0 +1,475 @@ +#!/usr/bin/env python3 +"""Lightweight grid A*/Theta* planner for random circular obstacles.""" + +import heapq +import math + +import rclpy +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Odometry, Path as NavPath +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.time import Time +from std_msgs.msg import Float32MultiArray, String +from tf2_ros import Buffer, TransformException, TransformListener + + +def yaw_from_quaternion(q): + return math.atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z), + ) + + +def normalize_angle(angle): + return math.atan2(math.sin(angle), math.cos(angle)) + + +def make_pose(frame_id, stamp, x, y, yaw): + pose = PoseStamped() + pose.header.frame_id = frame_id + pose.header.stamp = stamp + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = 0.0 + pose.pose.orientation.z = math.sin(0.5 * yaw) + pose.pose.orientation.w = math.cos(0.5 * yaw) + return pose + + +class GridAstarThetaPlanner(Node): + def __init__(self): + super().__init__("grid_astar_theta_planner") + + self.declare_parameter("map_frame", "map") + self.declare_parameter("odom_topic", "/odom") + self.declare_parameter("goal_pose_topic", "/goal_pose") + self.declare_parameter("obstacles_topic", "/random_obstacles") + self.declare_parameter("plan_topic", "/plan") + self.declare_parameter("status_topic", "/grid_planner_status") + self.declare_parameter("map_min_x", -5.0) + self.declare_parameter("map_min_y", -5.0) + self.declare_parameter("map_max_x", 5.0) + self.declare_parameter("map_max_y", 5.0) + self.declare_parameter("resolution", 0.10) + self.declare_parameter("obstacle_format", "xyr") + self.declare_parameter("default_obstacle_radius", 0.15) + self.declare_parameter("robot_radius", 0.14) + self.declare_parameter("localization_error", 0.10) + self.declare_parameter("latency_margin", 0.05) + self.declare_parameter("safety_margin", 0.05) + self.declare_parameter("unknown_obstacle_radius", 0.15) + self.declare_parameter("use_theta_star", True) + self.declare_parameter("allow_diagonal", True) + self.declare_parameter("plan_on_obstacle_update", True) + self.declare_parameter("max_iterations", 50000) + self.declare_parameter("path_resolution", 0.10) + self.declare_parameter("smooth_iterations", 2) + self.declare_parameter("max_odom_age", 0.80) + self.declare_parameter("tf_timeout", 0.05) + + self.map_frame = self.get_parameter("map_frame").value + self.map_min_x = float(self.get_parameter("map_min_x").value) + self.map_min_y = float(self.get_parameter("map_min_y").value) + self.map_max_x = float(self.get_parameter("map_max_x").value) + self.map_max_y = float(self.get_parameter("map_max_y").value) + self.resolution = float(self.get_parameter("resolution").value) + self.obstacle_format = str(self.get_parameter("obstacle_format").value) + self.default_obstacle_radius = float( + self.get_parameter("default_obstacle_radius").value + ) + self.robot_radius = float(self.get_parameter("robot_radius").value) + self.localization_error = float( + self.get_parameter("localization_error").value + ) + self.latency_margin = float(self.get_parameter("latency_margin").value) + self.safety_margin = float(self.get_parameter("safety_margin").value) + self.unknown_obstacle_radius = float( + self.get_parameter("unknown_obstacle_radius").value + ) + self.use_theta_star = bool(self.get_parameter("use_theta_star").value) + self.allow_diagonal = bool(self.get_parameter("allow_diagonal").value) + self.plan_on_obstacle_update = bool( + self.get_parameter("plan_on_obstacle_update").value + ) + self.max_iterations = int(self.get_parameter("max_iterations").value) + self.path_resolution = float(self.get_parameter("path_resolution").value) + self.smooth_iterations = int(self.get_parameter("smooth_iterations").value) + self.max_odom_age = float(self.get_parameter("max_odom_age").value) + self.tf_timeout = float(self.get_parameter("tf_timeout").value) + + if self.resolution <= 0.0: + raise RuntimeError("resolution must be positive") + if self.map_max_x <= self.map_min_x or self.map_max_y <= self.map_min_y: + raise RuntimeError("invalid map bounds") + + self.width = int(math.ceil((self.map_max_x - self.map_min_x) / self.resolution)) + self.height = int(math.ceil((self.map_max_y - self.map_min_y) / self.resolution)) + if self.width <= 1 or self.height <= 1: + raise RuntimeError("map bounds are too small for the selected resolution") + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.latest_odom = None + self.goal_pose = None + self.obstacles = [] + self.last_tf_warn_ns = 0 + + self.plan_pub = self.create_publisher( + NavPath, self.get_parameter("plan_topic").value, 10 + ) + self.status_pub = self.create_publisher( + String, self.get_parameter("status_topic").value, 10 + ) + + self.odom_sub = self.create_subscription( + Odometry, + self.get_parameter("odom_topic").value, + self._odom_callback, + 10, + ) + self.goal_sub = self.create_subscription( + PoseStamped, + self.get_parameter("goal_pose_topic").value, + self._goal_callback, + 10, + ) + self.obstacles_sub = self.create_subscription( + Float32MultiArray, + self.get_parameter("obstacles_topic").value, + self._obstacles_callback, + 10, + ) + + self._publish_status( + f"ready size={self.width}x{self.height} resolution={self.resolution:.3f}" + ) + + def _odom_callback(self, msg): + self.latest_odom = msg + + def _goal_callback(self, msg): + transformed = self._transform_pose_msg(msg) + if transformed is None: + self._publish_status("goal_rejected transform_failed") + return + self.goal_pose = transformed + self.plan() + + def _obstacles_callback(self, msg): + self.obstacles = self._parse_obstacles(msg.data) + if self.plan_on_obstacle_update and self.goal_pose is not None: + self.plan() + + def _parse_obstacles(self, values): + stride = 3 if self.obstacle_format == "xyr" else 2 + obstacles = [] + if stride <= 0: + return obstacles + count = len(values) // stride + for index in range(count): + base = index * stride + x = float(values[base]) + y = float(values[base + 1]) + radius = self.default_obstacle_radius + if stride == 3: + radius = max(0.0, float(values[base + 2])) + obstacles.append((x, y, radius)) + return obstacles + + def plan(self): + start_pose = self._current_pose() + if start_pose is None: + self._publish_status("plan_rejected no_recent_odom") + return False + if self.goal_pose is None: + self._publish_status("plan_rejected no_goal") + return False + + start = self._world_to_grid(start_pose[0], start_pose[1]) + goal = self._world_to_grid(self.goal_pose[0], self.goal_pose[1]) + if start is None or goal is None: + self._publish_status(f"plan_rejected out_of_bounds start={start} goal={goal}") + return False + + occupied = self._build_occupancy(start, goal) + if occupied[start[1]][start[0]] or occupied[goal[1]][goal[0]]: + self._publish_status("plan_rejected start_or_goal_occupied") + return False + + path_cells = self._search(start, goal, occupied) + if not path_cells: + self._publish_status("plan_failed no_path") + return False + + points = [self._grid_to_world(cell[0], cell[1]) for cell in path_cells] + points[0] = (start_pose[0], start_pose[1]) + points[-1] = (self.goal_pose[0], self.goal_pose[1]) + if self.smooth_iterations > 0: + points = self._smooth_by_line_of_sight(points, occupied) + points = self._densify(points) + self._publish_path(points) + self._publish_status( + f"plan_ok cells={len(path_cells)} points={len(points)} obstacles={len(self.obstacles)}" + ) + return True + + def _build_occupancy(self, start, goal): + occupied = [[False for _ in range(self.width)] for _ in range(self.height)] + base_padding = self.robot_radius + self.localization_error + self.latency_margin + self.safety_margin + for obs_x, obs_y, obs_radius in self.obstacles: + radius = max(obs_radius, self.unknown_obstacle_radius) + base_padding + min_cell = self._world_to_grid(obs_x - radius, obs_y - radius, clamp=True) + max_cell = self._world_to_grid(obs_x + radius, obs_y + radius, clamp=True) + radius_sq = radius * radius + for gy in range(min_cell[1], max_cell[1] + 1): + for gx in range(min_cell[0], max_cell[0] + 1): + wx, wy = self._grid_to_world(gx, gy) + if (wx - obs_x) * (wx - obs_x) + (wy - obs_y) * (wy - obs_y) <= radius_sq: + occupied[gy][gx] = True + occupied[start[1]][start[0]] = False + occupied[goal[1]][goal[0]] = False + return occupied + + def _search(self, start, goal, occupied): + neighbors = self._neighbors8 if self.allow_diagonal else self._neighbors4 + open_heap = [] + heapq.heappush(open_heap, (0.0, start)) + came_from = {start: start} + cost_so_far = {start: 0.0} + iterations = 0 + + while open_heap and iterations < self.max_iterations: + iterations += 1 + _, current = heapq.heappop(open_heap) + if current == goal: + return self._reconstruct_path(came_from, start, goal) + + for neighbor, move_cost in neighbors(current, occupied): + if occupied[neighbor[1]][neighbor[0]]: + continue + + parent = came_from[current] + if ( + self.use_theta_star + and parent != current + and self._line_of_sight(parent, neighbor, occupied) + ): + candidate_parent = parent + new_cost = cost_so_far[parent] + self._grid_distance(parent, neighbor) + else: + candidate_parent = current + new_cost = cost_so_far[current] + move_cost + + if new_cost < cost_so_far.get(neighbor, float("inf")): + cost_so_far[neighbor] = new_cost + came_from[neighbor] = candidate_parent + priority = new_cost + self._grid_distance(neighbor, goal) + heapq.heappush(open_heap, (priority, neighbor)) + + return [] + + def _neighbors4(self, cell, occupied): + del occupied + x, y = cell + for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)): + nx, ny = x + dx, y + dy + if 0 <= nx < self.width and 0 <= ny < self.height: + yield (nx, ny), 1.0 + + def _neighbors8(self, cell, occupied): + x, y = cell + for dx in (-1, 0, 1): + for dy in (-1, 0, 1): + if dx == 0 and dy == 0: + continue + nx, ny = x + dx, y + dy + if 0 <= nx < self.width and 0 <= ny < self.height: + if dx != 0 and dy != 0: + # Avoid cutting through the corner of two inflated cells. + if occupied[y][nx] or occupied[ny][x]: + continue + yield (nx, ny), math.hypot(dx, dy) + + def _reconstruct_path(self, came_from, start, goal): + current = goal + path = [current] + while current != start: + current = came_from[current] + path.append(current) + path.reverse() + return path + + def _line_of_sight(self, start, goal, occupied): + x0, y0 = start + x1, y1 = goal + dx = abs(x1 - x0) + dy = abs(y1 - y0) + sx = 1 if x0 < x1 else -1 + sy = 1 if y0 < y1 else -1 + err = dx - dy + x, y = x0, y0 + + while True: + if x < 0 or x >= self.width or y < 0 or y >= self.height: + return False + if occupied[y][x]: + return False + if x == x1 and y == y1: + return True + e2 = 2 * err + if e2 > -dy: + err -= dy + x += sx + if e2 < dx: + err += dx + y += sy + + def _smooth_by_line_of_sight(self, points, occupied): + if len(points) <= 2: + return points + cells = [self._world_to_grid(x, y, clamp=True) for x, y in points] + for _ in range(self.smooth_iterations): + smoothed = [points[0]] + anchor_index = 0 + test_index = 2 + while test_index < len(points): + if not self._line_of_sight(cells[anchor_index], cells[test_index], occupied): + smoothed.append(points[test_index - 1]) + anchor_index = test_index - 1 + test_index += 1 + smoothed.append(points[-1]) + points = smoothed + cells = [self._world_to_grid(x, y, clamp=True) for x, y in points] + return points + + def _densify(self, points): + if len(points) <= 1: + return points + dense = [points[0]] + step = max(self.resolution, self.path_resolution) + for target in points[1:]: + sx, sy = dense[-1] + tx, ty = target + distance = math.hypot(tx - sx, ty - sy) + if distance < 1e-6: + continue + steps = max(1, int(math.ceil(distance / step))) + for idx in range(1, steps + 1): + ratio = idx / steps + dense.append((sx + (tx - sx) * ratio, sy + (ty - sy) * ratio)) + return dense + + def _publish_path(self, points): + msg = NavPath() + msg.header.frame_id = self.map_frame + msg.header.stamp = self.get_clock().now().to_msg() + for index, (x, y) in enumerate(points): + if index + 1 < len(points): + nx, ny = points[index + 1] + yaw = math.atan2(ny - y, nx - x) + elif index > 0: + px, py = points[index - 1] + yaw = math.atan2(y - py, x - px) + else: + yaw = 0.0 + msg.poses.append(make_pose(self.map_frame, msg.header.stamp, x, y, yaw)) + self.plan_pub.publish(msg) + + def _current_pose(self): + if self.latest_odom is None: + return None + msg = self.latest_odom + if msg.header.stamp.sec != 0 or msg.header.stamp.nanosec != 0: + age = (self.get_clock().now() - Time.from_msg(msg.header.stamp)).nanoseconds * 1e-9 + if age > self.max_odom_age: + return None + + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + yaw = yaw_from_quaternion(msg.pose.pose.orientation) + source_frame = msg.header.frame_id or self.map_frame + if source_frame != self.map_frame: + return self._transform_pose_2d(x, y, yaw, source_frame) + return x, y, yaw + + def _transform_pose_msg(self, msg): + source_frame = msg.header.frame_id or self.map_frame + x = msg.pose.position.x + y = msg.pose.position.y + yaw = yaw_from_quaternion(msg.pose.orientation) + if source_frame != self.map_frame: + return self._transform_pose_2d(x, y, yaw, source_frame) + return x, y, yaw + + def _transform_pose_2d(self, x, y, yaw, source_frame): + try: + transform = self.tf_buffer.lookup_transform( + self.map_frame, + source_frame, + Time(), + timeout=Duration(seconds=self.tf_timeout), + ) + except TransformException as exc: + now_ns = self.get_clock().now().nanoseconds + if now_ns - self.last_tf_warn_ns > 2_000_000_000: + self.last_tf_warn_ns = now_ns + self.get_logger().warn( + f"TF unavailable {source_frame}->{self.map_frame}: {exc}" + ) + return None + + t = transform.transform.translation + q = transform.transform.rotation + transform_yaw = yaw_from_quaternion(q) + cos_yaw = math.cos(transform_yaw) + sin_yaw = math.sin(transform_yaw) + target_x = t.x + cos_yaw * x - sin_yaw * y + target_y = t.y + sin_yaw * x + cos_yaw * y + target_yaw = normalize_angle(transform_yaw + yaw) + return target_x, target_y, target_yaw + + def _world_to_grid(self, x, y, clamp=False): + gx = int(math.floor((x - self.map_min_x) / self.resolution)) + gy = int(math.floor((y - self.map_min_y) / self.resolution)) + if clamp: + gx = min(self.width - 1, max(0, gx)) + gy = min(self.height - 1, max(0, gy)) + return gx, gy + if gx < 0 or gx >= self.width or gy < 0 or gy >= self.height: + return None + return gx, gy + + def _grid_to_world(self, gx, gy): + return ( + self.map_min_x + (gx + 0.5) * self.resolution, + self.map_min_y + (gy + 0.5) * self.resolution, + ) + + @staticmethod + def _grid_distance(a, b): + return math.hypot(a[0] - b[0], a[1] - b[1]) + + def _publish_status(self, text): + msg = String() + msg.data = text + self.status_pub.publish(msg) + self.get_logger().info(text) + + +def main(args=None): + rclpy.init(args=args) + node = None + try: + node = GridAstarThetaPlanner() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/planner/scripts/topology_pure_pursuit_node.py b/src/planner/scripts/topology_pure_pursuit_node.py new file mode 100644 index 0000000..e2c430b --- /dev/null +++ b/src/planner/scripts/topology_pure_pursuit_node.py @@ -0,0 +1,629 @@ +#!/usr/bin/env python3 +"""Lightweight topology graph planner with Pure Pursuit tracking.""" + +import heapq +import math +from pathlib import Path + +import rclpy +from geometry_msgs.msg import PoseStamped, Twist +from nav_msgs.msg import Odometry, Path as NavPath +from rclpy.node import Node +from rclpy.time import Time +from std_msgs.msg import String +import yaml + +from rclpy.duration import Duration +from tf2_ros import Buffer, TransformException, TransformListener + + +def yaw_from_quaternion(q): + return math.atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z), + ) + + +def normalize_angle(angle): + return math.atan2(math.sin(angle), math.cos(angle)) + + +def make_pose(frame_id, stamp, x, y, yaw): + pose = PoseStamped() + pose.header.frame_id = frame_id + pose.header.stamp = stamp + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = 0.0 + pose.pose.orientation.z = math.sin(yaw * 0.5) + pose.pose.orientation.w = math.cos(yaw * 0.5) + return pose + + +class TopologyPurePursuit(Node): + def __init__(self): + super().__init__("topology_pure_pursuit") + + self.declare_parameter("graph_file", "") + self.declare_parameter("map_frame", "map") + self.declare_parameter("odom_topic", "/odom") + self.declare_parameter("goal_pose_topic", "/goal_pose") + self.declare_parameter("goal_node_topic", "/topology_goal") + self.declare_parameter("external_plan_topic", "/plan") + self.declare_parameter("plan_topic", "/plan") + self.declare_parameter("cmd_vel_topic", "/cmd_vel") + self.declare_parameter("status_topic", "/topology_status") + self.declare_parameter("enable_topology_planning", True) + self.declare_parameter("accept_external_plan", False) + self.declare_parameter("publish_cmd_vel", True) + self.declare_parameter("autoplan_default_goal", False) + self.declare_parameter("control_rate", 20.0) + self.declare_parameter("path_resolution", 0.10) + self.declare_parameter("nearest_node_max_distance", 2.0) + self.declare_parameter("lookahead_distance", 0.55) + self.declare_parameter("goal_tolerance", 0.25) + self.declare_parameter("linear_speed", 0.30) + self.declare_parameter("min_linear_speed", 0.08) + self.declare_parameter("reverse_speed", 0.12) + self.declare_parameter("slowdown_distance", 0.80) + self.declare_parameter("curvature_slowdown_gain", 0.18) + self.declare_parameter("max_angular_speed", 1.5) + self.declare_parameter("min_turning_radius", 0.35) + self.declare_parameter("latency_compensation", True) + self.declare_parameter("max_latency_compensation", 0.25) + self.declare_parameter("max_odom_age", 0.80) + self.declare_parameter("tf_timeout", 0.05) + self.declare_parameter("stop_without_plan", True) + + self.map_frame = self.get_parameter("map_frame").value + self.enable_topology_planning = bool( + self.get_parameter("enable_topology_planning").value + ) + self.accept_external_plan = bool( + self.get_parameter("accept_external_plan").value + ) + self.publish_cmd_vel = bool(self.get_parameter("publish_cmd_vel").value) + self.autoplan_default_goal = bool( + self.get_parameter("autoplan_default_goal").value + ) + self.path_resolution = float(self.get_parameter("path_resolution").value) + self.nearest_node_max_distance = float( + self.get_parameter("nearest_node_max_distance").value + ) + self.lookahead_distance = float( + self.get_parameter("lookahead_distance").value + ) + self.goal_tolerance = float(self.get_parameter("goal_tolerance").value) + self.linear_speed = float(self.get_parameter("linear_speed").value) + self.min_linear_speed = float(self.get_parameter("min_linear_speed").value) + self.reverse_speed = float(self.get_parameter("reverse_speed").value) + self.slowdown_distance = float(self.get_parameter("slowdown_distance").value) + self.curvature_slowdown_gain = float( + self.get_parameter("curvature_slowdown_gain").value + ) + self.max_angular_speed = float( + self.get_parameter("max_angular_speed").value + ) + self.min_turning_radius = float( + self.get_parameter("min_turning_radius").value + ) + self.latency_compensation = bool( + self.get_parameter("latency_compensation").value + ) + self.max_latency_compensation = float( + self.get_parameter("max_latency_compensation").value + ) + self.max_odom_age = float(self.get_parameter("max_odom_age").value) + self.tf_timeout = float(self.get_parameter("tf_timeout").value) + self.stop_without_plan = bool(self.get_parameter("stop_without_plan").value) + + self.nodes = {} + self.edges = {} + self.default_start_node = "" + self.default_goal_node = "" + if self.enable_topology_planning: + self._load_graph(str(self.get_parameter("graph_file").value).strip()) + elif not self.accept_external_plan: + raise RuntimeError( + "enable_topology_planning=false requires accept_external_plan=true" + ) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.latest_odom = None + self.active_path = [] + self.active_node_path = [] + self.active_goal_node = "" + self.current_path_index = 0 + self.completed = True + self.tried_default_goal = False + self.last_tf_warn_ns = 0 + + self.plan_pub = self.create_publisher( + NavPath, self.get_parameter("plan_topic").value, 10 + ) + self.cmd_pub = self.create_publisher( + Twist, self.get_parameter("cmd_vel_topic").value, 10 + ) + self.status_pub = self.create_publisher( + String, self.get_parameter("status_topic").value, 10 + ) + + self.odom_sub = self.create_subscription( + Odometry, + self.get_parameter("odom_topic").value, + self._odom_callback, + 10, + ) + self.goal_pose_sub = None + self.goal_node_sub = None + self.external_plan_sub = None + if self.enable_topology_planning: + self.goal_pose_sub = self.create_subscription( + PoseStamped, + self.get_parameter("goal_pose_topic").value, + self._goal_pose_callback, + 10, + ) + self.goal_node_sub = self.create_subscription( + String, + self.get_parameter("goal_node_topic").value, + self._goal_node_callback, + 10, + ) + if self.accept_external_plan: + self.external_plan_sub = self.create_subscription( + NavPath, + self.get_parameter("external_plan_topic").value, + self._external_plan_callback, + 10, + ) + + period = 1.0 / max(1.0, float(self.get_parameter("control_rate").value)) + self.timer = self.create_timer(period, self._control_loop) + self._publish_status( + f"ready graph_nodes={len(self.nodes)} graph_edges={sum(len(v) for v in self.edges.values())}" + ) + + def _load_graph(self, graph_file_value): + if not graph_file_value: + raise RuntimeError("graph_file parameter is empty") + graph_file = Path(graph_file_value) + if not graph_file.exists(): + raise RuntimeError(f"graph file does not exist: {graph_file}") + + with graph_file.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + + self.default_start_node = str(data.get("default_start", "")) + self.default_goal_node = str(data.get("default_goal", "")) + + raw_nodes = data.get("nodes", {}) + for name, value in raw_nodes.items(): + if isinstance(value, dict): + x = value["x"] + y = value["y"] + else: + x = value[0] + y = value[1] + self.nodes[str(name)] = (float(x), float(y)) + + self.edges = {name: [] for name in self.nodes} + for edge in data.get("edges", []): + if isinstance(edge, dict): + start = str(edge["from"]) + goal = str(edge["to"]) + cost = float(edge.get("cost", self._distance_nodes(start, goal))) + bidirectional = bool(edge.get("bidirectional", True)) + else: + start = str(edge[0]) + goal = str(edge[1]) + cost = self._distance_nodes(start, goal) + bidirectional = True + self._add_edge(start, goal, cost) + if bidirectional: + self._add_edge(goal, start, cost) + + if not self.nodes: + raise RuntimeError(f"graph contains no nodes: {graph_file}") + + def _distance_nodes(self, start, goal): + if start not in self.nodes or goal not in self.nodes: + raise RuntimeError(f"edge references unknown node: {start}->{goal}") + ax, ay = self.nodes[start] + bx, by = self.nodes[goal] + return math.hypot(bx - ax, by - ay) + + def _add_edge(self, start, goal, cost): + if start not in self.nodes or goal not in self.nodes: + raise RuntimeError(f"edge references unknown node: {start}->{goal}") + self.edges.setdefault(start, []).append((goal, float(cost))) + + def _odom_callback(self, msg): + self.latest_odom = msg + if ( + self.enable_topology_planning + and self.autoplan_default_goal + and not self.tried_default_goal + and self.default_goal_node + ): + self.tried_default_goal = True + self.plan_to_node(self.default_goal_node) + + def _goal_pose_callback(self, msg): + if not self.enable_topology_planning: + return + goal_node, distance = self._nearest_node( + msg.pose.position.x, msg.pose.position.y + ) + if not goal_node or distance > self.nearest_node_max_distance: + self._publish_status( + f"goal_pose_rejected nearest={goal_node} distance={distance:.2f}" + ) + return + self.plan_to_node(goal_node) + + def _goal_node_callback(self, msg): + if not self.enable_topology_planning: + return + self.plan_to_node(msg.data.strip()) + + def _external_plan_callback(self, msg): + if not self.accept_external_plan: + return + frame_id = msg.header.frame_id or self.map_frame + if frame_id != self.map_frame: + self._publish_status(f"external_plan_rejected frame={frame_id}") + return + if len(msg.poses) < 2: + self._publish_status("external_plan_rejected too_short") + if self.stop_without_plan: + self.active_path = [] + self.completed = True + return + + self.active_path = [ + ( + pose.pose.position.x, + pose.pose.position.y, + yaw_from_quaternion(pose.pose.orientation), + ) + for pose in msg.poses + ] + self.active_node_path = [] + self.active_goal_node = "external_plan" + self.current_path_index = 0 + self.completed = False + self._publish_status(f"external_plan_ok points={len(self.active_path)}") + + def plan_to_node(self, goal_node): + if goal_node not in self.nodes: + self._publish_status(f"goal_node_rejected unknown={goal_node}") + return False + + pose = self._current_pose(compensate=False) + if pose is None: + self._publish_status("plan_rejected no_odom") + return False + + if self.default_start_node and self.default_start_node in self.nodes: + start_hint = self.default_start_node + else: + start_hint = "" + start_node, start_distance = self._nearest_node(pose[0], pose[1]) + if start_hint and start_distance > self.nearest_node_max_distance: + start_node = start_hint + elif not start_node or start_distance > self.nearest_node_max_distance: + self._publish_status( + f"plan_rejected no_near_start nearest={start_node} distance={start_distance:.2f}" + ) + return False + + node_path = self._dijkstra(start_node, goal_node) + if not node_path: + self._publish_status(f"plan_failed {start_node}->{goal_node}") + return False + + dense_path = self._densify_path(node_path, pose) + if len(dense_path) < 2: + self._publish_status(f"plan_failed short_path {start_node}->{goal_node}") + return False + + self.active_path = dense_path + self.active_node_path = node_path + self.active_goal_node = goal_node + self.current_path_index = 0 + self.completed = False + self._publish_path() + self._publish_status( + f"plan_ok {'->'.join(node_path)} points={len(dense_path)}" + ) + return True + + def _nearest_node(self, x, y): + best_name = "" + best_dist = float("inf") + for name, (nx, ny) in self.nodes.items(): + distance = math.hypot(nx - x, ny - y) + if distance < best_dist: + best_name = name + best_dist = distance + return best_name, best_dist + + def _dijkstra(self, start, goal): + queue = [(0.0, start)] + previous = {} + cost_so_far = {start: 0.0} + + while queue: + cost, current = heapq.heappop(queue) + if current == goal: + break + if cost > cost_so_far[current]: + continue + for neighbor, edge_cost in self.edges.get(current, []): + new_cost = cost + edge_cost + if new_cost < cost_so_far.get(neighbor, float("inf")): + cost_so_far[neighbor] = new_cost + previous[neighbor] = current + heapq.heappush(queue, (new_cost, neighbor)) + + if goal not in cost_so_far: + return [] + + path = [goal] + while path[-1] != start: + path.append(previous[path[-1]]) + path.reverse() + return path + + def _densify_path(self, node_path, pose): + points = [(pose[0], pose[1], pose[2])] + for index, node_name in enumerate(node_path): + target = self.nodes[node_name] + if index == 0 and math.hypot(target[0] - pose[0], target[1] - pose[1]) < 0.05: + points[-1] = (target[0], target[1], points[-1][2]) + continue + self._append_segment(points, target) + return self._with_segment_yaws(points) + + def _append_segment(self, points, target): + sx, sy, _ = points[-1] + tx, ty = target + length = math.hypot(tx - sx, ty - sy) + if length < 1e-6: + return + steps = max(1, int(math.ceil(length / max(0.02, self.path_resolution)))) + for step in range(1, steps + 1): + ratio = step / steps + yaw = math.atan2(ty - sy, tx - sx) + points.append((sx + (tx - sx) * ratio, sy + (ty - sy) * ratio, yaw)) + + def _with_segment_yaws(self, points): + if len(points) <= 1: + return points + + updated = [] + for index, (x, y, yaw) in enumerate(points): + if index + 1 < len(points): + nx, ny, _ = points[index + 1] + if math.hypot(nx - x, ny - y) > 1e-6: + yaw = math.atan2(ny - y, nx - x) + elif updated: + yaw = updated[-1][2] + updated.append((x, y, yaw)) + return updated + + def _publish_path(self): + path_msg = NavPath() + path_msg.header.frame_id = self.map_frame + path_msg.header.stamp = self.get_clock().now().to_msg() + + for x, y, yaw in self.active_path: + path_msg.poses.append(make_pose(self.map_frame, path_msg.header.stamp, x, y, yaw)) + + self.plan_pub.publish(path_msg) + + def _current_pose(self, compensate=True): + if self.latest_odom is None: + return None + + msg = self.latest_odom + source_frame = msg.header.frame_id or self.map_frame + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + yaw = yaw_from_quaternion(msg.pose.pose.orientation) + vx = msg.twist.twist.linear.x + wz = msg.twist.twist.angular.z + + stamp = Time.from_msg(msg.header.stamp) + if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0: + age = 0.0 + else: + age = (self.get_clock().now() - stamp).nanoseconds * 1e-9 + if age < 0.0: + age = 0.0 + if age > self.max_odom_age: + return None + + if compensate and self.latency_compensation: + horizon = min(age, self.max_latency_compensation) + if abs(wz) < 1e-4: + x += vx * math.cos(yaw) * horizon + y += vx * math.sin(yaw) * horizon + else: + radius = vx / wz + new_yaw = yaw + wz * horizon + x += radius * (math.sin(new_yaw) - math.sin(yaw)) + y -= radius * (math.cos(new_yaw) - math.cos(yaw)) + yaw = normalize_angle(new_yaw) + + if source_frame != self.map_frame: + transformed = self._transform_pose_2d(x, y, yaw, source_frame) + if transformed is None: + return None + x, y, yaw = transformed + + return x, y, yaw, vx, wz + + def _transform_pose_2d(self, x, y, yaw, source_frame): + try: + transform = self.tf_buffer.lookup_transform( + self.map_frame, + source_frame, + Time(), + timeout=Duration(seconds=self.tf_timeout), + ) + except TransformException as exc: + now_ns = self.get_clock().now().nanoseconds + if now_ns - self.last_tf_warn_ns > 2_000_000_000: + self.last_tf_warn_ns = now_ns + self.get_logger().warn( + f"TF unavailable {source_frame}->{self.map_frame}: {exc}" + ) + return None + + t = transform.transform.translation + q = transform.transform.rotation + transform_yaw = yaw_from_quaternion(q) + cos_yaw = math.cos(transform_yaw) + sin_yaw = math.sin(transform_yaw) + target_x = t.x + cos_yaw * x - sin_yaw * y + target_y = t.y + sin_yaw * x + cos_yaw * y + target_yaw = normalize_angle(transform_yaw + yaw) + return target_x, target_y, target_yaw + + def _control_loop(self): + if not self.publish_cmd_vel: + return + + pose = self._current_pose(compensate=True) + if pose is None: + self._stop_if_needed("stop no_recent_odom") + return + + if self.completed or len(self.active_path) < 2: + if self.stop_without_plan: + self._publish_cmd(0.0, 0.0) + return + + x, y, yaw, _, _ = pose + goal_x, goal_y, _ = self.active_path[-1] + goal_distance = math.hypot(goal_x - x, goal_y - y) + if goal_distance <= self.goal_tolerance: + self.completed = True + self._publish_cmd(0.0, 0.0) + self._publish_status(f"goal_reached {self.active_goal_node}") + return + + nearest_index = self._nearest_path_index(x, y) + self.current_path_index = nearest_index + target_index = self._lookahead_index(x, y, nearest_index) + target_x, target_y, target_path_yaw = self.active_path[target_index] + + dx = target_x - x + dy = target_y - y + path_heading = self._segment_heading(target_index, x, y) + reverse = math.cos(normalize_angle(target_path_yaw - path_heading)) < 0.0 + control_yaw = normalize_angle(yaw + math.pi) if reverse else yaw + local_x = math.cos(control_yaw) * dx + math.sin(control_yaw) * dy + local_y = -math.sin(control_yaw) * dx + math.cos(control_yaw) * dy + lookahead = max(0.05, math.hypot(local_x, local_y)) + + curvature = 2.0 * local_y / (lookahead * lookahead) + if self.min_turning_radius > 1e-3: + max_curvature = 1.0 / self.min_turning_radius + curvature = max(-max_curvature, min(max_curvature, curvature)) + + speed = self._target_speed(abs(curvature), goal_distance, reverse) + angular = speed * curvature + angular = max(-self.max_angular_speed, min(self.max_angular_speed, angular)) + self._publish_cmd(speed, angular) + + def _nearest_path_index(self, x, y): + start = max(0, self.current_path_index - 5) + best_index = start + best_dist = float("inf") + for index in range(start, len(self.active_path)): + px, py, _ = self.active_path[index] + dist = (px - x) * (px - x) + (py - y) * (py - y) + if dist < best_dist: + best_index = index + best_dist = dist + return best_index + + def _lookahead_index(self, x, y, start_index): + target_index = len(self.active_path) - 1 + for index in range(start_index, len(self.active_path)): + px, py, _ = self.active_path[index] + if math.hypot(px - x, py - y) >= self.lookahead_distance: + target_index = index + break + return target_index + + def _segment_heading(self, index, current_x=None, current_y=None): + if len(self.active_path) < 2: + return 0.0 + if current_x is not None and current_y is not None: + tx, ty, target_yaw = self.active_path[index] + if math.hypot(tx - current_x, ty - current_y) > 1e-6: + return math.atan2(ty - current_y, tx - current_x) + return target_yaw + + if index <= 0: + start_index = 0 + end_index = 1 + else: + start_index = index - 1 + end_index = index + sx, sy, fallback_yaw = self.active_path[start_index] + tx, ty, _ = self.active_path[end_index] + if math.hypot(tx - sx, ty - sy) <= 1e-6: + return fallback_yaw + return math.atan2(ty - sy, tx - sx) + + def _target_speed(self, abs_curvature, goal_distance, reverse): + base_speed = self.reverse_speed if reverse else self.linear_speed + speed = base_speed + speed *= max(0.35, 1.0 - self.curvature_slowdown_gain * abs_curvature) + if goal_distance < self.slowdown_distance: + ratio = max(0.25, goal_distance / max(0.05, self.slowdown_distance)) + speed *= ratio + speed = max(self.min_linear_speed, min(base_speed, speed)) + return -speed if reverse else speed + + def _publish_cmd(self, linear, angular): + cmd = Twist() + cmd.linear.x = float(linear) + cmd.angular.z = float(angular) + self.cmd_pub.publish(cmd) + + def _stop_if_needed(self, reason): + self._publish_cmd(0.0, 0.0) + self._publish_status(reason) + + def _publish_status(self, text): + msg = String() + msg.data = text + self.status_pub.publish(msg) + self.get_logger().info(text) + + +def main(args=None): + rclpy.init(args=args) + node = None + try: + node = TopologyPurePursuit() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node._publish_cmd(0.0, 0.0) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/planner/src/grid_astar_theta_node.cpp b/src/planner/src/grid_astar_theta_node.cpp new file mode 100644 index 0000000..3b522ab --- /dev/null +++ b/src/planner/src/grid_astar_theta_node.cpp @@ -0,0 +1,11 @@ +#include + +int main() +{ + std::cout << "planner package: Nav2 minimal global planning stack" << std::endl; + std::cout << " - slam_toolbox (online_async)" << std::endl; + std::cout << " - planner_server (SmacPlannerHybrid)" << std::endl; + std::cout << " - global_costmap + local_costmap" << std::endl; + std::cout << " - lifecycle_manager" << std::endl; + return 0; +} diff --git a/src/planner/src/planner_version.cpp b/src/planner/src/planner_version.cpp index dc219bf..d9f04f4 100644 --- a/src/planner/src/planner_version.cpp +++ b/src/planner/src/planner_version.cpp @@ -1,11 +1,11 @@ -#include - -int main(int argc, char* argv[]) -{ - std::cout << "planner package: Nav2 minimal global planning stack" << std::endl; - std::cout << " - slam_toolbox (online_async)" << std::endl; - std::cout << " - planner_server (SmacPlannerHybrid)" << std::endl; - std::cout << " - global_costmap + local_costmap" << std::endl; - std::cout << " - lifecycle_manager" << std::endl; - return 0; -} +#include + +int main(int argc, char* argv[]) +{ + std::cout << "planner package: Nav2 minimal global planning stack" << std::endl; + std::cout << " - slam_toolbox (online_async)" << std::endl; + std::cout << " - planner_server (SmacPlannerHybrid)" << std::endl; + std::cout << " - global_costmap + local_costmap" << std::endl; + std::cout << " - lifecycle_manager" << std::endl; + return 0; +}