- Add method getWaypoints() into the astar class.

- No need to convert to serial when user passes ros2 message to methods.

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>

Author: Takumi Ito

planning/freespace_planning_algorithms/CMakeLists.txt

@@ -60,17 +60,17 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
 find_package(pybind11_vendor REQUIRED)
 find_package(pybind11 REQUIRED)
 
-pybind11_add_module(freespace_planning_algorithms_python SHARED
+pybind11_add_module(${PROJECT_NAME}_pybind SHARED
   scripts/bind/astar_search_pybind.cpp
 )
 
-include_directories(freespace_planning_algorithms_python PRIVATE
+include_directories(${PROJECT_NAME}_pybind PRIVATE
   include/
 )
-target_link_libraries(freespace_planning_algorithms_python PRIVATE
-  freespace_planning_algorithms
+target_link_libraries(${PROJECT_NAME}_pybind PRIVATE
+  ${PROJECT_NAME}
 )
 
-install(TARGETS freespace_planning_algorithms_python
+install(TARGETS ${PROJECT_NAME}_pybind
   DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
 )

planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py

@@ -0,0 +1,42 @@
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import Pose, Point, Quaternion
+
+import freespace_planning_algorithms.freespace_planning_algorithms_pybind as _fp
+from rclpy.serialization import serialize_message
+
+PlannerCommonParam = _fp.PlannerCommonParam
+VehicleShape = _fp.VehicleShape
+AstarParam = _fp.AstarParam
+
+class PlannerWaypoints:
+    def __init__(self):
+        self.waypoints = []
+        self.length = 0.0
+
+    def compute_length(self):
+        return self.length
+
+class AstarSearch:
+    def __init__(self, planner_param: PlannerCommonParam, vehicle_shape: VehicleShape, astar_param: AstarParam):
+        self.astar_search = _fp.AstarSearch(planner_param, vehicle_shape, astar_param)
+
+    def setMap(self, costmap: OccupancyGrid):
+        costmap_byte = serialize_message(costmap)
+        self.astar_search.setMap(costmap_byte)
+
+    def makePlan(self, start_pose: Pose, goal_pose: Pose):
+        start_pose_byte = serialize_message(start_pose)
+        goal_pose_byte = serialize_message(goal_pose)
+        return self.astar_search.makePlan(start_pose_byte, goal_pose_byte)
+    
+    def getWaypoints(self):
+        waypoints_vetor = self.astar_search.getWaypoints()
+        waypoints = PlannerWaypoints()
+        
+        waypoints.length = waypoints_vetor.length
+        for waypoint in waypoints_vetor.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))
+        
+        return waypoints

planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

@@ -22,9 +22,17 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
 using namespace freespace_planning_algorithms;
 
+struct PlannerWaypointsVector
+{
+  std::vector<std::vector<double>> waypoints;
+  double length;
+};
+
+
 class AstarSearchPython : public AstarSearch
 {
   using AstarSearch::AstarSearch;
@@ -71,12 +79,33 @@ class AstarSearchPython : public AstarSearch
 
     return AstarSearch::makePlan(start_pose, goal_pose);
   }
+
+  PlannerWaypointsVector getWaypointsAsVector()
+  {
+    PlannerWaypoints waypoints = AstarSearch::getWaypoints();
+    PlannerWaypointsVector waypoints_vector;
+    for (const auto& waypoint : waypoints.waypoints)
+    {
+      // NOTE: it was difficult to return the deserialized pose_byte and serialize the pose_byte on
+      // 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;
+      waypoints_vector.waypoints.push_back(std::vector<double>({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w}));
+    }
+    waypoints_vector.length = waypoints.compute_length();
+    return waypoints_vector;
+  }
 };
 
 namespace py = pybind11;
 
-PYBIND11_MODULE(freespace_planning_algorithms_python, p)
+PYBIND11_MODULE(freespace_planning_algorithms_pybind, p)
 {
+  auto pyPlannerWaypointsVector =
+    py::class_<PlannerWaypointsVector>(p, "PlannerWaypointsVector", py::dynamic_attr())
+      .def(py::init<>())
+      .def_readwrite("waypoints", &PlannerWaypointsVector::waypoints)
+      .def_readwrite("length", &PlannerWaypointsVector::length);
   auto pyAstarParam =
     py::class_<AstarParam>(p, "AstarParam", py::dynamic_attr())
       .def(py::init<>())
@@ -97,17 +126,19 @@ PYBIND11_MODULE(freespace_planning_algorithms_python, p)
       .def_readwrite("longitudinal_goal_range", &PlannerCommonParam::longitudinal_goal_range)
       .def_readwrite("angle_goal_range", &PlannerCommonParam::angle_goal_range)
       .def_readwrite("obstacle_threshold", &PlannerCommonParam::obstacle_threshold);
-  auto pyVSehicleShape = py::class_<VehicleShape>(p, "VehicleShape", py::dynamic_attr())
-                           .def(py::init<>())
-                           .def(py::init<double, double, double>())
-                           .def_readwrite("length", &VehicleShape::length)
-                           .def_readwrite("width", &VehicleShape::width)
-                           .def_readwrite("base2back", &VehicleShape::base2back);
+  auto pyVSehicleShape = 
+    py::class_<VehicleShape>(p, "VehicleShape", py::dynamic_attr())
+      .def(py::init<>())
+      .def(py::init<double, double, double>())
+      .def_readwrite("length", &VehicleShape::length)
+      .def_readwrite("width", &VehicleShape::width)
+      .def_readwrite("base2back", &VehicleShape::base2back);
 
   py::class_<AbstractPlanningAlgorithm>(p, "AbstractPlanningAlgorithm");
   py::class_<AstarSearch, AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
   py::class_<AstarSearchPython, AstarSearch>(p, "AstarSearch")
     .def(py::init<PlannerCommonParam &, VehicleShape &, AstarParam &>())
     .def("setMap", &AstarSearchPython::setMapByte)
-    .def("makePlan", &AstarSearchPython::makePlanByte);
+    .def("makePlan", &AstarSearchPython::makePlanByte)
+    .def("getWaypoints", &AstarSearchPython::getWaypointsAsVector);
 }

planning/freespace_planning_algorithms/scripts/example/example.py

@@ -1,8 +1,7 @@
-import freespace_planning_algorithms.freespace_planning_algorithms_python as fp
+import freespace_planning_algorithms.astar_search as fp
 from geometry_msgs.msg import Pose
 from nav_msgs.msg import OccupancyGrid
 from pyquaternion import Quaternion
-from rclpy.serialization import serialize_message
 
 # -- Vehicle Shape --
 vehicle_shape = fp.VehicleShape()
@@ -50,7 +49,7 @@
 costmap.info.origin.position.y = -size * resolution / 2
 costmap.data = [0 for i in range(size**2)]
 
-astar.setMap(serialize_message(costmap))
+astar.setMap(costmap)
 
 
 # -- Start and Goal Pose
@@ -73,7 +72,8 @@
 
 
 # -- Search --
-if astar.makePlan(serialize_message(start_pose), serialize_message(goal_pose)):
+if astar.makePlan(start_pose, goal_pose):
     print("Success to find path.")
+    print("  Path length:", astar.getWaypoints().length)
 else:
     print("Fail to find path.")