Skip to content

Commit 5248c96

Browse files
Takumi Itokosuke55
Takumi Ito
authored and
kosuke55
committed
- 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>
1 parent 71e61d8 commit 5248c96

File tree

5 files changed

+218
-115
lines changed

5 files changed

+218
-115
lines changed

‎planning/freespace_planning_algorithms/CMakeLists.txt

+5-5
Original file line numberDiff line numberDiff line change
@@ -60,17 +60,17 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
6060
find_package(pybind11_vendor REQUIRED)
6161
find_package(pybind11 REQUIRED)
6262

63-
pybind11_add_module(freespace_planning_algorithms_python SHARED
63+
pybind11_add_module(${PROJECT_NAME}_pybind SHARED
6464
scripts/bind/astar_search_pybind.cpp
6565
)
6666

67-
include_directories(freespace_planning_algorithms_python PRIVATE
67+
include_directories(${PROJECT_NAME}_pybind PRIVATE
6868
include/
6969
)
70-
target_link_libraries(freespace_planning_algorithms_python PRIVATE
71-
freespace_planning_algorithms
70+
target_link_libraries(${PROJECT_NAME}_pybind PRIVATE
71+
${PROJECT_NAME}
7272
)
7373

74-
install(TARGETS freespace_planning_algorithms_python
74+
install(TARGETS ${PROJECT_NAME}_pybind
7575
DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
7676
)

‎planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py

+50
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
import freespace_planning_algorithms.freespace_planning_algorithms_pybind as _fp
2+
from geometry_msgs.msg import Point
3+
from geometry_msgs.msg import Pose
4+
from geometry_msgs.msg import Quaternion
5+
from nav_msgs.msg import OccupancyGrid
6+
from rclpy.serialization import serialize_message
7+
8+
PlannerCommonParam = _fp.PlannerCommonParam
9+
VehicleShape = _fp.VehicleShape
10+
AstarParam = _fp.AstarParam
11+
12+
13+
class PlannerWaypoints:
14+
def __init__(self):
15+
self.waypoints = []
16+
self.length = 0.0
17+
18+
def compute_length(self):
19+
return self.length
20+
21+
22+
class AstarSearch:
23+
def __init__(
24+
self,
25+
planner_param: PlannerCommonParam,
26+
vehicle_shape: VehicleShape,
27+
astar_param: AstarParam,
28+
):
29+
self.astar_search = _fp.AstarSearch(planner_param, vehicle_shape, astar_param)
30+
31+
def setMap(self, costmap: OccupancyGrid):
32+
costmap_byte = serialize_message(costmap)
33+
self.astar_search.setMap(costmap_byte)
34+
35+
def makePlan(self, start_pose: Pose, goal_pose: Pose):
36+
start_pose_byte = serialize_message(start_pose)
37+
goal_pose_byte = serialize_message(goal_pose)
38+
return self.astar_search.makePlan(start_pose_byte, goal_pose_byte)
39+
40+
def getWaypoints(self):
41+
waypoints_vetor = self.astar_search.getWaypoints()
42+
waypoints = PlannerWaypoints()
43+
44+
waypoints.length = waypoints_vetor.length
45+
for waypoint in waypoints_vetor.waypoints:
46+
pos = Point(x=waypoint[0], y=waypoint[1], z=waypoint[2])
47+
quat = Quaternion(x=waypoint[3], y=waypoint[4], z=waypoint[5], w=waypoint[6])
48+
waypoints.waypoints.append(Pose(position=pos, orientation=quat))
49+
50+
return waypoints

‎planning/freespace_planning_algorithms/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1616
<buildtool_depend>autoware_cmake</buildtool_depend>
1717
<buildtool_depend>python_cmake_module</buildtool_depend>
18-
18+
1919
<depend>geometry_msgs</depend>
2020
<depend>nav_msgs</depend>
2121
<depend>nlohmann-json-dev</depend>

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

+137-80
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2024 Tier IV, Inc. All rights reserved.
1+
// Copyright 2024 TIER IV, Inc. All rights reserved.
22

33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,99 +12,156 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "freespace_planning_algorithms/astar_search.hpp"
1615
#include "freespace_planning_algorithms/abstract_algorithm.hpp"
16+
#include "freespace_planning_algorithms/astar_search.hpp"
17+
1718
#include <rclcpp/rclcpp.hpp>
1819
#include <rclcpp/serialization.hpp>
1920

2021
#include <geometry_msgs/msg/pose_array.hpp>
2122
#include <nav_msgs/msg/occupancy_grid.hpp>
2223

2324
#include <pybind11/pybind11.h>
25+
#include <pybind11/stl.h>
2426

25-
using namespace freespace_planning_algorithms;
26-
27-
class AstarSearchPython : public AstarSearch
27+
struct PlannerWaypointsVector
2828
{
29-
using AstarSearch::AstarSearch;
30-
public:
31-
void setMapByte(const std::string & costmap_byte)
32-
{
33-
rclcpp::SerializedMessage serialized_msg;
34-
static constexpr size_t message_header_length = 8u;
35-
serialized_msg.reserve(message_header_length + costmap_byte.size());
36-
serialized_msg.get_rcl_serialized_message().buffer_length = costmap_byte.size();
37-
for (size_t i = 0; i < costmap_byte.size(); ++i) {
38-
serialized_msg.get_rcl_serialized_message().buffer[i] = costmap_byte[i];
39-
}
40-
nav_msgs::msg::OccupancyGrid costmap;
41-
static rclcpp::Serialization<nav_msgs::msg::OccupancyGrid> serializer;
42-
serializer.deserialize_message(&serialized_msg, &costmap);
43-
44-
AstarSearch::setMap(costmap);
45-
}
46-
47-
bool makePlanByte(const std::string & start_pose_byte, const std::string & goal_pose_byte)
48-
{
49-
rclcpp::SerializedMessage serialized_start_msg;
50-
static constexpr size_t message_header_length = 8u;
51-
serialized_start_msg.reserve(message_header_length + start_pose_byte.size());
52-
serialized_start_msg.get_rcl_serialized_message().buffer_length = start_pose_byte.size();
53-
for (size_t i = 0; i < start_pose_byte.size(); ++i) {
54-
serialized_start_msg.get_rcl_serialized_message().buffer[i] = start_pose_byte[i];
55-
}
56-
geometry_msgs::msg::Pose start_pose;
57-
static rclcpp::Serialization<geometry_msgs::msg::Pose> start_serializer;
58-
start_serializer.deserialize_message(&serialized_start_msg, &start_pose);
59-
60-
rclcpp::SerializedMessage serialized_goal_msg;
61-
serialized_goal_msg.reserve(message_header_length + goal_pose_byte.size());
62-
serialized_goal_msg.get_rcl_serialized_message().buffer_length = goal_pose_byte.size();
63-
for (size_t i = 0; i < goal_pose_byte.size(); ++i) {
64-
serialized_goal_msg.get_rcl_serialized_message().buffer[i] = goal_pose_byte[i];
65-
}
66-
geometry_msgs::msg::Pose goal_pose;
67-
static rclcpp::Serialization<geometry_msgs::msg::Pose> goal_serializer;
68-
goal_serializer.deserialize_message(&serialized_goal_msg, &goal_pose);
69-
70-
return AstarSearch::makePlan(start_pose, goal_pose);
71-
}
29+
std::vector<std::vector<double>> waypoints;
30+
double length;
7231
};
7332

33+
class AstarSearchPython : public freespace_planning_algorithms::AstarSearch
34+
{
35+
using freespace_planning_algorithms::AstarSearch::AstarSearch;
36+
37+
public:
38+
void setMapByte(const std::string & costmap_byte)
39+
{
40+
rclcpp::SerializedMessage serialized_msg;
41+
static constexpr size_t message_header_length = 8u;
42+
serialized_msg.reserve(message_header_length + costmap_byte.size());
43+
serialized_msg.get_rcl_serialized_message().buffer_length = costmap_byte.size();
44+
for (size_t i = 0; i < costmap_byte.size(); ++i) {
45+
serialized_msg.get_rcl_serialized_message().buffer[i] = costmap_byte[i];
46+
}
47+
nav_msgs::msg::OccupancyGrid costmap;
48+
static rclcpp::Serialization<nav_msgs::msg::OccupancyGrid> serializer;
49+
serializer.deserialize_message(&serialized_msg, &costmap);
50+
51+
freespace_planning_algorithms::AstarSearch::setMap(costmap);
52+
}
53+
54+
bool makePlanByte(const std::string & start_pose_byte, const std::string & goal_pose_byte)
55+
{
56+
rclcpp::SerializedMessage serialized_start_msg;
57+
static constexpr size_t message_header_length = 8u;
58+
serialized_start_msg.reserve(message_header_length + start_pose_byte.size());
59+
serialized_start_msg.get_rcl_serialized_message().buffer_length = start_pose_byte.size();
60+
for (size_t i = 0; i < start_pose_byte.size(); ++i) {
61+
serialized_start_msg.get_rcl_serialized_message().buffer[i] = start_pose_byte[i];
62+
}
63+
geometry_msgs::msg::Pose start_pose;
64+
static rclcpp::Serialization<geometry_msgs::msg::Pose> start_serializer;
65+
start_serializer.deserialize_message(&serialized_start_msg, &start_pose);
66+
67+
rclcpp::SerializedMessage serialized_goal_msg;
68+
serialized_goal_msg.reserve(message_header_length + goal_pose_byte.size());
69+
serialized_goal_msg.get_rcl_serialized_message().buffer_length = goal_pose_byte.size();
70+
for (size_t i = 0; i < goal_pose_byte.size(); ++i) {
71+
serialized_goal_msg.get_rcl_serialized_message().buffer[i] = goal_pose_byte[i];
72+
}
73+
geometry_msgs::msg::Pose goal_pose;
74+
static rclcpp::Serialization<geometry_msgs::msg::Pose> goal_serializer;
75+
goal_serializer.deserialize_message(&serialized_goal_msg, &goal_pose);
76+
77+
return freespace_planning_algorithms::AstarSearch::makePlan(start_pose, goal_pose);
78+
}
79+
80+
PlannerWaypointsVector getWaypointsAsVector()
81+
{
82+
freespace_planning_algorithms::PlannerWaypoints waypoints =
83+
freespace_planning_algorithms::AstarSearch::getWaypoints();
84+
PlannerWaypointsVector waypoints_vector;
85+
for (const auto & waypoint : waypoints.waypoints) {
86+
// NOTE: it was difficult to return the deserialized pose_byte and serialize the pose_byte on
87+
// python-side. So this function returns [*position, *quaternion] as double array
88+
const auto & xyz = waypoint.pose.pose.position;
89+
const auto & quat = waypoint.pose.pose.orientation;
90+
waypoints_vector.waypoints.push_back(
91+
std::vector<double>({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w}));
92+
}
93+
waypoints_vector.length = waypoints.compute_length();
94+
return waypoints_vector;
95+
}
96+
};
7497

7598
namespace py = pybind11;
7699

77-
PYBIND11_MODULE(freespace_planning_algorithms_python, p)
100+
PYBIND11_MODULE(freespace_planning_algorithms_pybind, p)
78101
{
79-
auto pyAstarParam = py::class_<AstarParam>(p, "AstarParam", py::dynamic_attr())
80-
.def(py::init<>())
81-
.def_readwrite("only_behind_solutions", &AstarParam::only_behind_solutions)
82-
.def_readwrite("use_back", &AstarParam::use_back)
83-
.def_readwrite("distance_heuristic_weight", &AstarParam::distance_heuristic_weight);
84-
auto pyPlannerCommonParam = py::class_<PlannerCommonParam>(p, "PlannerCommonParam", py::dynamic_attr())
85-
.def(py::init<>())
86-
.def_readwrite("time_limit", &PlannerCommonParam::time_limit)
87-
.def_readwrite("minimum_turning_radius", &PlannerCommonParam::minimum_turning_radius)
88-
.def_readwrite("maximum_turning_radius", &PlannerCommonParam::maximum_turning_radius)
89-
.def_readwrite("turning_radius_size", &PlannerCommonParam::turning_radius_size)
90-
.def_readwrite("theta_size", &PlannerCommonParam::theta_size)
91-
.def_readwrite("curve_weight", &PlannerCommonParam::curve_weight)
92-
.def_readwrite("reverse_weight", &PlannerCommonParam::reverse_weight)
93-
.def_readwrite("lateral_goal_range", &PlannerCommonParam::lateral_goal_range)
94-
.def_readwrite("longitudinal_goal_range", &PlannerCommonParam::longitudinal_goal_range)
95-
.def_readwrite("angle_goal_range", &PlannerCommonParam::angle_goal_range)
96-
.def_readwrite("obstacle_threshold", &PlannerCommonParam::obstacle_threshold);
97-
auto pyVSehicleShape = py::class_<VehicleShape>(p, "VehicleShape", py::dynamic_attr())
98-
.def(py::init<>())
99-
.def(py::init<double, double, double>())
100-
.def_readwrite("length", &VehicleShape::length)
101-
.def_readwrite("width", &VehicleShape::width)
102-
.def_readwrite("base2back", &VehicleShape::base2back);
103-
104-
py::class_<AbstractPlanningAlgorithm>(p, "AbstractPlanningAlgorithm");
105-
py::class_<AstarSearch, AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
106-
py::class_<AstarSearchPython, AstarSearch>(p, "AstarSearch")
107-
.def(py::init<PlannerCommonParam &, VehicleShape &, AstarParam &>())
108-
.def("setMap", &AstarSearchPython::setMapByte)
109-
.def("makePlan", &AstarSearchPython::makePlanByte);
102+
auto pyPlannerWaypointsVector =
103+
py::class_<PlannerWaypointsVector>(p, "PlannerWaypointsVector", py::dynamic_attr())
104+
.def(py::init<>())
105+
.def_readwrite("waypoints", &PlannerWaypointsVector::waypoints)
106+
.def_readwrite("length", &PlannerWaypointsVector::length);
107+
auto pyAstarParam =
108+
py::class_<freespace_planning_algorithms::AstarParam>(p, "AstarParam", py::dynamic_attr())
109+
.def(py::init<>())
110+
.def_readwrite(
111+
"only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions)
112+
.def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back)
113+
.def_readwrite(
114+
"distance_heuristic_weight",
115+
&freespace_planning_algorithms::AstarParam::distance_heuristic_weight);
116+
auto pyPlannerCommonParam =
117+
py::class_<freespace_planning_algorithms::PlannerCommonParam>(
118+
p, "PlannerCommonParam", py::dynamic_attr())
119+
.def(py::init<>())
120+
.def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit)
121+
.def_readwrite(
122+
"minimum_turning_radius",
123+
&freespace_planning_algorithms::PlannerCommonParam::minimum_turning_radius)
124+
.def_readwrite(
125+
"maximum_turning_radius",
126+
&freespace_planning_algorithms::PlannerCommonParam::maximum_turning_radius)
127+
.def_readwrite(
128+
"turning_radius_size",
129+
&freespace_planning_algorithms::PlannerCommonParam::turning_radius_size)
130+
.def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)
131+
.def_readwrite(
132+
"curve_weight", &freespace_planning_algorithms::PlannerCommonParam::curve_weight)
133+
.def_readwrite(
134+
"reverse_weight", &freespace_planning_algorithms::PlannerCommonParam::reverse_weight)
135+
.def_readwrite(
136+
"lateral_goal_range",
137+
&freespace_planning_algorithms::PlannerCommonParam::lateral_goal_range)
138+
.def_readwrite(
139+
"longitudinal_goal_range",
140+
&freespace_planning_algorithms::PlannerCommonParam::longitudinal_goal_range)
141+
.def_readwrite(
142+
"angle_goal_range", &freespace_planning_algorithms::PlannerCommonParam::angle_goal_range)
143+
.def_readwrite(
144+
"obstacle_threshold",
145+
&freespace_planning_algorithms::PlannerCommonParam::obstacle_threshold);
146+
auto pyVSehicleShape =
147+
py::class_<freespace_planning_algorithms::VehicleShape>(p, "VehicleShape", py::dynamic_attr())
148+
.def(py::init<>())
149+
.def(py::init<double, double, double>())
150+
.def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length)
151+
.def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width)
152+
.def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back);
153+
154+
py::class_<freespace_planning_algorithms::AbstractPlanningAlgorithm>(
155+
p, "AbstractPlanningAlgorithm");
156+
py::class_<
157+
freespace_planning_algorithms::AstarSearch,
158+
freespace_planning_algorithms::AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
159+
py::class_<AstarSearchPython, freespace_planning_algorithms::AstarSearch>(p, "AstarSearch")
160+
.def(py::init<
161+
freespace_planning_algorithms::PlannerCommonParam &,
162+
freespace_planning_algorithms::VehicleShape &,
163+
freespace_planning_algorithms::AstarParam &>())
164+
.def("setMap", &AstarSearchPython::setMapByte)
165+
.def("makePlan", &AstarSearchPython::makePlanByte)
166+
.def("getWaypoints", &AstarSearchPython::getWaypointsAsVector);
110167
}

0 commit comments

Comments
 (0)