diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index b6749117a6177..350ad3fbdd3e4 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -59,10 +59,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -107,7 +107,8 @@ class CostmapGenerator : public rclcpp::Node sub_points_{this, "~/input/points_no_ground", autoware::universe_utils::SingleDepthSensorQoS()}; autoware::universe_utils::InterProcessPollingSubscriber sub_objects_{ this, "~/input/objects"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_internal_planning_msgs::msg::Scenario> sub_scenario_{this, "~/input/scenario"}; rclcpp::TimerBase::SharedPtr timer_; @@ -120,7 +121,7 @@ class CostmapGenerator : public rclcpp::Node PointsToCostmap points2costmap_{}; ObjectsToCostmap objects2costmap_; - tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; + autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; struct LayerName { diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 70b8051bcdc12..200a68eb5ec42 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -335,7 +335,7 @@ bool CostmapGenerator::isActive() if (!scenario_) return false; const auto & s = scenario_->activating_scenarios; return std::any_of(s.begin(), s.end(), [](const auto scenario) { - return scenario == tier4_planning_msgs::msg::Scenario::PARKING; + return scenario == autoware_internal_planning_msgs::msg::Scenario::PARKING; }); } diff --git a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp index f0db05b6fdd6f..aba7c07094d3a 100644 --- a/planning/autoware_costmap_generator/test/test_costmap_generator.cpp +++ b/planning/autoware_costmap_generator/test/test_costmap_generator.cpp @@ -25,7 +25,7 @@ #include using autoware::costmap_generator::CostmapGenerator; -using tier4_planning_msgs::msg::Scenario; +using autoware_internal_planning_msgs::msg::Scenario; class TestCostmapGenerator : public ::testing::Test { diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index bd431372b70dc..2d8cfe241ae6b 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -40,13 +40,13 @@ #include #include +#include #include #include #include #include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -76,6 +76,7 @@ using autoware::freespace_planning_algorithms::PlannerCommonParam; using autoware::freespace_planning_algorithms::RRTStar; using autoware::freespace_planning_algorithms::RRTStarParam; using autoware::freespace_planning_algorithms::VehicleShape; +using autoware_internal_planning_msgs::msg::Scenario; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::PoseArray; @@ -84,7 +85,6 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::Scenario; struct NodeParam { diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp index 1f2347958cfd2..9835b41c2328c 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp @@ -19,13 +19,13 @@ #include +#include #include #include #include #include #include #include -#include #include #include @@ -34,6 +34,7 @@ namespace autoware::freespace_planner::utils { using autoware::freespace_planning_algorithms::PlannerWaypoint; using autoware::freespace_planning_algorithms::PlannerWaypoints; +using autoware_internal_planning_msgs::msg::Scenario; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; @@ -41,7 +42,6 @@ using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::Scenario; PoseArray trajectory_to_pose_array(const Trajectory & trajectory); diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 020a184224a02..48df36d309d9a 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -63,7 +63,7 @@ void publishMandatoryTopics( autoware::test_utils::makeCostMapMsg()); test_manager->publishInput( test_target_node, "freespace_planner/input/scenario", - autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::PARKING)); + autoware::test_utils::makeScenarioMsg(autoware_internal_planning_msgs::msg::Scenario::PARKING)); } // the following tests are disable because they randomly fail diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 74f3f2f43077d..778e7ab30538f 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -58,7 +58,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( sub_planning_velocity_ = create_subscription( "/planning/scenario_planning/current_max_velocity", qos_transient_local, std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1)); - sub_scenario_ = this->create_subscription( + sub_scenario_ = this->create_subscription( "~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1)); pub_mission_remaining_distance_time_ = create_publisher( @@ -106,7 +106,7 @@ void RemainingDistanceTimeCalculatorNode::on_velocity_limit( } void RemainingDistanceTimeCalculatorNode::on_scenario( - const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg) + const autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr & msg) { scenario_ = msg; has_received_scenario_ = true; @@ -119,7 +119,7 @@ void RemainingDistanceTimeCalculatorNode::on_timer() } // check if the scenario is parking or not - if (scenario_->current_scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + if (scenario_->current_scenario == autoware_internal_planning_msgs::msg::Scenario::PARKING) { remaining_distance_ = 0.0; remaining_time_ = 0.0; publish_mission_remaining_distance_time(); diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index 8a38941d383ee..6c5ff165436a6 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -20,12 +20,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include @@ -56,7 +56,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_planning_velocity_; - rclcpp::Subscription::SharedPtr sub_scenario_; + rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::Publisher::SharedPtr pub_mission_remaining_distance_time_; @@ -73,7 +73,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node geometry_msgs::msg::Pose current_vehicle_pose_; geometry_msgs::msg::Vector3 current_vehicle_velocity_; geometry_msgs::msg::Pose goal_pose_; - tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; + autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; bool has_received_route_; bool has_received_scenario_; double velocity_limit_; @@ -90,7 +90,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node void on_route(const LaneletRoute::ConstSharedPtr & msg); void on_map(const HADMapBin::ConstSharedPtr & msg); void on_velocity_limit(const VelocityLimit::ConstSharedPtr & msg); - void on_scenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg); + void on_scenario(const autoware_internal_planning_msgs::msg::Scenario::ConstSharedPtr & msg); /** * @brief calculate mission remaining distance diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 7e215fcfef8dd..c74f39d50c93d 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -20,13 +20,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #include @@ -79,12 +79,12 @@ class ScenarioSelectorNode : public rclcpp::Node inline bool isCurrentLaneDriving() const { - return current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING; } inline bool isCurrentParking() const { - return current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING; + return current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::PARKING; } rclcpp::TimerBase::SharedPtr timer_; @@ -96,7 +96,7 @@ class ScenarioSelectorNode : public rclcpp::Node sub_lane_driving_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; - rclcpp::Publisher::SharedPtr pub_scenario_; + rclcpp::Publisher::SharedPtr pub_scenario_; rclcpp::Publisher::SharedPtr pub_processing_time_; diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 91e86bfea6dba..dcb87b7487fe0 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -132,10 +132,10 @@ bool isStopped( autoware_planning_msgs::msg::Trajectory::ConstSharedPtr ScenarioSelectorNode::getScenarioTrajectory( const std::string & scenario) { - if (scenario == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { + if (scenario == autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING) { return lane_driving_trajectory_; } - if (scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + if (scenario == autoware_internal_planning_msgs::msg::Scenario::PARKING) { return parking_trajectory_; } RCLCPP_ERROR_STREAM(this->get_logger(), "invalid scenario argument: " << scenario); @@ -151,25 +151,25 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() const auto is_in_parking_lot = isInParkingLot(route_handler_->getLaneletMapPtr(), current_pose_->pose.pose); - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::EMPTY) { + if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::EMPTY) { if (is_in_lane && is_goal_in_lane) { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING; } else if (is_in_parking_lot) { - return tier4_planning_msgs::msg::Scenario::PARKING; + return autoware_internal_planning_msgs::msg::Scenario::PARKING; } - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING; } - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { + if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING) { if (is_in_parking_lot && !is_goal_in_lane) { - return tier4_planning_msgs::msg::Scenario::PARKING; + return autoware_internal_planning_msgs::msg::Scenario::PARKING; } } - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) { + if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::PARKING) { if (is_parking_completed_ && is_in_lane) { is_parking_completed_ = false; - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; + return autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING; } } @@ -193,11 +193,12 @@ void ScenarioSelectorNode::updateCurrentScenario() if (enable_mode_switching_) { if (isCurrentLaneDriving()) { current_scenario_ = isSwitchToParking(is_stopped) - ? tier4_planning_msgs::msg::Scenario::PARKING + ? autoware_internal_planning_msgs::msg::Scenario::PARKING : current_scenario_; } else if (isCurrentParking()) { - current_scenario_ = isSwitchToLaneDriving() ? tier4_planning_msgs::msg::Scenario::LANEDRIVING - : current_scenario_; + current_scenario_ = isSwitchToLaneDriving() + ? autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING + : current_scenario_; } } @@ -274,7 +275,7 @@ void ScenarioSelectorNode::onRoute( // When the route id is the same (e.g. rerouting with modified goal) keep the current scenario. // Otherwise, reset the scenario. if (!route_handler_ || route_handler_->getRouteUuid() != msg->uuid) { - current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; + current_scenario_ = autoware_internal_planning_msgs::msg::Scenario::EMPTY; } route_ = msg; @@ -375,15 +376,15 @@ void ScenarioSelectorNode::onTimer() } // Initialize Scenario - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::EMPTY) { + if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::EMPTY) { current_scenario_ = selectScenarioByPosition(); } updateCurrentScenario(); - tier4_planning_msgs::msg::Scenario scenario; + autoware_internal_planning_msgs::msg::Scenario scenario; scenario.current_scenario = current_scenario_; - if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) { + if (current_scenario_ == autoware_internal_planning_msgs::msg::Scenario::PARKING) { scenario.activating_scenarios.push_back(current_scenario_); } @@ -401,7 +402,7 @@ void ScenarioSelectorNode::onLaneDrivingTrajectory( { lane_driving_trajectory_ = msg; - if (current_scenario_ != tier4_planning_msgs::msg::Scenario::LANEDRIVING) { + if (current_scenario_ != autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING) { return; } @@ -413,7 +414,7 @@ void ScenarioSelectorNode::onParkingTrajectory( { parking_trajectory_ = msg; - if (current_scenario_ != tier4_planning_msgs::msg::Scenario::PARKING) { + if (current_scenario_ != autoware_internal_planning_msgs::msg::Scenario::PARKING) { return; } @@ -438,7 +439,7 @@ void ScenarioSelectorNode::publishTrajectory( ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_options) : Node("scenario_selector", node_options), - current_scenario_(tier4_planning_msgs::msg::Scenario::EMPTY), + current_scenario_(autoware_internal_planning_msgs::msg::Scenario::EMPTY), update_rate_(this->declare_parameter("update_rate")), th_max_message_delay_sec_(this->declare_parameter("th_max_message_delay_sec")), th_arrived_distance_m_(this->declare_parameter("th_arrived_distance_m")), @@ -478,8 +479,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, "input/operation_mode_state", rclcpp::QoS{1}); // Output - pub_scenario_ = - this->create_publisher("output/scenario", rclcpp::QoS{1}); + pub_scenario_ = this->create_publisher( + "output/scenario", rclcpp::QoS{1}); pub_trajectory_ = this->create_publisher( "output/trajectory", rclcpp::QoS{1}); diff --git a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index 0256e7463b5a7..693bdf1c20009 100644 --- a/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -34,7 +34,7 @@ std::shared_ptr generateTestManager() auto test_manager = std::make_shared(); // set subscriber with topic name: scenario_selector → test_node_ - test_manager->subscribeOutput("output/scenario"); + test_manager->subscribeOutput("output/scenario"); return test_manager; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md index dd51d7d33035e..7300353e47165 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -98,7 +98,7 @@ The Planner Manager's responsibilities include: | ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Traffic signal information from the perception module | | ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | Vector map information | | ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | Current route from start to goal | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches Behavior Path Planner if current scenario == `Scenario:LaneDriving` | +| ~/input/scenario | ○ | `autoware_internal_planning_msgs::msg::Scenario` | Launches Behavior Path Planner if current scenario == `Scenario:LaneDriving` | | ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | Lateral offset to trigger side shift | | ~/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[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index bf87102134da7..281d6e345d51a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -40,7 +41,6 @@ #include #include #include -#include #include #include @@ -55,6 +55,7 @@ namespace autoware::behavior_path_planner using autoware::planning_factor_interface::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_internal_planning_msgs::msg::Scenario; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -70,7 +71,6 @@ using rcl_interfaces::msg::SetParametersResult; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::RerouteAvailability; -using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using DebugPublisher = autoware::universe_utils::DebugPublisher; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp index 26dc1589e679b..e1e088c098e63 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp @@ -97,7 +97,8 @@ void publishMandatoryTopics( autoware::test_utils::makeCostMapMsg()); test_manager->publishInput( test_target_node, "behavior_path_planner/input/scenario", - autoware::test_utils::makeScenarioMsg(tier4_planning_msgs::msg::Scenario::LANEDRIVING)); + autoware::test_utils::makeScenarioMsg( + autoware_internal_planning_msgs::msg::Scenario::LANEDRIVING)); test_manager->publishInput( test_target_node, "behavior_path_planner/input/vector_map", autoware::test_utils::makeMapBinMsg());