From d0c9268cc606b7287b5dad7281e09b9d5906c246 Mon Sep 17 00:00:00 2001 From: Takumi Ito Date: Mon, 5 Feb 2024 13:53:06 +0900 Subject: [PATCH] success to use AstarSearch::setMap and makePlan Signed-off-by: Takumi Ito --- .gitmodules | 3 - .../freespace_planning_algorithms/pybind11 | 1 - .../scripts/module/astar_search_python.cpp | 112 ++++++++++++++---- .../scripts/parameter_tuning_tools/test.py | 58 ++++++++- 4 files changed, 145 insertions(+), 29 deletions(-) delete mode 160000 planning/freespace_planning_algorithms/pybind11 diff --git a/.gitmodules b/.gitmodules index 92898c156b62d..e69de29bb2d1d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "planning/freespace_planning_algorithms/pybind11"] - path = planning/freespace_planning_algorithms/pybind11 - url = git@github.com:pybind/pybind11.git diff --git a/planning/freespace_planning_algorithms/pybind11 b/planning/freespace_planning_algorithms/pybind11 deleted file mode 160000 index aec6cc5406edb..0000000000000 --- a/planning/freespace_planning_algorithms/pybind11 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit aec6cc5406edb076f5a489c2d7f84bb07052c4a3 diff --git a/planning/freespace_planning_algorithms/scripts/module/astar_search_python.cpp b/planning/freespace_planning_algorithms/scripts/module/astar_search_python.cpp index c3b7a4f6e1407..f1106d0621d6c 100644 --- a/planning/freespace_planning_algorithms/scripts/module/astar_search_python.cpp +++ b/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 +#include + +#include +#include + + #include +// #include +// #include +// #include +// #include +// #include +// #include -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 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 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 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_(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_(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_(p, "VehicleShape", py::dynamic_attr()) - .def(py::init<>()) - .def(py::init()) - .def_readwrite("length", &VehicleShape::length) - .def_readwrite("width", &VehicleShape::width) - .def_readwrite("base2back", &VehicleShape::base2back); + .def(py::init<>()) + .def(py::init()) + .def_readwrite("length", &VehicleShape::length) + .def_readwrite("width", &VehicleShape::width) + .def_readwrite("base2back", &VehicleShape::base2back); py::class_(p, "AbstractPlanningAlgorithm"); - py::class_(p, "AstarSearch") - .def(py::init()); + py::class_(p, "AstarSearchCpp"); + py::class_(p, "AstarSearch") + .def(py::init()) + .def("setMap", &AstarSearchPython::setMapByte) + .def("makePlan", &AstarSearchPython::makePlanByte); } diff --git a/planning/freespace_planning_algorithms/scripts/parameter_tuning_tools/test.py b/planning/freespace_planning_algorithms/scripts/parameter_tuning_tools/test.py index 94075a2e0a1e1..fff6f07a5d1be 100644 --- a/planning/freespace_planning_algorithms/scripts/parameter_tuning_tools/test.py +++ b/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) \ No newline at end of file +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") \ No newline at end of file