Skip to content

Commit c629998

Browse files
isamu-takagiStepTurtle
isamu-takagi
authored and
StepTurtle
committed
feat(mission_planner)!: introduce route_selector node (autowarefoundation#6363)
* feat(mission_planner): introduce route_selector node Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * remove unused file Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix use goal pose only when resuming Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: change mrm mode if route set is successful Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * add interrupted state Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix mrm set route uuid Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * remove unused reference Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * add resume route function Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * try to resume planned route Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * remove debug code Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * use full license text instead of spdx Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 557a798 commit c629998

File tree

12 files changed

+829
-707
lines changed

12 files changed

+829
-707
lines changed

‎launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml

+5-14
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,10 @@
11
<launch>
2-
<arg name="modified_goal_topic_name" default="/planning/scenario_planning/modified_goal"/>
3-
<arg name="map_topic_name" default="/map/vector_map"/>
4-
<arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
52
<arg name="mission_planner_param_path" default="$(find-pkg-share mission_planner)/config/mission_planner.param.yaml"/>
6-
7-
<node_container pkg="rclcpp_components" exec="component_container" name="mission_planner_container" namespace="">
8-
<composable_node pkg="mission_planner" plugin="mission_planner::MissionPlanner" name="mission_planner" namespace="">
9-
<param from="$(var mission_planner_param_path)"/>
10-
<remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
11-
<remap from="input/vector_map" to="$(var map_topic_name)"/>
12-
<remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
13-
</composable_node>
14-
<composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
15-
</node_container>
16-
3+
<group>
4+
<include file="$(find-pkg-share mission_planner)/launch/mission_planner.launch.xml">
5+
<arg name="mission_planner_param_path" value="$(var mission_planner_param_path)"/>
6+
</include>
7+
</group>
178
<group>
189
<include file="$(find-pkg-share mission_planner)/launch/goal_pose_visualizer.launch.xml"/>
1910
</group>

‎planning/mission_planner/CMakeLists.txt

+12-5
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,20 @@ rclcpp_components_register_node(goal_pose_visualizer_component
1414
)
1515

1616
ament_auto_add_library(${PROJECT_NAME}_component SHARED
17-
src/mission_planner/mission_planner.cpp
1817
src/mission_planner/arrival_checker.cpp
18+
src/mission_planner/service_utils.cpp
19+
src/mission_planner/mission_planner.cpp
20+
src/mission_planner/route_selector.cpp
1921
)
2022

2123
rclcpp_components_register_node(${PROJECT_NAME}_component
2224
PLUGIN "mission_planner::MissionPlanner"
23-
EXECUTABLE ${PROJECT_NAME}
25+
EXECUTABLE mission_planner
26+
)
27+
28+
rclcpp_components_register_node(${PROJECT_NAME}_component
29+
PLUGIN "mission_planner::RouteSelector"
30+
EXECUTABLE route_selector
2431
)
2532

2633
ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED
@@ -30,7 +37,7 @@ ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED
3037
pluginlib_export_plugin_description_file(mission_planner plugins/plugin_description.xml)
3138

3239
ament_auto_package(
33-
INSTALL_TO_SHARE
34-
config
35-
launch
40+
INSTALL_TO_SHARE
41+
config
42+
launch
3643
)

‎planning/mission_planner/include/mission_planner/mission_planner_interface.hpp

-50
This file was deleted.

‎planning/mission_planner/launch/mission_planner.launch.xml

+20-6
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,24 @@
44
<arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
55
<arg name="mission_planner_param_path" default="$(find-pkg-share mission_planner)/config/mission_planner.param.yaml"/>
66

7-
<node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
8-
<param from="$(var mission_planner_param_path)"/>
9-
<remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
10-
<remap from="input/vector_map" to="$(var map_topic_name)"/>
11-
<remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
12-
</node>
7+
<node_container pkg="rclcpp_components" exec="component_container_mt" name="mission_planner_container" namespace="">
8+
<composable_node pkg="mission_planner" plugin="mission_planner::MissionPlanner" name="mission_planner" namespace="">
9+
<param from="$(var mission_planner_param_path)"/>
10+
<remap from="~/input/modified_goal" to="$(var modified_goal_topic_name)"/>
11+
<remap from="~/input/vector_map" to="$(var map_topic_name)"/>
12+
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
13+
<remap from="~/input/reroute_availability" to="/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/is_reroute_available"/>
14+
<remap from="~/route" to="route"/>
15+
<remap from="~/state" to="state"/>
16+
<remap from="~/debug/route_marker" to="$(var visualization_topic_name)"/>
17+
</composable_node>
18+
<composable_node pkg="mission_planner" plugin="mission_planner::RouteSelector" name="route_selector" namespace="">
19+
<remap from="~/planner/clear_route" to="mission_planner/clear_route"/>
20+
<remap from="~/planner/set_lanelet_route" to="mission_planner/set_lanelet_route"/>
21+
<remap from="~/planner/set_waypoint_route" to="mission_planner/set_waypoint_route"/>
22+
<remap from="~/planner/route" to="route"/>
23+
<remap from="~/planner/state" to="state"/>
24+
</composable_node>
25+
<composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
26+
</node_container>
1327
</launch>

‎planning/mission_planner/package.xml

+1-4
Original file line numberDiff line numberDiff line change
@@ -18,18 +18,15 @@
1818
<buildtool_depend>autoware_cmake</buildtool_depend>
1919

2020
<depend>autoware_adapi_v1_msgs</depend>
21-
<depend>autoware_auto_planning_msgs</depend>
21+
<depend>autoware_auto_mapping_msgs</depend>
2222
<depend>autoware_planning_msgs</depend>
23-
<depend>component_interface_specs</depend>
24-
<depend>component_interface_utils</depend>
2523
<depend>geometry_msgs</depend>
2624
<depend>lanelet2_extension</depend>
2725
<depend>motion_utils</depend>
2826
<depend>pluginlib</depend>
2927
<depend>rclcpp</depend>
3028
<depend>rclcpp_components</depend>
3129
<depend>route_handler</depend>
32-
<depend>std_srvs</depend>
3330
<depend>tf2_geometry_msgs</depend>
3431
<depend>tf2_ros</depend>
3532
<depend>tier4_autoware_utils</depend>

‎planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node)
150150

151151
const auto durable_qos = rclcpp::QoS(1).transient_local();
152152
pub_goal_footprint_marker_ =
153-
node_->create_publisher<MarkerArray>("debug/goal_footprint", durable_qos);
153+
node_->create_publisher<MarkerArray>("~/debug/goal_footprint", durable_qos);
154154

155155
vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
156156
param_.goal_angle_threshold_deg = node_->declare_parameter<double>("goal_angle_threshold_deg");
@@ -164,7 +164,7 @@ void DefaultPlanner::initialize(rclcpp::Node * node)
164164
{
165165
initialize_common(node);
166166
map_subscriber_ = node_->create_subscription<HADMapBin>(
167-
"input/vector_map", rclcpp::QoS{10}.transient_local(),
167+
"~/input/vector_map", rclcpp::QoS{10}.transient_local(),
168168
std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1));
169169
}
170170

0 commit comments

Comments
 (0)