Skip to content

Commit f0cd1a5

Browse files
authored
feat!: replace scenario msg from tier4_planning_msgs to autoware_internal_planning_msgs (autowarefoundation#10180)
Signed-off-by: Ryohsuke Mitsudome <[email protected]>
1 parent 769673c commit f0cd1a5

File tree

14 files changed

+51
-48
lines changed

14 files changed

+51
-48
lines changed

planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -59,10 +59,10 @@
5959
#include <rclcpp/rclcpp.hpp>
6060

6161
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
62+
#include <autoware_internal_planning_msgs/msg/scenario.hpp>
6263
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
6364
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
6465
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
65-
#include <tier4_planning_msgs/msg/scenario.hpp>
6666

6767
#include <grid_map_msgs/msg/grid_map.h>
6868
#include <message_filters/subscriber.h>
@@ -107,7 +107,8 @@ class CostmapGenerator : public rclcpp::Node
107107
sub_points_{this, "~/input/points_no_ground", autoware::universe_utils::SingleDepthSensorQoS()};
108108
autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> sub_objects_{
109109
this, "~/input/objects"};
110-
autoware::universe_utils::InterProcessPollingSubscriber<tier4_planning_msgs::msg::Scenario>
110+
autoware::universe_utils::InterProcessPollingSubscriber<
111+
autoware_internal_planning_msgs::msg::Scenario>
111112
sub_scenario_{this, "~/input/scenario"};
112113

113114
rclcpp::TimerBase::SharedPtr timer_;
@@ -120,7 +121,7 @@ class CostmapGenerator : public rclcpp::Node
120121
PointsToCostmap points2costmap_{};
121122
ObjectsToCostmap objects2costmap_;
122123

123-
tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_;
124+
autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr scenario_;
124125

125126
struct LayerName
126127
{

planning/autoware_costmap_generator/src/costmap_generator.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -335,7 +335,7 @@ bool CostmapGenerator::isActive()
335335
if (!scenario_) return false;
336336
const auto & s = scenario_->activating_scenarios;
337337
return std::any_of(s.begin(), s.end(), [](const auto scenario) {
338-
return scenario == tier4_planning_msgs::msg::Scenario::PARKING;
338+
return scenario == autoware_internal_planning_msgs::msg::Scenario::PARKING;
339339
});
340340
}
341341

planning/autoware_costmap_generator/test/test_costmap_generator.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
#include <vector>
2626

2727
using autoware::costmap_generator::CostmapGenerator;
28-
using tier4_planning_msgs::msg::Scenario;
28+
using autoware_internal_planning_msgs::msg::Scenario;
2929

3030
class TestCostmapGenerator : public ::testing::Test
3131
{

planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -40,13 +40,13 @@
4040
#include <rclcpp/rclcpp.hpp>
4141

4242
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
43+
#include <autoware_internal_planning_msgs/msg/scenario.hpp>
4344
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
4445
#include <autoware_planning_msgs/msg/trajectory.hpp>
4546
#include <geometry_msgs/msg/twist.hpp>
4647
#include <nav_msgs/msg/occupancy_grid.hpp>
4748
#include <nav_msgs/msg/odometry.hpp>
4849
#include <std_msgs/msg/bool.hpp>
49-
#include <tier4_planning_msgs/msg/scenario.hpp>
5050

5151
#ifdef ROS_DISTRO_GALACTIC
5252
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -76,6 +76,7 @@ using autoware::freespace_planning_algorithms::PlannerCommonParam;
7676
using autoware::freespace_planning_algorithms::RRTStar;
7777
using autoware::freespace_planning_algorithms::RRTStarParam;
7878
using autoware::freespace_planning_algorithms::VehicleShape;
79+
using autoware_internal_planning_msgs::msg::Scenario;
7980
using autoware_planning_msgs::msg::LaneletRoute;
8081
using autoware_planning_msgs::msg::Trajectory;
8182
using geometry_msgs::msg::PoseArray;
@@ -84,7 +85,6 @@ using geometry_msgs::msg::TransformStamped;
8485
using geometry_msgs::msg::Twist;
8586
using nav_msgs::msg::OccupancyGrid;
8687
using nav_msgs::msg::Odometry;
87-
using tier4_planning_msgs::msg::Scenario;
8888

8989
struct NodeParam
9090
{

planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -19,13 +19,13 @@
1919

2020
#include <rclcpp/rclcpp.hpp>
2121

22+
#include <autoware_internal_planning_msgs/msg/scenario.hpp>
2223
#include <autoware_planning_msgs/msg/trajectory.hpp>
2324
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
2425
#include <geometry_msgs/msg/pose_array.hpp>
2526
#include <geometry_msgs/msg/pose_stamped.hpp>
2627
#include <geometry_msgs/msg/transform_stamped.hpp>
2728
#include <nav_msgs/msg/odometry.hpp>
28-
#include <tier4_planning_msgs/msg/scenario.hpp>
2929

3030
#include <deque>
3131
#include <vector>
@@ -34,14 +34,14 @@ namespace autoware::freespace_planner::utils
3434
{
3535
using autoware::freespace_planning_algorithms::PlannerWaypoint;
3636
using autoware::freespace_planning_algorithms::PlannerWaypoints;
37+
using autoware_internal_planning_msgs::msg::Scenario;
3738
using autoware_planning_msgs::msg::Trajectory;
3839
using autoware_planning_msgs::msg::TrajectoryPoint;
3940
using geometry_msgs::msg::Pose;
4041
using geometry_msgs::msg::PoseArray;
4142
using geometry_msgs::msg::PoseStamped;
4243
using geometry_msgs::msg::TransformStamped;
4344
using nav_msgs::msg::Odometry;
44-
using tier4_planning_msgs::msg::Scenario;
4545

4646
PoseArray trajectory_to_pose_array(const Trajectory & trajectory);
4747

planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ void publishMandatoryTopics(
6363
autoware::test_utils::makeCostMapMsg());
6464
test_manager->publishInput(
6565
test_target_node, "freespace_planner/input/scenario",
66-
autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::PARKING));
66+
autoware::test_utils::makeScenarioMsg(autoware_internal_planning_msgs::msg::Scenario::PARKING));
6767
}
6868

6969
// the following tests are disable because they randomly fail

planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode(
5858
sub_planning_velocity_ = create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
5959
"/planning/scenario_planning/current_max_velocity", qos_transient_local,
6060
std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1));
61-
sub_scenario_ = this->create_subscription<tier4_planning_msgs::msg::Scenario>(
61+
sub_scenario_ = this->create_subscription<autoware_internal_planning_msgs::msg::Scenario>(
6262
"~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1));
6363

6464
pub_mission_remaining_distance_time_ = create_publisher<MissionRemainingDistanceTime>(
@@ -106,7 +106,7 @@ void RemainingDistanceTimeCalculatorNode::on_velocity_limit(
106106
}
107107

108108
void RemainingDistanceTimeCalculatorNode::on_scenario(
109-
const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg)
109+
const autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr & msg)
110110
{
111111
scenario_ = msg;
112112
has_received_scenario_ = true;
@@ -119,7 +119,7 @@ void RemainingDistanceTimeCalculatorNode::on_timer()
119119
}
120120

121121
// check if the scenario is parking or not
122-
if (scenario_->current_scenario == tier4_planning_msgs::msg::Scenario::PARKING) {
122+
if (scenario_->current_scenario == autoware_internal_planning_msgs::msg::Scenario::PARKING) {
123123
remaining_distance_ = 0.0;
124124
remaining_time_ = 0.0;
125125
publish_mission_remaining_distance_time();

planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -20,12 +20,12 @@
2020
#include <remaining_distance_time_calculator_parameters.hpp>
2121

2222
#include <autoware_internal_msgs/msg/mission_remaining_distance_time.hpp>
23+
#include <autoware_internal_planning_msgs/msg/scenario.hpp>
2324
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
2425
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2526
#include <geometry_msgs/msg/pose.hpp>
2627
#include <geometry_msgs/msg/vector3.hpp>
2728
#include <nav_msgs/msg/odometry.hpp>
28-
#include <tier4_planning_msgs/msg/scenario.hpp>
2929
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
3030

3131
#include <lanelet2_core/Forward.h>
@@ -56,7 +56,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
5656
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
5757
rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
5858
rclcpp::Subscription<VelocityLimit>::SharedPtr sub_planning_velocity_;
59-
rclcpp::Subscription<tier4_planning_msgs::msg::Scenario>::SharedPtr sub_scenario_;
59+
rclcpp::Subscription<autoware_internal_planning_msgs::msg::Scenario>::SharedPtr sub_scenario_;
6060

6161
rclcpp::Publisher<MissionRemainingDistanceTime>::SharedPtr pub_mission_remaining_distance_time_;
6262

@@ -73,7 +73,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
7373
geometry_msgs::msg::Pose current_vehicle_pose_;
7474
geometry_msgs::msg::Vector3 current_vehicle_velocity_;
7575
geometry_msgs::msg::Pose goal_pose_;
76-
tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_;
76+
autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr scenario_;
7777
bool has_received_route_;
7878
bool has_received_scenario_;
7979
double velocity_limit_;
@@ -90,7 +90,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
9090
void on_route(const LaneletRoute::ConstSharedPtr & msg);
9191
void on_map(const HADMapBin::ConstSharedPtr & msg);
9292
void on_velocity_limit(const VelocityLimit::ConstSharedPtr & msg);
93-
void on_scenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg);
93+
void on_scenario(const autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr & msg);
9494

9595
/**
9696
* @brief calculate mission remaining distance

planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -20,13 +20,13 @@
2020

2121
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
2222
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
23+
#include <autoware_internal_planning_msgs/msg/scenario.hpp>
2324
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
2425
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
2526
#include <autoware_planning_msgs/msg/trajectory.hpp>
2627
#include <geometry_msgs/msg/twist_stamped.hpp>
2728
#include <nav_msgs/msg/odometry.hpp>
2829
#include <std_msgs/msg/bool.hpp>
29-
#include <tier4_planning_msgs/msg/scenario.hpp>
3030

3131
#include <lanelet2_core/LaneletMap.h>
3232
#include <lanelet2_routing/RoutingGraph.h>
@@ -79,12 +79,12 @@ class ScenarioSelectorNode : public rclcpp::Node
7979

8080
inline bool isCurrentLaneDriving() const
8181
{
82-
return current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING;
82+
return current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING;
8383
}
8484

8585
inline bool isCurrentParking() const
8686
{
87-
return current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING;
87+
return current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::PARKING;
8888
}
8989

9090
rclcpp::TimerBase::SharedPtr timer_;
@@ -96,7 +96,7 @@ class ScenarioSelectorNode : public rclcpp::Node
9696
sub_lane_driving_trajectory_;
9797
rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_parking_trajectory_;
9898
rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
99-
rclcpp::Publisher<tier4_planning_msgs::msg::Scenario>::SharedPtr pub_scenario_;
99+
rclcpp::Publisher<autoware_internal_planning_msgs::msg::Scenario>::SharedPtr pub_scenario_;
100100
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
101101
pub_processing_time_;
102102

planning/autoware_scenario_selector/src/node.cpp

+23-22
Original file line numberDiff line numberDiff line change
@@ -132,10 +132,10 @@ bool isStopped(
132132
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr ScenarioSelectorNode::getScenarioTrajectory(
133133
const std::string & scenario)
134134
{
135-
if (scenario == tier4_planning_msgs::msg::Scenario::LANEDRIVING) {
135+
if (scenario == autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING) {
136136
return lane_driving_trajectory_;
137137
}
138-
if (scenario == tier4_planning_msgs::msg::Scenario::PARKING) {
138+
if (scenario == autoware_internal_planning_msgs::msg::Scenario::PARKING) {
139139
return parking_trajectory_;
140140
}
141141
RCLCPP_ERROR_STREAM(this->get_logger(), "invalid scenario argument: " << scenario);
@@ -151,25 +151,25 @@ std::string ScenarioSelectorNode::selectScenarioByPosition()
151151
const auto is_in_parking_lot =
152152
isInParkingLot(route_handler_->getLaneletMapPtr(), current_pose_->pose.pose);
153153

154-
if (current_scenario_ == tier4_planning_msgs::msg::Scenario::EMPTY) {
154+
if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::EMPTY) {
155155
if (is_in_lane && is_goal_in_lane) {
156-
return tier4_planning_msgs::msg::Scenario::LANEDRIVING;
156+
return autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING;
157157
} else if (is_in_parking_lot) {
158-
return tier4_planning_msgs::msg::Scenario::PARKING;
158+
return autoware_internal_planning_msgs::msg::Scenario::PARKING;
159159
}
160-
return tier4_planning_msgs::msg::Scenario::LANEDRIVING;
160+
return autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING;
161161
}
162162

163-
if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) {
163+
if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING) {
164164
if (is_in_parking_lot && !is_goal_in_lane) {
165-
return tier4_planning_msgs::msg::Scenario::PARKING;
165+
return autoware_internal_planning_msgs::msg::Scenario::PARKING;
166166
}
167167
}
168168

169-
if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) {
169+
if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::PARKING) {
170170
if (is_parking_completed_ && is_in_lane) {
171171
is_parking_completed_ = false;
172-
return tier4_planning_msgs::msg::Scenario::LANEDRIVING;
172+
return autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING;
173173
}
174174
}
175175

@@ -193,11 +193,12 @@ void ScenarioSelectorNode::updateCurrentScenario()
193193
if (enable_mode_switching_) {
194194
if (isCurrentLaneDriving()) {
195195
current_scenario_ = isSwitchToParking(is_stopped)
196-
? tier4_planning_msgs::msg::Scenario::PARKING
196+
? autoware_internal_planning_msgs::msg::Scenario::PARKING
197197
: current_scenario_;
198198
} else if (isCurrentParking()) {
199-
current_scenario_ = isSwitchToLaneDriving() ? tier4_planning_msgs::msg::Scenario::LANEDRIVING
200-
: current_scenario_;
199+
current_scenario_ = isSwitchToLaneDriving()
200+
? autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING
201+
: current_scenario_;
201202
}
202203
}
203204

@@ -274,7 +275,7 @@ void ScenarioSelectorNode::onRoute(
274275
// When the route id is the same (e.g. rerouting with modified goal) keep the current scenario.
275276
// Otherwise, reset the scenario.
276277
if (!route_handler_ || route_handler_->getRouteUuid() != msg->uuid) {
277-
current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY;
278+
current_scenario_ = autoware_internal_planning_msgs::msg::Scenario::EMPTY;
278279
}
279280

280281
route_ = msg;
@@ -375,15 +376,15 @@ void ScenarioSelectorNode::onTimer()
375376
}
376377

377378
// Initialize Scenario
378-
if (current_scenario_ == tier4_planning_msgs::msg::Scenario::EMPTY) {
379+
if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::EMPTY) {
379380
current_scenario_ = selectScenarioByPosition();
380381
}
381382

382383
updateCurrentScenario();
383-
tier4_planning_msgs::msg::Scenario scenario;
384+
autoware_internal_planning_msgs::msg::Scenario scenario;
384385
scenario.current_scenario = current_scenario_;
385386

386-
if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) {
387+
if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::PARKING) {
387388
scenario.activating_scenarios.push_back(current_scenario_);
388389
}
389390

@@ -401,7 +402,7 @@ void ScenarioSelectorNode::onLaneDrivingTrajectory(
401402
{
402403
lane_driving_trajectory_ = msg;
403404

404-
if (current_scenario_ != tier4_planning_msgs::msg::Scenario::LANEDRIVING) {
405+
if (current_scenario_ != autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING) {
405406
return;
406407
}
407408

@@ -413,7 +414,7 @@ void ScenarioSelectorNode::onParkingTrajectory(
413414
{
414415
parking_trajectory_ = msg;
415416

416-
if (current_scenario_ != tier4_planning_msgs::msg::Scenario::PARKING) {
417+
if (current_scenario_ != autoware_internal_planning_msgs::msg::Scenario::PARKING) {
417418
return;
418419
}
419420

@@ -438,7 +439,7 @@ void ScenarioSelectorNode::publishTrajectory(
438439

439440
ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_options)
440441
: Node("scenario_selector", node_options),
441-
current_scenario_(tier4_planning_msgs::msg::Scenario::EMPTY),
442+
current_scenario_(autoware_internal_planning_msgs::msg::Scenario::EMPTY),
442443
update_rate_(this->declare_parameter<double>("update_rate")),
443444
th_max_message_delay_sec_(this->declare_parameter<double>("th_max_message_delay_sec")),
444445
th_arrived_distance_m_(this->declare_parameter<double>("th_arrived_distance_m")),
@@ -478,8 +479,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti
478479
this, "input/operation_mode_state", rclcpp::QoS{1});
479480

480481
// Output
481-
pub_scenario_ =
482-
this->create_publisher<tier4_planning_msgs::msg::Scenario>("output/scenario", rclcpp::QoS{1});
482+
pub_scenario_ = this->create_publisher<autoware_internal_planning_msgs::msg::Scenario>(
483+
"output/scenario", rclcpp::QoS{1});
483484
pub_trajectory_ = this->create_publisher<autoware_planning_msgs::msg::Trajectory>(
484485
"output/trajectory", rclcpp::QoS{1});
485486

planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
3434
auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
3535

3636
// set subscriber with topic name: scenario_selector → test_node_
37-
test_manager->subscribeOutput<tier4_planning_msgs::msg::Scenario>("output/scenario");
37+
test_manager->subscribeOutput<autoware_internal_planning_msgs::msg::Scenario>("output/scenario");
3838

3939
return test_manager;
4040
}

planning/behavior_path_planner/autoware_behavior_path_planner/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ The Planner Manager's responsibilities include:
9898
| ~/input/traffic_signals || `autoware_perception_msgs::msg::TrafficLightGroupArray` | Traffic signal information from the perception module |
9999
| ~/input/vector_map || `autoware_map_msgs::msg::LaneletMapBin` | Vector map information |
100100
| ~/input/route || `autoware_planning_msgs::msg::LaneletRoute` | Current route from start to goal |
101-
| ~/input/scenario || `tier4_planning_msgs::msg::Scenario` | Launches Behavior Path Planner if current scenario == `Scenario:LaneDriving` |
101+
| ~/input/scenario || `autoware_internal_planning_msgs::msg::Scenario` | Launches Behavior Path Planner if current scenario == `Scenario:LaneDriving` |
102102
| ~/input/lateral_offset || `tier4_planning_msgs::msg::LateralOffset` | Lateral offset to trigger side shift |
103103
| ~/system/operation_mode/state || `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows the planning module to know if vehicle is in autonomous mode or if it can be controlled<sup>[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md)</sup> |
104104

0 commit comments

Comments
 (0)