use max vel instead

Signed-off-by: M. Fatih Cırıt <mfc@leodrive.ai>

Author: M. Fatih Cırıt

planning/autoware_remaining_distance_time_calculator/package.xml

@@ -23,6 +23,7 @@
   <depend>route_handler</depend>
   <depend>std_msgs</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>tier4_planning_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp

@@ -21,6 +21,8 @@
 #include <rclcpp/timer.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
+#include <tier4_planning_msgs/msg/velocity_limit.hpp>
+
 #include <chrono>
 #include <functional>
 #include <memory>
@@ -34,7 +36,8 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode(
   const rclcpp::NodeOptions & options)
 : Node("remaining_distance_time_calculator", options),
   is_graph_ready_{false},
-  has_received_route_{false}
+  has_received_route_{false},
+  velocity_limit_{99.99}
 {
   using std::placeholders::_1;
 
@@ -49,6 +52,9 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode(
   sub_route_ = create_subscription<LaneletRoute>(
     "~/input/route", qos_transient_local,
     std::bind(&RemainingDistanceTimeCalculatorNode::on_route, this, _1));
+  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));
 
   pub_mission_remaining_distance_time_ = create_publisher<MissionRemainingDistanceTime>(
     "~/output/mission_remaining_distance_time",
@@ -84,6 +90,15 @@ void RemainingDistanceTimeCalculatorNode::on_route(const LaneletRoute::ConstShar
   has_received_route_ = true;
 }
 
+void RemainingDistanceTimeCalculatorNode::on_velocity_limit(
+  const VelocityLimit::ConstSharedPtr& msg)
+{
+  // store the velocity for releasing the stop
+  if (msg->max_velocity > 1e-5) {
+    velocity_limit_ = msg->max_velocity;
+  }
+}
+
 void RemainingDistanceTimeCalculatorNode::on_timer()
 {
   if (is_graph_ready_ && has_received_route_) {
@@ -121,6 +136,7 @@ double RemainingDistanceTimeCalculatorNode::calculate_remaining_distance() const
       lanelet::ArcCoordinates arc_coord =
         lanelet::utils::getArcCoordinates({llt}, current_vehicle_pose_);
       double this_lanelet_length = lanelet::utils::getLaneletLength2d(llt);
+
       remaining_distance += this_lanelet_length - arc_coord.length;
     } else if (index == (remaining_shortest_path.size() - 1)) {
       lanelet::ArcCoordinates arc_coord = lanelet::utils::getArcCoordinates({llt}, goal_pose_);
@@ -138,17 +154,7 @@ double RemainingDistanceTimeCalculatorNode::calculate_remaining_distance() const
 double RemainingDistanceTimeCalculatorNode::calculate_remaining_time(
   const double remaining_distance) const
 {
-  double current_velocity_norm = std::sqrt(
-    current_vehicle_velocity_.x * current_vehicle_velocity_.x +
-    current_vehicle_velocity_.y * current_vehicle_velocity_.y);
-
-  if (remaining_distance < 0.01 || current_velocity_norm < 0.01) {
-    return 0.0;
-  }
-
-  double remaining_time = remaining_distance / current_velocity_norm;
-
-  return remaining_time;
+  return remaining_distance / velocity_limit_;
 }
 
 void RemainingDistanceTimeCalculatorNode::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/velocity_limit.hpp>
 
 #include <lanelet2_core/Forward.h>
 #include <lanelet2_core/utility/Optional.h>
@@ -36,12 +37,6 @@
 
 namespace autoware::remaining_distance_time_calculator
 {
-using autoware_auto_mapping_msgs::msg::HADMapBin;
-using autoware_internal_msgs::msg::MissionRemainingDistanceTime;
-using autoware_planning_msgs::msg::LaneletRoute;
-using geometry_msgs::msg::Pose;
-using geometry_msgs::msg::Vector3;
-using nav_msgs::msg::Odometry;
 
 struct NodeParam
 {
@@ -54,9 +49,16 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
   explicit RemainingDistanceTimeCalculatorNode(const rclcpp::NodeOptions & options);
 
 private:
+  using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute;
+  using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+  using Odometry = nav_msgs::msg::Odometry;
+  using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit;
+  using MissionRemainingDistanceTime = autoware_internal_msgs::msg::MissionRemainingDistanceTime;
+
   rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
+  rclcpp::Subscription<VelocityLimit>::SharedPtr sub_planning_velocity_;
 
   rclcpp::Publisher<MissionRemainingDistanceTime>::SharedPtr pub_mission_remaining_distance_time_;
 
@@ -70,10 +72,11 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
   bool is_graph_ready_;
 
   // Data Buffer
-  Pose current_vehicle_pose_;
-  Vector3 current_vehicle_velocity_;
-  Pose goal_pose_;
+  geometry_msgs::msg::Pose current_vehicle_pose_;
+  geometry_msgs::msg::Vector3 current_vehicle_velocity_;
+  geometry_msgs::msg::Pose goal_pose_;
   bool has_received_route_;
+  double velocity_limit_;
 
   // Parameter
   NodeParam node_param_;
@@ -83,6 +86,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node
   void on_odometry(const Odometry::ConstSharedPtr & msg);
   void on_route(const LaneletRoute::ConstSharedPtr & msg);
   void on_map(const HADMapBin::ConstSharedPtr & msg);
+  void on_velocity_limit(const VelocityLimit::ConstSharedPtr& msg);
 
   /**
    * @brief calculate mission remaining distance