diff --git a/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py index 2ffa20150fb0c..7b03a4024dc1b 100644 --- a/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py +++ b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py @@ -24,6 +24,12 @@ AstarParam = _fp.AstarParam +class PlannerWaypoint: + def __init__(self, pose, is_back): + self.pose = pose + self.is_back = is_back + + class PlannerWaypoints: def __init__(self): self.waypoints = [] @@ -59,6 +65,9 @@ def getWaypoints(self): for waypoint in waypoints_vector.waypoints: pos = Point(x=waypoint[0], y=waypoint[1], z=waypoint[2]) quat = Quaternion(x=waypoint[3], y=waypoint[4], z=waypoint[5], w=waypoint[6]) - waypoints.waypoints.append(Pose(position=pos, orientation=quat)) + is_bask = bool(waypoint[7]) + waypoints.waypoints.append( + PlannerWaypoint(Pose(position=pos, orientation=quat), is_bask) + ) return waypoints diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index f557926806382..8932fb277abe8 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -89,8 +89,9 @@ class AstarSearchPython : public freespace_planning_algorithms::AstarSearch // python-side. So this function returns [*position, *quaternion] as double array const auto & xyz = waypoint.pose.pose.position; const auto & quat = waypoint.pose.pose.orientation; + const double & is_back = waypoint.is_back; waypoints_vector.waypoints.push_back( - std::vector({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w})); + std::vector({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w, is_back})); } waypoints_vector.length = waypoints.compute_length(); return waypoints_vector;