Skip to content

Commit 6e31ed8

Browse files
soblinsoblin
authored
feat(behavior_path_goal planner): add example plot for development (#8772)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 8f79195 commit 6e31ed8

File tree

3 files changed

+319
-0
lines changed

3 files changed

+319
-0
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt

+25
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,10 @@
11
cmake_minimum_required(VERSION 3.14)
22
project(autoware_behavior_path_goal_planner_module)
33

4+
set(ament_cmake_lint_cmake_FOUND TRUE)
5+
6+
option(BUILD_EXAMPLES "Build examples" OFF)
7+
48
find_package(autoware_cmake REQUIRED)
59
autoware_package()
610
pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml)
@@ -16,4 +20,25 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1620
src/manager.cpp
1721
)
1822

23+
if(BUILD_EXAMPLES)
24+
message(STATUS "Building examples")
25+
include(FetchContent)
26+
FetchContent_Declare(
27+
matplotlibcpp17
28+
GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git
29+
GIT_TAG master
30+
)
31+
FetchContent_MakeAvailable(matplotlibcpp17)
32+
33+
file(GLOB_RECURSE example_files examples/*.cpp)
34+
35+
foreach(example_file ${example_files})
36+
get_filename_component(example_name ${example_file} NAME_WE)
37+
ament_auto_add_executable(${example_name} ${example_file})
38+
target_include_directories(${example_name} SYSTEM INTERFACE matplotlibcpp17::matplotlibcpp17)
39+
set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter)
40+
target_link_libraries(${example_name} matplotlibcpp17::matplotlibcpp17)
41+
endforeach()
42+
endif()
43+
1944
ament_auto_package(INSTALL_TO_SHARE config)

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp

+293
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,293 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <ament_index_cpp/get_package_share_directory.hpp>
16+
#include <autoware/behavior_path_goal_planner_module/manager.hpp>
17+
#include <autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp>
18+
#include <autoware/behavior_path_planner/behavior_path_planner_node.hpp>
19+
#include <autoware/behavior_path_planner_common/data_manager.hpp>
20+
#include <autoware/behavior_path_planner_common/utils/path_utils.hpp>
21+
#include <autoware/route_handler/route_handler.hpp>
22+
#include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
23+
#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
24+
#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
25+
26+
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
27+
#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
28+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
29+
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
30+
#include <geometry_msgs/msg/point.hpp>
31+
#include <geometry_msgs/msg/pose.hpp>
32+
#include <geometry_msgs/msg/quaternion.hpp>
33+
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
34+
35+
#include <lanelet2_io/Io.h>
36+
#include <matplotlibcpp17/pyplot.h>
37+
38+
#include <chrono>
39+
#include <iostream>
40+
41+
using namespace std::chrono_literals; // NOLINT
42+
43+
void plot_path_with_lane_id(
44+
matplotlibcpp17::axes::Axes & axes, const tier4_planning_msgs::msg::PathWithLaneId path)
45+
{
46+
std::vector<double> xs, ys;
47+
for (const auto & point : path.points) {
48+
xs.push_back(point.point.pose.position.x);
49+
ys.push_back(point.point.pose.position.y);
50+
}
51+
axes.plot(Args(xs, ys), Kwargs("color"_a = "red", "linewidth"_a = 1.0));
52+
}
53+
54+
void plot_lanelet(
55+
matplotlibcpp17::axes::Axes & axes, lanelet::ConstLanelet lanelet,
56+
const std::string & color = "blue", const double linewidth = 0.5)
57+
{
58+
const auto lefts = lanelet.leftBound();
59+
const auto rights = lanelet.rightBound();
60+
std::vector<double> xs_left, ys_left;
61+
for (const auto & point : lefts) {
62+
xs_left.push_back(point.x());
63+
ys_left.push_back(point.y());
64+
}
65+
66+
std::vector<double> xs_right, ys_right;
67+
for (const auto & point : rights) {
68+
xs_right.push_back(point.x());
69+
ys_right.push_back(point.y());
70+
}
71+
72+
std::vector<double> xs_center, ys_center;
73+
for (const auto & point : lanelet.centerline()) {
74+
xs_center.push_back(point.x());
75+
ys_center.push_back(point.y());
76+
}
77+
78+
axes.plot(Args(xs_left, ys_left), Kwargs("color"_a = color, "linewidth"_a = linewidth));
79+
axes.plot(Args(xs_right, ys_right), Kwargs("color"_a = color, "linewidth"_a = linewidth));
80+
axes.plot(
81+
Args(xs_center, ys_center),
82+
Kwargs("color"_a = "black", "linewidth"_a = linewidth, "linestyle"_a = "dashed"));
83+
}
84+
85+
int main(int argc, char ** argv)
86+
{
87+
rclcpp::init(argc, argv);
88+
89+
const auto road_shoulder_test_map_path =
90+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner_common") +
91+
"/test_map/road_shoulder/lanelet2_map.osm";
92+
93+
lanelet::ErrorMessages errors{};
94+
lanelet::projection::MGRSProjector projector{};
95+
const lanelet::LaneletMapPtr lanelet_map_ptr =
96+
lanelet::load(road_shoulder_test_map_path, projector, &errors);
97+
if (!errors.empty()) {
98+
for (const auto & error : errors) {
99+
std::cout << error << std::endl;
100+
}
101+
return 1;
102+
}
103+
104+
autoware_planning_msgs::msg::LaneletRoute route_msg;
105+
route_msg.start_pose = geometry_msgs::build<geometry_msgs::msg::Pose>()
106+
.position(geometry_msgs::build<geometry_msgs::msg::Point>()
107+
.x(530.707103865218)
108+
.y(436.8758309382301)
109+
.z(100.0))
110+
.orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>()
111+
.x(0.0)
112+
.y(0.0)
113+
.z(0.2187392230193602)
114+
.w(0.9757833531644647));
115+
route_msg.goal_pose = geometry_msgs::build<geometry_msgs::msg::Pose>()
116+
.position(geometry_msgs::build<geometry_msgs::msg::Point>()
117+
.x(571.8018955394537)
118+
.y(435.6819504507543)
119+
.z(100.0))
120+
.orientation(geometry_msgs::build<geometry_msgs::msg::Quaternion>()
121+
.x(0.0)
122+
.y(0.0)
123+
.z(-0.3361155457734493)
124+
.w(0.9418207578352774));
125+
/*
126+
route_msg.goal_pose =
127+
geometry_msgs::build<geometry_msgs::msg::Pose>()
128+
.position(
129+
geometry_msgs::build<geometry_msgs::msg::Point>().x(568.836731).y(437.871904).z(100.0))
130+
.orientation(
131+
geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0.0).y(0.0).z(-0.292125).w(
132+
0.956380));
133+
*/
134+
135+
route_msg.segments = std::vector<autoware_planning_msgs::msg::LaneletSegment>{
136+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletSegment>()
137+
.preferred_primitive(
138+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
139+
.id(17757)
140+
.primitive_type(""))
141+
.primitives(std::vector<autoware_planning_msgs::msg::LaneletPrimitive>{
142+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
143+
.id(17756)
144+
.primitive_type("lane"),
145+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
146+
.id(17757)
147+
.primitive_type("lane"),
148+
}),
149+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletSegment>()
150+
.preferred_primitive(
151+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
152+
.id(18494)
153+
.primitive_type(""))
154+
.primitives(std::vector<autoware_planning_msgs::msg::LaneletPrimitive>{
155+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
156+
.id(18494)
157+
.primitive_type("lane"),
158+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
159+
.id(18493)
160+
.primitive_type("lane"),
161+
}),
162+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletSegment>()
163+
.preferred_primitive(
164+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
165+
.id(18496)
166+
.primitive_type(""))
167+
.primitives(std::vector<autoware_planning_msgs::msg::LaneletPrimitive>{
168+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
169+
.id(18496)
170+
.primitive_type("lane"),
171+
autoware_planning_msgs::build<autoware_planning_msgs::msg::LaneletPrimitive>()
172+
.id(18497)
173+
.primitive_type("lane"),
174+
}),
175+
};
176+
route_msg.allow_modification = false;
177+
autoware_map_msgs::msg::LaneletMapBin map_bin;
178+
lanelet::utils::conversion::toBinMsg(
179+
lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler
180+
181+
/*
182+
reference:
183+
https://github.com/ros2/rclcpp/blob/ee94bc63e4ce47a502891480a2796b53d54fcdfc/rclcpp/test/rclcpp/test_parameter_client.cpp#L927
184+
*/
185+
/*
186+
auto node = rclcpp::Node::make_shared(
187+
"node", "namespace", rclcpp::NodeOptions().allow_undeclared_parameters(true));
188+
auto param_node = std::make_shared<rclcpp::AsyncParametersClient>(node, "");
189+
{
190+
auto future = param_node->load_parameters(
191+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") +
192+
"/config/behavior_path_planner.param.yaml");
193+
if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) {
194+
RCLCPP_INFO(node->get_logger(), "failed to load behavior_path_planner.param.yaml");
195+
}
196+
}
197+
{
198+
auto future = param_node->load_parameters(
199+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") +
200+
"/config/drivable_area_expansion.param.yaml");
201+
if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) {
202+
RCLCPP_INFO(node->get_logger(), "failed to load drivable_area_expansion.param.yaml");
203+
}
204+
}
205+
{
206+
auto future = param_node->load_parameters(
207+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") +
208+
"/config/scene_module_manager.param.yaml");
209+
if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) {
210+
RCLCPP_INFO(node->get_logger(), "failed to load scene_module_manager.param.yaml");
211+
}
212+
}
213+
*/
214+
auto node_options = rclcpp::NodeOptions{};
215+
node_options.parameter_overrides(
216+
std::vector<rclcpp::Parameter>{{"launch_modules", std::vector<std::string>{}}});
217+
node_options.arguments(std::vector<std::string>{
218+
"--ros-args", "--params-file",
219+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") +
220+
"/config/behavior_path_planner.param.yaml",
221+
"--params-file",
222+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") +
223+
"/config/drivable_area_expansion.param.yaml",
224+
"--params-file",
225+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") +
226+
"/config/scene_module_manager.param.yaml",
227+
"--params-file",
228+
ament_index_cpp::get_package_share_directory("autoware_test_utils") +
229+
"/config/test_common.param.yaml",
230+
"--params-file",
231+
ament_index_cpp::get_package_share_directory("autoware_test_utils") +
232+
"/config/test_nearest_search.param.yaml",
233+
"--params-file",
234+
ament_index_cpp::get_package_share_directory("autoware_test_utils") +
235+
"/config/test_vehicle_info.param.yaml",
236+
"--params-file",
237+
ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") +
238+
"/config/goal_planner.param.yaml"});
239+
auto node = rclcpp::Node::make_shared("plot_map", node_options);
240+
241+
auto planner_data = std::make_shared<autoware::behavior_path_planner::PlannerData>();
242+
planner_data->init_parameters(*node);
243+
planner_data->route_handler->setMap(map_bin);
244+
planner_data->route_handler->setRoute(route_msg);
245+
nav_msgs::msg::Odometry odom;
246+
odom.pose.pose = route_msg.start_pose;
247+
auto odometry = std::make_shared<const nav_msgs::msg::Odometry>(odom);
248+
planner_data->self_odometry = odometry;
249+
lanelet::ConstLanelet current_route_lanelet;
250+
planner_data->route_handler->getClosestLaneletWithinRoute(
251+
route_msg.start_pose, &current_route_lanelet);
252+
std::cout << "current_route_lanelet is " << current_route_lanelet.id() << std::endl;
253+
const auto reference_path =
254+
autoware::behavior_path_planner::utils::getReferencePath(current_route_lanelet, planner_data);
255+
auto goal_planner_parameter =
256+
autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters(
257+
node.get(), "goal_planner.");
258+
const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo();
259+
autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{};
260+
lane_departure_checker.setVehicleInfo(vehicle_info);
261+
autoware::lane_departure_checker::Param lane_depature_checker_params;
262+
lane_depature_checker_params.footprint_extra_margin =
263+
goal_planner_parameter.lane_departure_check_expansion_margin;
264+
lane_departure_checker.setParam(lane_depature_checker_params);
265+
auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver(
266+
*node, goal_planner_parameter, lane_departure_checker);
267+
const auto pull_over_path_opt =
268+
shift_pull_over_planner.plan(planner_data, reference_path, route_msg.goal_pose);
269+
270+
pybind11::scoped_interpreter guard{};
271+
auto plt = matplotlibcpp17::pyplot::import();
272+
auto [fig, ax] = plt.subplots();
273+
274+
const std::vector<lanelet::Id> ids{17756, 17757, 18493, 18494, 18496, 18497,
275+
18492, 18495, 18498, 18499, 18500};
276+
for (const auto & id : ids) {
277+
const auto lanelet = lanelet_map_ptr->laneletLayer.get(id);
278+
plot_lanelet(ax, lanelet);
279+
}
280+
281+
plot_path_with_lane_id(ax, reference_path.path);
282+
std::cout << pull_over_path_opt.has_value() << std::endl;
283+
if (pull_over_path_opt) {
284+
const auto & pull_over_path = pull_over_path_opt.value();
285+
const auto full_path = pull_over_path.getFullPath();
286+
plot_path_with_lane_id(ax, full_path);
287+
}
288+
ax.set_aspect(Args("equal"));
289+
plt.show();
290+
291+
rclcpp::shutdown();
292+
return 0;
293+
}

‎planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ namespace autoware::behavior_path_planner
2929

3030
class GoalPlannerModuleManager : public SceneModuleManagerInterface
3131
{
32+
public:
3233
static GoalPlannerParameters initGoalPlannerParameters(
3334
rclcpp::Node * node, const std::string & base_ns);
3435

0 commit comments

Comments
 (0)