From 76cf36820c41b11b93847934411cd92f061de5a0 Mon Sep 17 00:00:00 2001 From: Takumi Ito Date: Fri, 9 Aug 2024 09:19:11 +0900 Subject: [PATCH 1/3] add get distance to obstacle function in the pyrhon wrapper Signed-off-by: Takumi Ito --- .../astar_search.py | 4 ++++ .../scripts/bind/astar_search_pybind.cpp | 19 ++++++++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py index 7b03a4024dc1b..50741971a7f80 100644 --- a/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py +++ b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py @@ -71,3 +71,7 @@ def getWaypoints(self): ) return waypoints + + def getDistanceToObstacle(self, pose: Pose): + pose_byte = serialize_message(pose) + return self.astar_search.getDistanceToObstacle(pose_byte) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 7808d18f24aed..1a8aba04b458a 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -99,6 +99,22 @@ class AstarSearchPython : public freespace_planning_algorithms::AstarSearch waypoints_vector.length = waypoints.compute_length(); return waypoints_vector; } + + double getDistanceToObstacle(const std::string & pose_byte) + { + rclcpp::SerializedMessage serialized_msg; + static constexpr size_t message_header_length = 8u; + serialized_msg.reserve(message_header_length + pose_byte.size()); + serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size(); + for (size_t i = 0; i < pose_byte.size(); ++i) { + serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i]; + } + geometry_msgs::msg::Pose pose; + static rclcpp::Serialization serializer; + serializer.deserialize_message(&serialized_msg, &pose); + + return freespace_planning_algorithms::AstarSearch::getDistanceToObstacle(pose); + } }; namespace py = pybind11; @@ -187,6 +203,7 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) freespace_planning_algorithms::AstarParam &>()) .def("setMap", &AstarSearchPython::setMapByte) .def("makePlan", &AstarSearchPython::makePlanByte) - .def("getWaypoints", &AstarSearchPython::getWaypointsAsVector); + .def("getWaypoints", &AstarSearchPython::getWaypointsAsVector) + .def("getDistanceToObstacle", &AstarSearchPython::getDistanceToObstacle); } } // namespace autoware::freespace_planning_algorithms From 35adb94dc86b81d19ec592a37876095d9c86fed0 Mon Sep 17 00:00:00 2001 From: Takumi Ito Date: Fri, 23 Aug 2024 14:02:56 +0900 Subject: [PATCH 2/3] add setMinMaxDimension Function Signed-off-by: Takumi Ito --- .../scripts/bind/astar_search_pybind.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 1a8aba04b458a..ca2558e71ce98 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -123,12 +123,12 @@ namespace py = pybind11; PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) { auto pyPlannerWaypointsVector = - py::class_(p, "PlannerWaypointsVector", py::dynamic_attr()) + py::class_(p, "PlannerWaypointsVector") .def(py::init<>()) .def_readwrite("waypoints", &PlannerWaypointsVector::waypoints) .def_readwrite("length", &PlannerWaypointsVector::length); auto pyAstarParam = - py::class_(p, "AstarParam", py::dynamic_attr()) + py::class_(p, "AstarParam") .def(py::init<>()) .def_readwrite("search_method", &freespace_planning_algorithms::AstarParam::search_method) .def_readwrite( @@ -154,7 +154,7 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) &freespace_planning_algorithms::AstarParam::goal_lat_distance_weight); auto pyPlannerCommonParam = py::class_( - p, "PlannerCommonParam", py::dynamic_attr()) + p, "PlannerCommonParam") .def(py::init<>()) .def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit) .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size) @@ -181,14 +181,16 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) "obstacle_threshold", &freespace_planning_algorithms::PlannerCommonParam::obstacle_threshold); auto pyVehicleShape = - py::class_(p, "VehicleShape", py::dynamic_attr()) + py::class_(p, "VehicleShape") .def(py::init<>()) .def(py::init()) + .def("setMinMaxDimension", &freespace_planning_algorithms::VehicleShape::setMinMaxDimension) .def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length) .def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width) .def_readwrite("base_length", &freespace_planning_algorithms::VehicleShape::base_length) .def_readwrite("max_steering", &freespace_planning_algorithms::VehicleShape::max_steering) - .def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back); + .def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back) + .def_readwrite("min_dimension", &freespace_planning_algorithms::VehicleShape::min_dimension); auto pyAbstractPlanningAlgorithm = py::class_( From 271d8c3e7e31926fc16b96493f0b3dfefa659c0c Mon Sep 17 00:00:00 2001 From: Takumi Ito Date: Wed, 9 Oct 2024 07:52:11 -0400 Subject: [PATCH 3/3] run pre-commit Signed-off-by: Takumi Ito --- .../astar_search.py | 2 +- .../scripts/bind/astar_search_pybind.cpp | 12 +++++------- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py index 50741971a7f80..8078f3bb1e92f 100644 --- a/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py +++ b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py @@ -71,7 +71,7 @@ def getWaypoints(self): ) return waypoints - + def getDistanceToObstacle(self, pose: Pose): pose_byte = serialize_message(pose) return self.astar_search.getDistanceToObstacle(pose_byte) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index ca2558e71ce98..66dc8dd807c10 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -122,11 +122,10 @@ namespace py = pybind11; // cppcheck-suppress syntaxError PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) { - auto pyPlannerWaypointsVector = - py::class_(p, "PlannerWaypointsVector") - .def(py::init<>()) - .def_readwrite("waypoints", &PlannerWaypointsVector::waypoints) - .def_readwrite("length", &PlannerWaypointsVector::length); + auto pyPlannerWaypointsVector = py::class_(p, "PlannerWaypointsVector") + .def(py::init<>()) + .def_readwrite("waypoints", &PlannerWaypointsVector::waypoints) + .def_readwrite("length", &PlannerWaypointsVector::length); auto pyAstarParam = py::class_(p, "AstarParam") .def(py::init<>()) @@ -153,8 +152,7 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) "goal_lat_distance_weight", &freespace_planning_algorithms::AstarParam::goal_lat_distance_weight); auto pyPlannerCommonParam = - py::class_( - p, "PlannerCommonParam") + py::class_(p, "PlannerCommonParam") .def(py::init<>()) .def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit) .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)