From 4072f020e05d97f962217c183dfd31b6ab3d8d87 Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Wed, 14 Feb 2024 13:41:05 +0900
Subject: [PATCH 1/9] feat(freespace_planning_algorithms): add Astar search by
 python

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
---
 .../CMakeLists.txt                            |  22 ++++
 .../freespace_planning_algorithms/__init__.py |   0
 .../freespace_planning_algorithms/package.xml |   3 +-
 .../scripts/bind/astar_search_pybind.cpp      | 103 ++++++++++++++++++
 .../scripts/example/example.py                |  83 ++++++++++++++
 5 files changed, 210 insertions(+), 1 deletion(-)
 create mode 100644 planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py
 create mode 100644 planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
 create mode 100644 planning/freespace_planning_algorithms/scripts/example/example.py

diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt
index d5a9d32db1817..1dd9c66a22633 100644
--- 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(freespace_planning_algorithms_python SHARED
+  scripts/bind/astar_search_pybind.cpp
+)
+
+include_directories(freespace_planning_algorithms_python PRIVATE
+  include/
+)
+target_link_libraries(freespace_planning_algorithms_python PRIVATE
+  freespace_planning_algorithms
+)
+
+install(TARGETS freespace_planning_algorithms_python
+  DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+)
diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py b/planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py
new file mode 100644
index 0000000000000..e69de29bb2d1d
diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml
index 2b20a935c4ae1..a9fb8995a29f7 100644
--- a/planning/freespace_planning_algorithms/package.xml
+++ b/planning/freespace_planning_algorithms/package.xml
@@ -14,7 +14,8 @@
 
   <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>
   <depend>nlohmann-json-dev</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
new file mode 100644
index 0000000000000..357466047f25f
--- /dev/null
+++ b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -0,0 +1,103 @@
+#include "freespace_planning_algorithms/astar_search.hpp"
+#include "freespace_planning_algorithms/abstract_algorithm.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>
+// #include <pybind11/stl_bind.h>
+// #include <pybind11/eigen.h>
+// #include <pybind11/chrono.h>
+// #include <pybind11/complex.h>
+// #include <pybind11/functional.h>
+
+using namespace freespace_planning_algorithms;
+
+class AstarSearchPython : public AstarSearch
+{
+    using 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);
+
+            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 AstarSearch::makePlan(start_pose, goal_pose);
+        }
+};
+
+
+namespace py = pybind11;
+
+PYBIND11_MODULE(freespace_planning_algorithms_python, p)
+{
+    auto pyAstarParam = py::class_<AstarParam>(p, "AstarParam", py::dynamic_attr())
+        .def(py::init<>())
+        .def_readwrite("only_behind_solutions", &AstarParam::only_behind_solutions)
+        .def_readwrite("use_back", &AstarParam::use_back)
+        .def_readwrite("distance_heuristic_weight", &AstarParam::distance_heuristic_weight);
+    auto pyPlannerCommonParam = py::class_<PlannerCommonParam>(p, "PlannerCommonParam", py::dynamic_attr())
+        .def(py::init<>())
+        .def_readwrite("time_limit", &PlannerCommonParam::time_limit)
+        .def_readwrite("minimum_turning_radius", &PlannerCommonParam::minimum_turning_radius)
+        .def_readwrite("maximum_turning_radius", &PlannerCommonParam::maximum_turning_radius)
+        .def_readwrite("turning_radius_size", &PlannerCommonParam::turning_radius_size)
+        .def_readwrite("theta_size", &PlannerCommonParam::theta_size)
+        .def_readwrite("curve_weight", &PlannerCommonParam::curve_weight)
+        .def_readwrite("reverse_weight", &PlannerCommonParam::reverse_weight)
+        .def_readwrite("lateral_goal_range", &PlannerCommonParam::lateral_goal_range)
+        .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);
+
+    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);
+}
diff --git a/planning/freespace_planning_algorithms/scripts/example/example.py b/planning/freespace_planning_algorithms/scripts/example/example.py
new file mode 100644
index 0000000000000..517cda7dd59cb
--- /dev/null
+++ b/planning/freespace_planning_algorithms/scripts/example/example.py
@@ -0,0 +1,83 @@
+import freespace_planning_algorithms.freespace_planning_algorithms_python as fp
+
+from pyquaternion import Quaternion
+
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import Pose
+from rclpy.serialization import serialize_message
+
+
+# -- 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(serialize_message(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
+quaterinon = Quaternion(axis=[0, 0, 1], angle=yaw)
+
+goal_pose.orientation.w = quaterinon.w
+goal_pose.orientation.x = quaterinon.x
+goal_pose.orientation.y = quaterinon.y
+goal_pose.orientation.z = quaterinon.z
+
+
+# -- Search --
+if astar.makePlan(serialize_message(start_pose),\
+                  serialize_message(goal_pose)):
+    print('Success to find path.')
+else:
+    print('Fail to find path.')
\ No newline at end of file

From c1667d77499f79c37a13b0ec3e0230e8aef712dd Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Mon, 19 Feb 2024 09:21:35 +0900
Subject: [PATCH 2/9] add copywrite and remove unneeded comment

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
---
 .../scripts/bind/astar_search_pybind.cpp      | 21 ++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
index 357466047f25f..68c2d0db34b30 100644
--- a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
+++ b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -1,3 +1,17 @@
+// 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/astar_search.hpp"
 #include "freespace_planning_algorithms/abstract_algorithm.hpp"
 #include <rclcpp/rclcpp.hpp>
@@ -6,14 +20,7 @@
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
-
 #include <pybind11/pybind11.h>
-// #include <pybind11/stl.h>
-// #include <pybind11/stl_bind.h>
-// #include <pybind11/eigen.h>
-// #include <pybind11/chrono.h>
-// #include <pybind11/complex.h>
-// #include <pybind11/functional.h>
 
 using namespace freespace_planning_algorithms;
 

From 0f9457651d879963825facf97a72715d13ce59dc Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Mon, 19 Feb 2024 09:37:32 +0900
Subject: [PATCH 3/9] - 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>
---
 .../CMakeLists.txt                            |  10 +-
 .../astar_search.py                           |  50 ++++
 .../freespace_planning_algorithms/package.xml |   2 +-
 .../scripts/bind/astar_search_pybind.cpp      | 217 +++++++++++-------
 .../scripts/example/example.py                |  54 ++---
 5 files changed, 218 insertions(+), 115 deletions(-)
 create mode 100644 planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py

diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt
index 1dd9c66a22633..726aa47f7a5e5 100644
--- a/planning/freespace_planning_algorithms/CMakeLists.txt
+++ b/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}"
 )
diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
new file mode 100644
index 0000000000000..0ba2b7de76bc3
--- /dev/null
+++ b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
@@ -0,0 +1,50 @@
+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_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
diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml
index a9fb8995a29f7..2fb3e066f0bdd 100644
--- a/planning/freespace_planning_algorithms/package.xml
+++ b/planning/freespace_planning_algorithms/package.xml
@@ -15,7 +15,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>
   <depend>nlohmann-json-dev</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
index 68c2d0db34b30..b3e1cbb663428 100644
--- a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
+++ b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc. All rights reserved.
+// 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.
@@ -12,8 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "freespace_planning_algorithms/astar_search.hpp"
 #include "freespace_planning_algorithms/abstract_algorithm.hpp"
+#include "freespace_planning_algorithms/astar_search.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/serialization.hpp>
 
@@ -21,90 +22,146 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
 
-using namespace freespace_planning_algorithms;
-
-class AstarSearchPython : public AstarSearch
+struct PlannerWaypointsVector
 {
-    using 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);
-
-            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 AstarSearch::makePlan(start_pose, goal_pose);
-        }
+  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_python, p)
+PYBIND11_MODULE(freespace_planning_algorithms_pybind, p)
 {
-    auto pyAstarParam = py::class_<AstarParam>(p, "AstarParam", py::dynamic_attr())
-        .def(py::init<>())
-        .def_readwrite("only_behind_solutions", &AstarParam::only_behind_solutions)
-        .def_readwrite("use_back", &AstarParam::use_back)
-        .def_readwrite("distance_heuristic_weight", &AstarParam::distance_heuristic_weight);
-    auto pyPlannerCommonParam = py::class_<PlannerCommonParam>(p, "PlannerCommonParam", py::dynamic_attr())
-        .def(py::init<>())
-        .def_readwrite("time_limit", &PlannerCommonParam::time_limit)
-        .def_readwrite("minimum_turning_radius", &PlannerCommonParam::minimum_turning_radius)
-        .def_readwrite("maximum_turning_radius", &PlannerCommonParam::maximum_turning_radius)
-        .def_readwrite("turning_radius_size", &PlannerCommonParam::turning_radius_size)
-        .def_readwrite("theta_size", &PlannerCommonParam::theta_size)
-        .def_readwrite("curve_weight", &PlannerCommonParam::curve_weight)
-        .def_readwrite("reverse_weight", &PlannerCommonParam::reverse_weight)
-        .def_readwrite("lateral_goal_range", &PlannerCommonParam::lateral_goal_range)
-        .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);
-
-    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);
+  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 pyVSehicleShape =
+    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
index 517cda7dd59cb..81b4928486ae3 100644
--- a/planning/freespace_planning_algorithms/scripts/example/example.py
+++ b/planning/freespace_planning_algorithms/scripts/example/example.py
@@ -1,13 +1,9 @@
-import freespace_planning_algorithms.freespace_planning_algorithms_python as fp
-
-from pyquaternion import Quaternion
-
-from nav_msgs.msg import OccupancyGrid
+import freespace_planning_algorithms.astar_search as fp
 from geometry_msgs.msg import Pose
-from rclpy.serialization import serialize_message
-
+from nav_msgs.msg import OccupancyGrid
+from pyquaternion import Quaternion
 
-# -- Vehicle Shape -- 
+# -- Vehicle Shape --
 vehicle_shape = fp.VehicleShape()
 vehicle_shape.length = 2.0
 vehicle_shape.width = 1.0
@@ -17,26 +13,26 @@
 # -- 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
+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
+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
+planner_param.obstacle_threshold = 100
 
 
 # -- A* search Configurations --
 astar_param = fp.AstarParam()
-astar_param.only_behind_solutions= False
+astar_param.only_behind_solutions = False
 astar_param.use_back = True
-astar_param.distance_heuristic_weight= 1.0
+astar_param.distance_heuristic_weight = 1.0
 
 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)
 
@@ -49,11 +45,11 @@
 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) ]
+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(serialize_message(costmap))
+astar.setMap(costmap)
 
 
 # -- Start and Goal Pose
@@ -76,8 +72,8 @@
 
 
 # -- Search --
-if astar.makePlan(serialize_message(start_pose),\
-                  serialize_message(goal_pose)):
-    print('Success to find path.')
+if astar.makePlan(start_pose, goal_pose):
+    print("Success to find path.")
+    print("  Path length:", astar.getWaypoints().length)
 else:
-    print('Fail to find path.')
\ No newline at end of file
+    print("Fail to find path.")

From 541b10dc8e7bd3f3e9e4b4cdf70b8124351b1743 Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Thu, 4 Apr 2024 10:04:53 +0900
Subject: [PATCH 4/9] insert astar python documents into the README

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
---
 .../freespace_planning_algorithms/README.md    | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/planning/freespace_planning_algorithms/README.md b/planning/freespace_planning_algorithms/README.md
index 7069a1c0fabfc..8bd015589d58d 100644
--- 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`

From 539537a2c2ee75fc0b34366bc942254d09614a73 Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Thu, 4 Apr 2024 10:23:20 +0900
Subject: [PATCH 5/9] trivial modification

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
---
 .../freespace_planning_algorithms/astar_search.py           | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
index 0ba2b7de76bc3..444474e1e3378 100644
--- a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
+++ b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
@@ -38,11 +38,11 @@ def makePlan(self, start_pose: Pose, goal_pose: Pose):
         return self.astar_search.makePlan(start_pose_byte, goal_pose_byte)
 
     def getWaypoints(self):
-        waypoints_vetor = self.astar_search.getWaypoints()
+        waypoints_vector = self.astar_search.getWaypoints()
         waypoints = PlannerWaypoints()
 
-        waypoints.length = waypoints_vetor.length
-        for waypoint in waypoints_vetor.waypoints:
+        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))

From 5dab1ef6e9cec701d23eb30946639e4a6f26f118 Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Tue, 16 Apr 2024 10:29:43 +0900
Subject: [PATCH 6/9] modify typo

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
---
 .../scripts/example/example.py                         | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/planning/freespace_planning_algorithms/scripts/example/example.py b/planning/freespace_planning_algorithms/scripts/example/example.py
index 81b4928486ae3..b7d0cc0f018c8 100644
--- a/planning/freespace_planning_algorithms/scripts/example/example.py
+++ b/planning/freespace_planning_algorithms/scripts/example/example.py
@@ -63,12 +63,12 @@
 goal_pose.position.y = 0.0
 
 yaw = 0
-quaterinon = Quaternion(axis=[0, 0, 1], angle=yaw)
+quaternion = Quaternion(axis=[0, 0, 1], angle=yaw)
 
-goal_pose.orientation.w = quaterinon.w
-goal_pose.orientation.x = quaterinon.x
-goal_pose.orientation.y = quaterinon.y
-goal_pose.orientation.z = quaterinon.z
+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 --

From 74e4ae7ef69cd6c814eb0751e21f3868c7cab45a Mon Sep 17 00:00:00 2001
From: TakumIto <61740530+TakumIto@users.noreply.github.com>
Date: Tue, 16 Apr 2024 13:14:02 +0900
Subject: [PATCH 7/9] Update
 planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
---
 .../scripts/bind/astar_search_pybind.cpp                        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
index b3e1cbb663428..cfcaa27c9f230 100644
--- a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
+++ b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -143,7 +143,7 @@ PYBIND11_MODULE(freespace_planning_algorithms_pybind, p)
       .def_readwrite(
         "obstacle_threshold",
         &freespace_planning_algorithms::PlannerCommonParam::obstacle_threshold);
-  auto pyVSehicleShape =
+  auto pyVehicleShape =
     py::class_<freespace_planning_algorithms::VehicleShape>(p, "VehicleShape", py::dynamic_attr())
       .def(py::init<>())
       .def(py::init<double, double, double>())

From d8af308734176e093c91930a1ff983abe6dc3755 Mon Sep 17 00:00:00 2001
From: Takumi Ito <takumi.ito@tier4.jp>
Date: Wed, 17 Apr 2024 10:30:17 +0900
Subject: [PATCH 8/9] ad copyright

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

fix copyright

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

add copyright

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
---
 .../freespace_planning_algorithms/astar_search.py  | 14 ++++++++++++++
 .../scripts/example/example.py                     | 14 ++++++++++++++
 2 files changed, 28 insertions(+)

diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
index 444474e1e3378..c9dcc351e3ce2 100644
--- a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
+++ b/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py
@@ -1,3 +1,17 @@
+# 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
diff --git a/planning/freespace_planning_algorithms/scripts/example/example.py b/planning/freespace_planning_algorithms/scripts/example/example.py
index b7d0cc0f018c8..73f53e23bc540 100644
--- a/planning/freespace_planning_algorithms/scripts/example/example.py
+++ b/planning/freespace_planning_algorithms/scripts/example/example.py
@@ -1,3 +1,17 @@
+# 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 54fff6dacfd1d52eb4bc3e45571c9f802e529f7a Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Wed, 24 Apr 2024 20:22:37 +0900
Subject: [PATCH 9/9] Update
 planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

---
 .../scripts/bind/astar_search_pybind.cpp                    | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
index cfcaa27c9f230..9d10cf311fb16 100644
--- a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
+++ b/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -1,11 +1,11 @@
 // 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.