feat(freespace_planning_algorithms): add is_back flag into the return…

… of A* python wrapper (autowarefoundation#7831)

add is_back flag to the return of getWaypoints

Signed-off-by: Takumi Ito <[email protected]>
Co-authored-by: Takumi Ito <[email protected]>

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

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<double>({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w}));
+        std::vector<double>({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;