Skip to content

Commit 3d714ac

Browse files
takayuki5168takayuki5168
authored
feat(static centerline optimizer): new package (#2013)
* feat(mission_planner): add another initialization function Signed-off-by: Takayuki Murooka <[email protected]> * feat(static_centerline_optimizer): add a new package Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * update README.md Signed-off-by: Takayuki Murooka <[email protected]> * refactor Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * fix spell Signed-off-by: Takayuki Murooka <[email protected]> * revert mission_planner's change Signed-off-by: Takayuki Murooka <[email protected]> * fix png path Signed-off-by: Takayuki Murooka <[email protected]> * remove unnecessary cerr Signed-off-by: Takayuki Murooka <[email protected]> * fix colcon test Signed-off-by: Takayuki Murooka <[email protected]> * update cmake version Signed-off-by: Takayuki Murooka <[email protected]> * tmp fix Signed-off-by: Takayuki Murooka <[email protected]> * fix CI error Signed-off-by: Takayuki Murooka <[email protected]> * fix CI error Signed-off-by: Takayuki Murooka <[email protected]> Signed-off-by: Takayuki Murooka <[email protected]>
1 parent a117014 commit 3d714ac

22 files changed

+2331
-0
lines changed

‎planning/static_centerline_optimizer/CMakeLists.txt

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(static_centerline_optimizer)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
6+
find_package(builtin_interfaces REQUIRED)
7+
find_package(geometry_msgs REQUIRED)
8+
find_package(rosidl_default_generators REQUIRED)
9+
find_package(std_msgs REQUIRED)
10+
find_package(Eigen3 REQUIRED)
11+
find_package(OpenCV REQUIRED)
12+
13+
rosidl_generate_interfaces(
14+
static_centerline_optimizer
15+
"srv/LoadMap.srv"
16+
"srv/PlanRoute.srv"
17+
"srv/PlanPath.srv"
18+
"msg/PointsWithLaneId.msg"
19+
DEPENDENCIES builtin_interfaces geometry_msgs
20+
)
21+
22+
autoware_package()
23+
24+
ament_auto_add_executable(main
25+
src/main.cpp
26+
src/static_centerline_optimizer_node.cpp
27+
src/collision_free_optimizer_node.cpp
28+
src/utils.cpp
29+
)
30+
31+
target_include_directories(main
32+
SYSTEM PUBLIC
33+
${OpenCV_INCLUDE_DIRS}
34+
${EIGEN3_INCLUDE_DIR}
35+
)
36+
37+
target_link_libraries(main
38+
${OpenCV_LIBRARIES}
39+
)
40+
41+
if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
42+
rosidl_target_interfaces(main
43+
static_centerline_optimizer "rosidl_typesupport_cpp")
44+
else()
45+
rosidl_get_typesupport_target(
46+
cpp_typesupport_target static_centerline_optimizer "rosidl_typesupport_cpp")
47+
target_link_libraries(main "${cpp_typesupport_target}")
48+
endif()
49+
50+
ament_auto_package(
51+
INSTALL_TO_SHARE
52+
launch
53+
config
54+
rviz
55+
)
56+
57+
install(PROGRAMS
58+
scripts/app.py
59+
DESTINATION lib/${PROJECT_NAME}
60+
)

‎planning/static_centerline_optimizer/README.md

+83
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
# Static Centerline Optimizer
2+
3+
## Purpose
4+
5+
This package statically calculates the centerline satisfying path footprints inside the drivable area.
6+
7+
On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area.
8+
To make path footprints inside the drivable area, we use online path shape optimization by [the obstacle_avoidance_planner package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/obstacle_avoidance_planner/).
9+
10+
Instead of online path shape optimization, we introduce static centerline optimization.
11+
With this static centerline optimization, we have following advantages.
12+
13+
- We can see the optimized centerline shape in advance.
14+
- With the default autoware, path shape is not determined until the vehicle drives there.
15+
- This enables offline path shape evaluation.
16+
- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area.
17+
18+
## Use cases
19+
20+
There are two interfaces to communicate with the centerline optimizer.
21+
22+
### Vector Map Builder Interface
23+
24+
Note: This function of Vector Map Builder has not been released. Please wait for a while.
25+
Currently there is no documentation about Vector Map Builder's operation for this function.
26+
27+
The optimized centerline can be generated from Vector Map Builder's operation.
28+
29+
We can run
30+
31+
- path planning server
32+
- http server to connect path planning server and Vector Map Builder
33+
34+
with the following command by designating `<vehicle_model>`
35+
36+
```sh
37+
ros2 launch static_centerline_optimizer run_planning_server.launch.xml vehicle_model:=<vehicle-model>
38+
```
39+
40+
FYI, port ID of the http server is 4010 by default.
41+
42+
### Command Line Interface
43+
44+
The optimized centerline can be generated from the command line interface by designating
45+
46+
- `<input-osm-path>`
47+
- `<output-osm-path>` (not mandatory)
48+
- `<start-lanelet-id>`
49+
- `<end-lanelet-id>`
50+
- `<vehicle-model>`
51+
52+
```sh
53+
ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_backgrond:=false lanelet2_input_file_path:=<input-osm-path> lanelet2_output_file_path:=<output-osm-path> start_lanelet_id:=<start-lane-id> end_lanelet_id:=<end-lane-id> vehicle_model:=<vehicle-model>
54+
```
55+
56+
The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`.
57+
If you want to change the output map path, you can remap the path by designating `<output-osm-path>`.
58+
59+
## Visualization
60+
61+
When launching the path planning server, rviz is launched as well as follows.
62+
![rviz](./media/rviz.png)
63+
64+
- The yellow footprints are the original ones from the osm map file.
65+
- FYI: Footprints are generated based on the centerline and vehicle size.
66+
- The red footprints are the optimized ones.
67+
- The gray area is the drivable area.
68+
- You can see that the red footprints are inside the drivable area although the yellow ones are outside.
69+
70+
### Unsafe footprints
71+
72+
Sometimes the optimized centerline footprints are close to the lanes' boundaries.
73+
We can check how close they are with `unsafe footprints` marker as follows.
74+
75+
Footprints' color depends on its distance to the boundaries, and text expresses its distance.
76+
77+
![rviz](./media/unsafe_footprints.png)
78+
79+
By default, footprints' color is
80+
81+
- when the distance is less than 0.1 [m] : red
82+
- when the distance is less than 0.2 [m] : green
83+
- when the distance is less than 0.3 [m] : blue

‎planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
/**:
2+
ros__parameters:
3+
marker_color: ["FF0000", "00FF00", "0000FF"]
4+
marker_color_dist_thresh : [0.1, 0.2, 0.3]

‎planning/static_centerline_optimizer/include/static_centerline_optimizer/collision_free_optimizer_node.hpp

+105
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
// Copyright 2022 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+
// NOTE: This file was copied from a part of implementation in
16+
// https://github.com/autowarefoundation/autoware.universe/blob/main/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
17+
18+
#ifndef STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_
19+
#define STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_
20+
21+
#include "motion_utils/motion_utils.hpp"
22+
#include "obstacle_avoidance_planner/common_structs.hpp"
23+
#include "obstacle_avoidance_planner/costmap_generator.hpp"
24+
#include "obstacle_avoidance_planner/eb_path_optimizer.hpp"
25+
#include "obstacle_avoidance_planner/mpt_optimizer.hpp"
26+
#include "rclcpp/clock.hpp"
27+
#include "rclcpp/rclcpp.hpp"
28+
#include "static_centerline_optimizer/type_alias.hpp"
29+
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
30+
31+
#include "boost/optional.hpp"
32+
33+
#include <algorithm>
34+
#include <memory>
35+
#include <string>
36+
#include <vector>
37+
38+
namespace static_centerline_optimizer
39+
{
40+
class CollisionFreeOptimizerNode : public rclcpp::Node
41+
{
42+
private:
43+
rclcpp::Clock logger_ros_clock_;
44+
int eb_solved_count_;
45+
bool is_driving_forward_{true};
46+
47+
bool is_publishing_debug_visualization_marker_;
48+
bool is_publishing_area_with_objects_;
49+
bool is_publishing_object_clearance_map_;
50+
bool is_publishing_clearance_map_;
51+
bool is_showing_debug_info_;
52+
bool is_showing_calculation_time_;
53+
bool is_stopping_if_outside_drivable_area_;
54+
bool enable_avoidance_;
55+
bool enable_pre_smoothing_;
56+
bool skip_optimization_;
57+
bool reset_prev_optimization_;
58+
59+
// vehicle circles info for for mpt constraints
60+
std::string vehicle_circle_method_;
61+
int vehicle_circle_num_for_calculation_;
62+
std::vector<double> vehicle_circle_radius_ratios_;
63+
64+
// params for replan
65+
double max_path_shape_change_dist_for_replan_;
66+
double max_ego_moving_dist_for_replan_;
67+
double max_delta_time_sec_for_replan_;
68+
69+
// logic
70+
std::unique_ptr<CostmapGenerator> costmap_generator_ptr_;
71+
std::unique_ptr<EBPathOptimizer> eb_path_optimizer_ptr_;
72+
std::unique_ptr<MPTOptimizer> mpt_optimizer_ptr_;
73+
74+
// params
75+
TrajectoryParam traj_param_;
76+
EBParam eb_param_;
77+
VehicleParam vehicle_param_;
78+
MPTParam mpt_param_;
79+
int mpt_visualize_sampling_num_;
80+
81+
// debug
82+
mutable DebugData debug_data_;
83+
84+
geometry_msgs::msg::Pose current_ego_pose_;
85+
std::unique_ptr<Trajectories> prev_optimal_trajs_ptr_;
86+
std::unique_ptr<std::vector<PathPoint>> prev_path_points_ptr_;
87+
88+
std::unique_ptr<rclcpp::Time> latest_replanned_time_ptr_;
89+
90+
// ROS
91+
rclcpp::Subscription<Path>::SharedPtr path_sub_;
92+
93+
// functions
94+
void resetPlanning();
95+
void resetPrevOptimization();
96+
97+
public:
98+
explicit CollisionFreeOptimizerNode(const rclcpp::NodeOptions & node_options);
99+
100+
// subscriber callback functions
101+
Trajectory pathCallback(const Path::ConstSharedPtr);
102+
};
103+
} // namespace static_centerline_optimizer
104+
105+
#endif // STATIC_CENTERLINE_OPTIMIZER__COLLISION_FREE_OPTIMIZER_NODE_HPP_

‎planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

+90
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
// Copyright 2022 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+
#ifndef STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
16+
#define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_
17+
18+
#include "rclcpp/rclcpp.hpp"
19+
#include "static_centerline_optimizer/srv/load_map.hpp"
20+
#include "static_centerline_optimizer/srv/plan_path.hpp"
21+
#include "static_centerline_optimizer/srv/plan_route.hpp"
22+
#include "static_centerline_optimizer/type_alias.hpp"
23+
#include "vehicle_info_util/vehicle_info_util.hpp"
24+
25+
#include <memory>
26+
#include <string>
27+
#include <vector>
28+
29+
namespace static_centerline_optimizer
30+
{
31+
using static_centerline_optimizer::srv::LoadMap;
32+
using static_centerline_optimizer::srv::PlanPath;
33+
using static_centerline_optimizer::srv::PlanRoute;
34+
35+
class StaticCenterlineOptimizerNode : public rclcpp::Node
36+
{
37+
public:
38+
enum class PlanPathResult {
39+
SUCCESS = 0,
40+
ROUTE_IS_NOT_READY = 1,
41+
};
42+
43+
explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options);
44+
void run();
45+
46+
private:
47+
// load map
48+
void load_map(const std::string & lanelet2_input_file_path);
49+
void on_load_map(
50+
const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response);
51+
52+
// plan route
53+
std::vector<unsigned int> plan_route(const int start_lanelet_id, const int end_lanelet_id);
54+
void on_plan_route(
55+
const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response);
56+
57+
// plan path
58+
void on_plan_path(
59+
const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
60+
PlanPathResult plan_path(const std::vector<unsigned int> & route_lane_ids);
61+
void evaluate(const std::vector<unsigned int> & route_lane_ids);
62+
63+
void save_map(
64+
const std::string & lanelet2_output_file_path,
65+
const std::vector<unsigned int> & route_lane_ids);
66+
67+
HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
68+
std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
69+
std::vector<TrajectoryPoint> optimized_traj_points_{};
70+
71+
// publisher
72+
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_{nullptr};
73+
rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
74+
rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
75+
rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_unsafe_footprints_{nullptr};
76+
rclcpp::Publisher<Trajectory>::SharedPtr pub_optimized_centerline_{nullptr};
77+
78+
// service
79+
rclcpp::Service<LoadMap>::SharedPtr srv_load_map_;
80+
rclcpp::Service<PlanRoute>::SharedPtr srv_plan_route_;
81+
rclcpp::Service<PlanPath>::SharedPtr srv_plan_path_;
82+
83+
// callback group for service
84+
rclcpp::CallbackGroup::SharedPtr callback_group_;
85+
86+
// vehicle info
87+
vehicle_info_util::VehicleInfo vehicle_info_;
88+
};
89+
} // namespace static_centerline_optimizer
90+
#endif // STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_

‎planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
// Copyright 2022 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+
#ifndef STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_
15+
#define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_
16+
17+
#include "route_handler/route_handler.hpp"
18+
19+
#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
20+
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
21+
#include "autoware_auto_planning_msgs/msg/had_map_route.hpp"
22+
#include "autoware_auto_planning_msgs/msg/path.hpp"
23+
#include "autoware_auto_planning_msgs/msg/path_point.hpp"
24+
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
25+
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
26+
#include "visualization_msgs/msg/marker_array.hpp"
27+
28+
namespace static_centerline_optimizer
29+
{
30+
using autoware_auto_mapping_msgs::msg::HADMapBin;
31+
using autoware_auto_perception_msgs::msg::PredictedObjects;
32+
using autoware_auto_planning_msgs::msg::HADMapRoute;
33+
using autoware_auto_planning_msgs::msg::Path;
34+
using autoware_auto_planning_msgs::msg::PathPoint;
35+
using autoware_auto_planning_msgs::msg::PathWithLaneId;
36+
using autoware_auto_planning_msgs::msg::Trajectory;
37+
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
38+
using route_handler::RouteHandler;
39+
using tier4_autoware_utils::LinearRing2d;
40+
using tier4_autoware_utils::LineString2d;
41+
using tier4_autoware_utils::Point2d;
42+
using visualization_msgs::msg::MarkerArray;
43+
} // namespace static_centerline_optimizer
44+
45+
#endif // STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_

0 commit comments

Comments
 (0)