success to use AstarSearch::setMap and makePlan

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

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "planning/freespace_planning_algorithms/pybind11"]
-	path = planning/freespace_planning_algorithms/pybind11
-	url = [email protected]:pybind/pybind11.git

planning/freespace_planning_algorithms/scripts/module/astar_search_python.cpp

@@ -1,39 +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>
 
-namespace py = pybind11;
 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);
+        .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_tuning_radius", &PlannerCommonParam::time_limit)
-            .def_readwrite("maximum_tuning_radius", &PlannerCommonParam::time_limit)
-            .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);
+        .def(py::init<>())
+        .def_readwrite("time_limit", &PlannerCommonParam::time_limit)
+        .def_readwrite("minimum_tuning_radius", &PlannerCommonParam::time_limit)
+        .def_readwrite("maximum_tuning_radius", &PlannerCommonParam::time_limit)
+        .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);
+        .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, "AstarSearch")
-    .def(py::init<PlannerCommonParam &, VehicleShape &, AstarParam &>());
+    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);
 }

planning/freespace_planning_algorithms/scripts/parameter_tuning_tools/test.py

@@ -3,11 +3,67 @@
 # sys.path.append("/home/takumiito/pilot-auto.freespace_param_tuning/build/freespace_planning_algorithms")
 
 import freespace_planning_algorithms.freespace_planning_algorithms_python as fp
+import numpy as np
+from pyquaternion import Quaternion
+
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import Pose
+from rclpy.serialization import serialize_message
 
 vehicle_shape = fp.VehicleShape(3, 2, 1)
 astar_param = fp.AstarParam()
 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.only_behind_solutions= False
+astar_param.distance_heuristic_weight= 1.0
+
 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)
 
-print(astar)
+costmap = OccupancyGrid()
+costmap.info.resolution = 0.2
+costmap.info.height = 350
+costmap.info.width = 350
+costmap.data = [0 for i in range(350*350)]
+costmap_byte = serialize_message(costmap)
+
+astar.setMap(costmap_byte)
+
+start_pose = Pose()
+goal_pose = Pose()
+
+for x in range(-10,10):
+    for y in range(-10,10):
+        for yaw in range(10):
+            # start_pose.position.x = 1.0
+            # start_pose.orientation.y = 1.0
+            goal_pose.position.x = float(x)
+            goal_pose.position.y = float(y)
+
+            quaterinon = Quaternion(axis=[0, 0, 1], angle=2*np.pi*(yaw/10))
+            goal_pose.orientation.w = quaterinon.w
+            goal_pose.orientation.x = quaterinon.x
+            goal_pose.orientation.y = quaterinon.y
+            goal_pose.orientation.z = quaterinon.z
+
+            start_pose_byte = serialize_message(start_pose)
+            goal_pose_byte = serialize_message(goal_pose)
+
+            if astar.makePlan(start_pose_byte, goal_pose_byte):
+                print("success")