From 1d0d2934ed022daf39e53d76ccc7c2af7901959a Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri, 9 Dec 2022 19:53:51 +0800 Subject: [PATCH] fix(freespace_planning_algorithms): fix rrtstar can't arrive goal error (#2350) Signed-off-by: NorahXiong Signed-off-by: NorahXiong Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Signed-off-by: kminoda --- planning/freespace_planning_algorithms/src/rrtstar.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/freespace_planning_algorithms/src/rrtstar.cpp index b954760406bdc..d710b74111b1a 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar.cpp @@ -150,14 +150,18 @@ void RRTStar::setRRTPath(const std::vector & waypoints) pose.pose = local2global(costmap_, pose_local); pose.header = header; PlannerWaypoint pw; - if (i == waypoints.size() - 1) { - pw.is_back = waypoints_.waypoints.at(i - 1).is_back; - } else { + if (0 == i) { const auto & pt_now = waypoints.at(i); const auto & pt_next = waypoints.at(i + 1); const double inpro = cos(pt_now.yaw) * (pt_next.x - pt_now.x) + sin(pt_now.yaw) * (pt_next.y - pt_now.y); pw.is_back = (inpro < 0.0); + } else { + const auto & pt_pre = waypoints.at(i - 1); + const auto & pt_now = waypoints.at(i); + const double inpro = + cos(pt_pre.yaw) * (pt_now.x - pt_pre.x) + sin(pt_pre.yaw) * (pt_now.y - pt_pre.y); + pw.is_back = !(inpro > 0.0); } pw.pose = pose; waypoints_.waypoints.push_back(pw);