Skip to content

Commit 071caf5

Browse files
Takumi Itokosuke55
Takumi Ito
authored and
kosuke55
committed
feat(freespace_planning_algorithms): add Astar search by python
Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
1 parent fc2b2c6 commit 071caf5

File tree

5 files changed

+210
-1
lines changed

5 files changed

+210
-1
lines changed

‎planning/freespace_planning_algorithms/CMakeLists.txt

+22
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,10 @@ cmake_minimum_required(VERSION 3.14)
22
project(freespace_planning_algorithms)
33

44
find_package(autoware_cmake REQUIRED)
5+
find_package(python_cmake_module REQUIRED)
6+
57
autoware_package()
8+
ament_python_install_package(${PROJECT_NAME})
69

710
ament_auto_add_library(reeds_shepp SHARED
811
src/reeds_shepp.cpp
@@ -52,3 +55,22 @@ install(PROGRAMS
5255
test/debug_plot.py
5356
DESTINATION lib/${PROJECT_NAME}
5457
)
58+
59+
find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
60+
find_package(pybind11_vendor REQUIRED)
61+
find_package(pybind11 REQUIRED)
62+
63+
pybind11_add_module(freespace_planning_algorithms_python SHARED
64+
scripts/bind/astar_search_pybind.cpp
65+
)
66+
67+
include_directories(freespace_planning_algorithms_python PRIVATE
68+
include/
69+
)
70+
target_link_libraries(freespace_planning_algorithms_python PRIVATE
71+
freespace_planning_algorithms
72+
)
73+
74+
install(TARGETS freespace_planning_algorithms_python
75+
DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
76+
)

‎planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py

Whitespace-only changes.

‎planning/freespace_planning_algorithms/package.xml

+2-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,8 @@
1414

1515
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1616
<buildtool_depend>autoware_cmake</buildtool_depend>
17-
17+
<buildtool_depend>python_cmake_module</buildtool_depend>
18+
1819
<depend>geometry_msgs</depend>
1920
<depend>nav_msgs</depend>
2021
<depend>nlohmann-json-dev</depend>

‎planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

+103
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
#include "freespace_planning_algorithms/astar_search.hpp"
2+
#include "freespace_planning_algorithms/abstract_algorithm.hpp"
3+
#include <rclcpp/rclcpp.hpp>
4+
#include <rclcpp/serialization.hpp>
5+
6+
#include <geometry_msgs/msg/pose_array.hpp>
7+
#include <nav_msgs/msg/occupancy_grid.hpp>
8+
9+
10+
#include <pybind11/pybind11.h>
11+
// #include <pybind11/stl.h>
12+
// #include <pybind11/stl_bind.h>
13+
// #include <pybind11/eigen.h>
14+
// #include <pybind11/chrono.h>
15+
// #include <pybind11/complex.h>
16+
// #include <pybind11/functional.h>
17+
18+
using namespace freespace_planning_algorithms;
19+
20+
class AstarSearchPython : public AstarSearch
21+
{
22+
using AstarSearch::AstarSearch;
23+
public:
24+
void setMapByte(const std::string & costmap_byte)
25+
{
26+
rclcpp::SerializedMessage serialized_msg;
27+
static constexpr size_t message_header_length = 8u;
28+
serialized_msg.reserve(message_header_length + costmap_byte.size());
29+
serialized_msg.get_rcl_serialized_message().buffer_length = costmap_byte.size();
30+
for (size_t i = 0; i < costmap_byte.size(); ++i) {
31+
serialized_msg.get_rcl_serialized_message().buffer[i] = costmap_byte[i];
32+
}
33+
nav_msgs::msg::OccupancyGrid costmap;
34+
static rclcpp::Serialization<nav_msgs::msg::OccupancyGrid> serializer;
35+
serializer.deserialize_message(&serialized_msg, &costmap);
36+
37+
AstarSearch::setMap(costmap);
38+
}
39+
40+
bool makePlanByte(const std::string & start_pose_byte, const std::string & goal_pose_byte)
41+
{
42+
rclcpp::SerializedMessage serialized_start_msg;
43+
static constexpr size_t message_header_length = 8u;
44+
serialized_start_msg.reserve(message_header_length + start_pose_byte.size());
45+
serialized_start_msg.get_rcl_serialized_message().buffer_length = start_pose_byte.size();
46+
for (size_t i = 0; i < start_pose_byte.size(); ++i) {
47+
serialized_start_msg.get_rcl_serialized_message().buffer[i] = start_pose_byte[i];
48+
}
49+
geometry_msgs::msg::Pose start_pose;
50+
static rclcpp::Serialization<geometry_msgs::msg::Pose> start_serializer;
51+
start_serializer.deserialize_message(&serialized_start_msg, &start_pose);
52+
53+
rclcpp::SerializedMessage serialized_goal_msg;
54+
serialized_goal_msg.reserve(message_header_length + goal_pose_byte.size());
55+
serialized_goal_msg.get_rcl_serialized_message().buffer_length = goal_pose_byte.size();
56+
for (size_t i = 0; i < goal_pose_byte.size(); ++i) {
57+
serialized_goal_msg.get_rcl_serialized_message().buffer[i] = goal_pose_byte[i];
58+
}
59+
geometry_msgs::msg::Pose goal_pose;
60+
static rclcpp::Serialization<geometry_msgs::msg::Pose> goal_serializer;
61+
goal_serializer.deserialize_message(&serialized_goal_msg, &goal_pose);
62+
63+
return AstarSearch::makePlan(start_pose, goal_pose);
64+
}
65+
};
66+
67+
68+
namespace py = pybind11;
69+
70+
PYBIND11_MODULE(freespace_planning_algorithms_python, p)
71+
{
72+
auto pyAstarParam = py::class_<AstarParam>(p, "AstarParam", py::dynamic_attr())
73+
.def(py::init<>())
74+
.def_readwrite("only_behind_solutions", &AstarParam::only_behind_solutions)
75+
.def_readwrite("use_back", &AstarParam::use_back)
76+
.def_readwrite("distance_heuristic_weight", &AstarParam::distance_heuristic_weight);
77+
auto pyPlannerCommonParam = py::class_<PlannerCommonParam>(p, "PlannerCommonParam", py::dynamic_attr())
78+
.def(py::init<>())
79+
.def_readwrite("time_limit", &PlannerCommonParam::time_limit)
80+
.def_readwrite("minimum_turning_radius", &PlannerCommonParam::minimum_turning_radius)
81+
.def_readwrite("maximum_turning_radius", &PlannerCommonParam::maximum_turning_radius)
82+
.def_readwrite("turning_radius_size", &PlannerCommonParam::turning_radius_size)
83+
.def_readwrite("theta_size", &PlannerCommonParam::theta_size)
84+
.def_readwrite("curve_weight", &PlannerCommonParam::curve_weight)
85+
.def_readwrite("reverse_weight", &PlannerCommonParam::reverse_weight)
86+
.def_readwrite("lateral_goal_range", &PlannerCommonParam::lateral_goal_range)
87+
.def_readwrite("longitudinal_goal_range", &PlannerCommonParam::longitudinal_goal_range)
88+
.def_readwrite("angle_goal_range", &PlannerCommonParam::angle_goal_range)
89+
.def_readwrite("obstacle_threshold", &PlannerCommonParam::obstacle_threshold);
90+
auto pyVSehicleShape = py::class_<VehicleShape>(p, "VehicleShape", py::dynamic_attr())
91+
.def(py::init<>())
92+
.def(py::init<double, double, double>())
93+
.def_readwrite("length", &VehicleShape::length)
94+
.def_readwrite("width", &VehicleShape::width)
95+
.def_readwrite("base2back", &VehicleShape::base2back);
96+
97+
py::class_<AbstractPlanningAlgorithm>(p, "AbstractPlanningAlgorithm");
98+
py::class_<AstarSearch, AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
99+
py::class_<AstarSearchPython, AstarSearch>(p, "AstarSearch")
100+
.def(py::init<PlannerCommonParam &, VehicleShape &, AstarParam &>())
101+
.def("setMap", &AstarSearchPython::setMapByte)
102+
.def("makePlan", &AstarSearchPython::makePlanByte);
103+
}

‎planning/freespace_planning_algorithms/scripts/example/example.py

+83
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
import freespace_planning_algorithms.freespace_planning_algorithms_python as fp
2+
3+
from pyquaternion import Quaternion
4+
5+
from nav_msgs.msg import OccupancyGrid
6+
from geometry_msgs.msg import Pose
7+
from rclpy.serialization import serialize_message
8+
9+
10+
# -- Vehicle Shape --
11+
vehicle_shape = fp.VehicleShape()
12+
vehicle_shape.length = 2.0
13+
vehicle_shape.width = 1.0
14+
vehicle_shape.base2back = 1.0
15+
16+
17+
# -- Planner Common Parameter --
18+
planner_param = fp.PlannerCommonParam()
19+
# base configs
20+
planner_param.time_limit= 30000.0
21+
planner_param.minimum_turning_radius= 9.0
22+
planner_param.maximum_turning_radius= 9.0
23+
planner_param.turning_radius_size= 1
24+
# search configs
25+
planner_param.theta_size= 144
26+
planner_param.angle_goal_range= 6.0
27+
planner_param.curve_weight= 1.2
28+
planner_param.reverse_weight= 2.0
29+
planner_param.lateral_goal_range= 0.5
30+
planner_param.longitudinal_goal_range= 2.0
31+
# costmap configs
32+
planner_param.obstacle_threshold= 100
33+
34+
35+
# -- A* search Configurations --
36+
astar_param = fp.AstarParam()
37+
astar_param.only_behind_solutions= False
38+
astar_param.use_back = True
39+
astar_param.distance_heuristic_weight= 1.0
40+
41+
astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)
42+
43+
44+
# -- Costmap Definition
45+
size = 350
46+
resolution = 0.2
47+
48+
costmap = OccupancyGrid()
49+
costmap.info.resolution = resolution
50+
costmap.info.height = size
51+
costmap.info.width = size
52+
costmap.info.origin.position.x = -size*resolution/2
53+
costmap.info.origin.position.y = -size*resolution/2
54+
costmap.data = [0 for i in range(size**2) ]
55+
56+
astar.setMap(serialize_message(costmap))
57+
58+
59+
# -- Start and Goal Pose
60+
start_pose = Pose()
61+
goal_pose = Pose()
62+
63+
start_pose.position.x = 0.0
64+
start_pose.position.y = 0.0
65+
66+
goal_pose.position.x = 10.0
67+
goal_pose.position.y = 0.0
68+
69+
yaw = 0
70+
quaterinon = Quaternion(axis=[0, 0, 1], angle=yaw)
71+
72+
goal_pose.orientation.w = quaterinon.w
73+
goal_pose.orientation.x = quaterinon.x
74+
goal_pose.orientation.y = quaterinon.y
75+
goal_pose.orientation.z = quaterinon.z
76+
77+
78+
# -- Search --
79+
if astar.makePlan(serialize_message(start_pose),\
80+
serialize_message(goal_pose)):
81+
print('Success to find path.')
82+
else:
83+
print('Fail to find path.')

0 commit comments

Comments
 (0)