Skip to content

Commit fc0085b

Browse files
author
Takumi Ito
committedFeb 19, 2024
run pre-commit
Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>
1 parent e776033 commit fc0085b

File tree

3 files changed

+102
-103
lines changed

3 files changed

+102
-103
lines changed
 

‎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

+79-76
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,9 @@
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

@@ -26,85 +27,87 @@ using namespace freespace_planning_algorithms;
2627

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

74-
7576
namespace py = pybind11;
7677

7778
PYBIND11_MODULE(freespace_planning_algorithms_python, p)
7879
{
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);
80+
auto pyAstarParam =
81+
py::class_<AstarParam>(p, "AstarParam", py::dynamic_attr())
82+
.def(py::init<>())
83+
.def_readwrite("only_behind_solutions", &AstarParam::only_behind_solutions)
84+
.def_readwrite("use_back", &AstarParam::use_back)
85+
.def_readwrite("distance_heuristic_weight", &AstarParam::distance_heuristic_weight);
86+
auto pyPlannerCommonParam =
87+
py::class_<PlannerCommonParam>(p, "PlannerCommonParam", py::dynamic_attr())
88+
.def(py::init<>())
89+
.def_readwrite("time_limit", &PlannerCommonParam::time_limit)
90+
.def_readwrite("minimum_turning_radius", &PlannerCommonParam::minimum_turning_radius)
91+
.def_readwrite("maximum_turning_radius", &PlannerCommonParam::maximum_turning_radius)
92+
.def_readwrite("turning_radius_size", &PlannerCommonParam::turning_radius_size)
93+
.def_readwrite("theta_size", &PlannerCommonParam::theta_size)
94+
.def_readwrite("curve_weight", &PlannerCommonParam::curve_weight)
95+
.def_readwrite("reverse_weight", &PlannerCommonParam::reverse_weight)
96+
.def_readwrite("lateral_goal_range", &PlannerCommonParam::lateral_goal_range)
97+
.def_readwrite("longitudinal_goal_range", &PlannerCommonParam::longitudinal_goal_range)
98+
.def_readwrite("angle_goal_range", &PlannerCommonParam::angle_goal_range)
99+
.def_readwrite("obstacle_threshold", &PlannerCommonParam::obstacle_threshold);
100+
auto pyVSehicleShape = py::class_<VehicleShape>(p, "VehicleShape", py::dynamic_attr())
101+
.def(py::init<>())
102+
.def(py::init<double, double, double>())
103+
.def_readwrite("length", &VehicleShape::length)
104+
.def_readwrite("width", &VehicleShape::width)
105+
.def_readwrite("base2back", &VehicleShape::base2back);
106+
107+
py::class_<AbstractPlanningAlgorithm>(p, "AbstractPlanningAlgorithm");
108+
py::class_<AstarSearch, AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
109+
py::class_<AstarSearchPython, AstarSearch>(p, "AstarSearch")
110+
.def(py::init<PlannerCommonParam &, VehicleShape &, AstarParam &>())
111+
.def("setMap", &AstarSearchPython::setMapByte)
112+
.def("makePlan", &AstarSearchPython::makePlanByte);
110113
}

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

+22-26
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,10 @@
11
import freespace_planning_algorithms.freespace_planning_algorithms_python as fp
2-
3-
from pyquaternion import Quaternion
4-
5-
from nav_msgs.msg import OccupancyGrid
62
from geometry_msgs.msg import Pose
3+
from nav_msgs.msg import OccupancyGrid
4+
from pyquaternion import Quaternion
75
from rclpy.serialization import serialize_message
86

9-
10-
# -- Vehicle Shape --
7+
# -- Vehicle Shape --
118
vehicle_shape = fp.VehicleShape()
129
vehicle_shape.length = 2.0
1310
vehicle_shape.width = 1.0
@@ -17,26 +14,26 @@
1714
# -- Planner Common Parameter --
1815
planner_param = fp.PlannerCommonParam()
1916
# 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
17+
planner_param.time_limit = 30000.0
18+
planner_param.minimum_turning_radius = 9.0
19+
planner_param.maximum_turning_radius = 9.0
20+
planner_param.turning_radius_size = 1
2421
# 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
22+
planner_param.theta_size = 144
23+
planner_param.angle_goal_range = 6.0
24+
planner_param.curve_weight = 1.2
25+
planner_param.reverse_weight = 2.0
26+
planner_param.lateral_goal_range = 0.5
27+
planner_param.longitudinal_goal_range = 2.0
3128
# costmap configs
32-
planner_param.obstacle_threshold= 100
29+
planner_param.obstacle_threshold = 100
3330

3431

3532
# -- A* search Configurations --
3633
astar_param = fp.AstarParam()
37-
astar_param.only_behind_solutions= False
34+
astar_param.only_behind_solutions = False
3835
astar_param.use_back = True
39-
astar_param.distance_heuristic_weight= 1.0
36+
astar_param.distance_heuristic_weight = 1.0
4037

4138
astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)
4239

@@ -49,9 +46,9 @@
4946
costmap.info.resolution = resolution
5047
costmap.info.height = size
5148
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) ]
49+
costmap.info.origin.position.x = -size * resolution / 2
50+
costmap.info.origin.position.y = -size * resolution / 2
51+
costmap.data = [0 for i in range(size**2)]
5552

5653
astar.setMap(serialize_message(costmap))
5754

@@ -76,8 +73,7 @@
7673

7774

7875
# -- Search --
79-
if astar.makePlan(serialize_message(start_pose),\
80-
serialize_message(goal_pose)):
81-
print('Success to find path.')
76+
if astar.makePlan(serialize_message(start_pose), serialize_message(goal_pose)):
77+
print("Success to find path.")
8278
else:
83-
print('Fail to find path.')
79+
print("Fail to find path.")

0 commit comments

Comments
 (0)