diff --git a/planning/autoware_remaining_distance_time_calculator/README.md b/planning/autoware_remaining_distance_time_calculator/README.md index 694c6764de91c..c227b73b87448 100644 --- a/planning/autoware_remaining_distance_time_calculator/README.md +++ b/planning/autoware_remaining_distance_time_calculator/README.md @@ -8,6 +8,7 @@ This package aims to provide mission remaining distance and remaining time calcu - The calculations are activated once we have a route planned for a mission in Autoware. - The calculations are triggered timely based on the `update_rate` parameter. +- The calculations are skipped if the scenario is PARKING, and the remaining time and distance values are set to 0.0. ### Module Parameters diff --git a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml index cfb456c57ca41..be7898bdd8913 100644 --- a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml +++ b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml @@ -4,6 +4,7 @@ + @@ -13,6 +14,7 @@ + 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 e507cc910cf86..d1254f9f034e9 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 @@ -37,6 +37,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( : Node("remaining_distance_time_calculator", options), is_graph_ready_{false}, has_received_route_{false}, + has_received_scenario_{false}, velocity_limit_{99.99}, remaining_distance_{0.0}, remaining_time_{0.0} @@ -57,6 +58,8 @@ 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( + "~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1)); pub_mission_remaining_distance_time_ = create_publisher( "~/output/mission_remaining_distance_time", @@ -100,9 +103,25 @@ void RemainingDistanceTimeCalculatorNode::on_velocity_limit( } } +void RemainingDistanceTimeCalculatorNode::on_scenario( + const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg) +{ + scenario_ = msg; + has_received_scenario_ = true; +} + void RemainingDistanceTimeCalculatorNode::on_timer() { - if (is_graph_ready_ && has_received_route_) { + if (!has_received_scenario_) { + return; + } + + // check if the scenario is parking or not + if (scenario_->current_scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + remaining_distance_ = 0.0; + remaining_time_ = 0.0; + publish_mission_remaining_distance_time(); + } else if (is_graph_ready_ && has_received_route_) { calculate_remaining_distance(); calculate_remaining_time(); 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 d6c900af4dfed..b7af8861bf524 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 @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -59,6 +60,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::Publisher::SharedPtr pub_mission_remaining_distance_time_; @@ -75,7 +77,9 @@ 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_; bool has_received_route_; + bool has_received_scenario_; double velocity_limit_; double remaining_distance_; @@ -90,6 +94,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); /** * @brief calculate mission remaining distance