feat(freespace_planning_algorithms): add Astar search by python (autowarefoundation#6398)

TakumIto · Takumi Ito · kosuke55 · karishma1911 · commit bc1ea9f8da53 · 2024-06-03T10:23:24.000+05:30
* feat(freespace_planning_algorithms): add Astar search by python

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

* add copywrite and remove unneeded comment

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

* - 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 &lt;takumi.ito@tier4.jp&gt;

* insert astar python documents into the README

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

* trivial modification

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

* modify typo

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

* Update planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

Co-authored-by: Kosuke Takeuchi &lt;kosuke.tnp@gmail.com&gt;

* ad copyright

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

fix copyright

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

add copyright

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;

* Update planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

---------

Signed-off-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;
Co-authored-by: Takumi Ito &lt;takumi.ito@tier4.jp&gt;
Co-authored-by: Kosuke Takeuchi &lt;kosuke.tnp@gmail.com&gt;
diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt
@@ -2,7 +2,10 @@ cmake_minimum_required(VERSION 3.14)
 project(freespace_planning_algorithms)
 
 find_package(autoware_cmake REQUIRED)
+find_package(python_cmake_module REQUIRED)
+
 autoware_package()
+ament_python_install_package(${PROJECT_NAME})
 
 ament_auto_add_library(reeds_shepp SHARED
   src/reeds_shepp.cpp
@@ -52,3 +55,22 @@ install(PROGRAMS
   test/debug_plot.py
   DESTINATION lib/${PROJECT_NAME}
 )
+
+find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
+find_package(pybind11_vendor REQUIRED)
+find_package(pybind11 REQUIRED)
+
+pybind11_add_module(${PROJECT_NAME}_pybind SHARED
+  scripts/bind/astar_search_pybind.cpp
+)
+
+include_directories(${PROJECT_NAME}_pybind PRIVATE
+  include/
+)
+target_link_libraries(${PROJECT_NAME}_pybind PRIVATE
+  ${PROJECT_NAME}
+)
+
+install(TARGETS ${PROJECT_NAME}_pybind
+  DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+)
diff --git a/planning/freespace_planning_algorithms/README.md b/planning/freespace_planning_algorithms/README.md
@@ -74,6 +74,24 @@ The black cells, green box, and red box, respectively, indicate obstacles,
 start configuration, and goal configuration.
 The sequence of the blue boxes indicate the solution path.
 
+## Extension to Python module (only A\* supported)
+
+There is an implementation of the extension to the python module.
+You can try A\* search via Python by setting follows:
+
+- parameters,
+- costmap,
+- start pose,
+- goal pose.
+
+Then, you can get
+
+- success or failure,
+- searched trajectory.
+
+The example code is [scripts/example/example.py](scripts/example/example.py).
+Note that you need to build this package and source the setup shell script in advance.
+
 ## License notice
 
 Files `src/reeds_shepp.cpp` and `include/astar_search/reeds_shepp.h`
diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py b/planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py
diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
@@ -0,0 +1,64 @@
+# Copyright 2024 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import freespace_planning_algorithms.freespace_planning_algorithms_pybind as _fp
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Quaternion
+from nav_msgs.msg import OccupancyGrid
+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_vector = self.astar_search.getWaypoints()
+        waypoints = PlannerWaypoints()
+
+        waypoints.length = waypoints_vector.length
+        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))
+
+        return waypoints
diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml
@@ -14,6 +14,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>python_cmake_module</buildtool_depend>
 
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
diff --git a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -0,0 +1,167 @@
+// Copyright 2024 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "freespace_planning_algorithms/abstract_algorithm.hpp"
+#include "freespace_planning_algorithms/astar_search.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+struct PlannerWaypointsVector
+{
+  std::vector<std::vector<double>> waypoints;
+  double length;
+};
+
+class AstarSearchPython : public freespace_planning_algorithms::AstarSearch
+{
+  using freespace_planning_algorithms::AstarSearch::AstarSearch;
+
+public:
+  void setMapByte(const std::string & costmap_byte)
+  {
+    rclcpp::SerializedMessage serialized_msg;
+    static constexpr size_t message_header_length = 8u;
+    serialized_msg.reserve(message_header_length + costmap_byte.size());
+    serialized_msg.get_rcl_serialized_message().buffer_length = costmap_byte.size();
+    for (size_t i = 0; i < costmap_byte.size(); ++i) {
+      serialized_msg.get_rcl_serialized_message().buffer[i] = costmap_byte[i];
+    }
+    nav_msgs::msg::OccupancyGrid costmap;
+    static rclcpp::Serialization<nav_msgs::msg::OccupancyGrid> serializer;
+    serializer.deserialize_message(&serialized_msg, &costmap);
+
+    freespace_planning_algorithms::AstarSearch::setMap(costmap);
+  }
+
+  bool makePlanByte(const std::string & start_pose_byte, const std::string & goal_pose_byte)
+  {
+    rclcpp::SerializedMessage serialized_start_msg;
+    static constexpr size_t message_header_length = 8u;
+    serialized_start_msg.reserve(message_header_length + start_pose_byte.size());
+    serialized_start_msg.get_rcl_serialized_message().buffer_length = start_pose_byte.size();
+    for (size_t i = 0; i < start_pose_byte.size(); ++i) {
+      serialized_start_msg.get_rcl_serialized_message().buffer[i] = start_pose_byte[i];
+    }
+    geometry_msgs::msg::Pose start_pose;
+    static rclcpp::Serialization<geometry_msgs::msg::Pose> start_serializer;
+    start_serializer.deserialize_message(&serialized_start_msg, &start_pose);
+
+    rclcpp::SerializedMessage serialized_goal_msg;
+    serialized_goal_msg.reserve(message_header_length + goal_pose_byte.size());
+    serialized_goal_msg.get_rcl_serialized_message().buffer_length = goal_pose_byte.size();
+    for (size_t i = 0; i < goal_pose_byte.size(); ++i) {
+      serialized_goal_msg.get_rcl_serialized_message().buffer[i] = goal_pose_byte[i];
+    }
+    geometry_msgs::msg::Pose goal_pose;
+    static rclcpp::Serialization<geometry_msgs::msg::Pose> goal_serializer;
+    goal_serializer.deserialize_message(&serialized_goal_msg, &goal_pose);
+
+    return freespace_planning_algorithms::AstarSearch::makePlan(start_pose, goal_pose);
+  }
+
+  PlannerWaypointsVector getWaypointsAsVector()
+  {
+    freespace_planning_algorithms::PlannerWaypoints waypoints =
+      freespace_planning_algorithms::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_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_<freespace_planning_algorithms::AstarParam>(p, "AstarParam", py::dynamic_attr())
+      .def(py::init<>())
+      .def_readwrite(
+        "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions)
+      .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back)
+      .def_readwrite(
+        "distance_heuristic_weight",
+        &freespace_planning_algorithms::AstarParam::distance_heuristic_weight);
+  auto pyPlannerCommonParam =
+    py::class_<freespace_planning_algorithms::PlannerCommonParam>(
+      p, "PlannerCommonParam", py::dynamic_attr())
+      .def(py::init<>())
+      .def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit)
+      .def_readwrite(
+        "minimum_turning_radius",
+        &freespace_planning_algorithms::PlannerCommonParam::minimum_turning_radius)
+      .def_readwrite(
+        "maximum_turning_radius",
+        &freespace_planning_algorithms::PlannerCommonParam::maximum_turning_radius)
+      .def_readwrite(
+        "turning_radius_size",
+        &freespace_planning_algorithms::PlannerCommonParam::turning_radius_size)
+      .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)
+      .def_readwrite(
+        "curve_weight", &freespace_planning_algorithms::PlannerCommonParam::curve_weight)
+      .def_readwrite(
+        "reverse_weight", &freespace_planning_algorithms::PlannerCommonParam::reverse_weight)
+      .def_readwrite(
+        "lateral_goal_range",
+        &freespace_planning_algorithms::PlannerCommonParam::lateral_goal_range)
+      .def_readwrite(
+        "longitudinal_goal_range",
+        &freespace_planning_algorithms::PlannerCommonParam::longitudinal_goal_range)
+      .def_readwrite(
+        "angle_goal_range", &freespace_planning_algorithms::PlannerCommonParam::angle_goal_range)
+      .def_readwrite(
+        "obstacle_threshold",
+        &freespace_planning_algorithms::PlannerCommonParam::obstacle_threshold);
+  auto pyVehicleShape =
+    py::class_<freespace_planning_algorithms::VehicleShape>(p, "VehicleShape", py::dynamic_attr())
+      .def(py::init<>())
+      .def(py::init<double, double, double>())
+      .def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length)
+      .def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width)
+      .def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back);
+
+  py::class_<freespace_planning_algorithms::AbstractPlanningAlgorithm>(
+    p, "AbstractPlanningAlgorithm");
+  py::class_<
+    freespace_planning_algorithms::AstarSearch,
+    freespace_planning_algorithms::AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
+  py::class_<AstarSearchPython, freespace_planning_algorithms::AstarSearch>(p, "AstarSearch")
+    .def(py::init<
+         freespace_planning_algorithms::PlannerCommonParam &,
+         freespace_planning_algorithms::VehicleShape &,
+         freespace_planning_algorithms::AstarParam &>())
+    .def("setMap", &AstarSearchPython::setMapByte)
+    .def("makePlan", &AstarSearchPython::makePlanByte)
+    .def("getWaypoints", &AstarSearchPython::getWaypointsAsVector);
+}
diff --git a/planning/freespace_planning_algorithms/scripts/example/example.py b/planning/freespace_planning_algorithms/scripts/example/example.py
@@ -0,0 +1,93 @@
+# Copyright 2024 TIER IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import freespace_planning_algorithms.astar_search as fp
+from geometry_msgs.msg import Pose
+from nav_msgs.msg import OccupancyGrid
+from pyquaternion import Quaternion
+
+# -- Vehicle Shape --
+vehicle_shape = fp.VehicleShape()
+vehicle_shape.length = 2.0
+vehicle_shape.width = 1.0
+vehicle_shape.base2back = 1.0
+
+
+# -- Planner Common Parameter --
+planner_param = fp.PlannerCommonParam()
+# base configs
+planner_param.time_limit = 30000.0
+planner_param.minimum_turning_radius = 9.0
+planner_param.maximum_turning_radius = 9.0
+planner_param.turning_radius_size = 1
+# search configs
+planner_param.theta_size = 144
+planner_param.angle_goal_range = 6.0
+planner_param.curve_weight = 1.2
+planner_param.reverse_weight = 2.0
+planner_param.lateral_goal_range = 0.5
+planner_param.longitudinal_goal_range = 2.0
+# costmap configs
+planner_param.obstacle_threshold = 100
+
+
+# -- A* search Configurations --
+astar_param = fp.AstarParam()
+astar_param.only_behind_solutions = False
+astar_param.use_back = True
+astar_param.distance_heuristic_weight = 1.0
+
+astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)
+
+
+# -- Costmap Definition
+size = 350
+resolution = 0.2
+
+costmap = OccupancyGrid()
+costmap.info.resolution = resolution
+costmap.info.height = size
+costmap.info.width = size
+costmap.info.origin.position.x = -size * resolution / 2
+costmap.info.origin.position.y = -size * resolution / 2
+costmap.data = [0 for i in range(size**2)]
+
+astar.setMap(costmap)
+
+
+# -- Start and Goal Pose
+start_pose = Pose()
+goal_pose = Pose()
+
+start_pose.position.x = 0.0
+start_pose.position.y = 0.0
+
+goal_pose.position.x = 10.0
+goal_pose.position.y = 0.0
+
+yaw = 0
+quaternion = Quaternion(axis=[0, 0, 1], angle=yaw)
+
+goal_pose.orientation.w = quaternion.w
+goal_pose.orientation.x = quaternion.x
+goal_pose.orientation.y = quaternion.y
+goal_pose.orientation.z = quaternion.z
+
+
+# -- Search --
+if astar.makePlan(start_pose, goal_pose):
+    print("Success to find path.")
+    print("  Path length:", astar.getWaypoints().length)
+else:
+    print("Fail to find path.")
