feat(remaining_distance_time_calculator): skip calculation during parking

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
 

planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml

@@ -4,6 +4,7 @@
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_map" default="/map/vector_map"/>
   <arg name="input_route" default="/planning/mission_planning/route"/>
+  <arg name="input_scenario" default="/planning/scenario_planning/scenario"/>
 
   <node pkg="autoware_remaining_distance_time_calculator" exec="autoware_remaining_distance_time_calculator_node" name="remaining_distance_time_calculator" output="screen">
     <!-- param -->
@@ -13,6 +14,7 @@
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/map" to="$(var input_map)"/>
     <remap from="~/input/route" to="$(var input_route)"/>
+    <remap from="~/input/scenario" to="$(var input_scenario)"/>
 
     <!-- output -->
     <remap from="~/output/mission_remaining_distance_time" to="/planning/mission_remaining_distance_time"/>

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<tier4_planning_msgs::msg::VelocityLimit>(
     "/planning/scenario_planning/current_max_velocity", qos_transient_local,
     std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1));
+  sub_scenario_ = this->create_subscription<tier4_planning_msgs::msg::Scenario>(
+    "~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1));
 
   pub_mission_remaining_distance_time_ = create_publisher<MissionRemainingDistanceTime>(
     "~/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();

planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp

@@ -24,6 +24,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_planning_msgs/msg/scenario.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -59,6 +60,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
   rclcpp::Subscription<VelocityLimit>::SharedPtr sub_planning_velocity_;
+  rclcpp::Subscription<tier4_planning_msgs::msg::Scenario>::SharedPtr sub_scenario_;
 
   rclcpp::Publisher<MissionRemainingDistanceTime>::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