feat(remaining_dist_eta): add publisher and using the remaining distance and eta functions from behavior path planner run

Signed-off-by: Ahmed Ebrahim <ahmed.ebrahim@leodrive.ai>

Author: Ahmed Ebrahim

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -194,6 +194,7 @@
       <remap from="~/input/accel" to="/localization/acceleration"/>
       <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
       <remap from="~/output/path" to="path_with_lane_id"/>
+      <remap from="~/output/remaining_distance_eta" to="/planning/remaining_distance_eta"/>
       <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
       <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd"/>
       <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -32,8 +32,10 @@
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
+#include <autoware_planning_msgs/msg/remaining_distance_eta.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module.hpp>
@@ -62,8 +64,10 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using autoware_perception_msgs::msg::TrafficSignalArray;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::PoseWithUuidStamped;
+using autoware_planning_msgs::msg::RemainingDistanceETA;
 using nav_msgs::msg::OccupancyGrid;
 using nav_msgs::msg::Odometry;
+using geometry_msgs::msg::Pose;
 using rcl_interfaces::msg::SetParametersResult;
 using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
@@ -98,6 +102,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<LateralOffset>::SharedPtr lateral_offset_subscriber_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
+  rclcpp::Publisher<RemainingDistanceETA>::SharedPtr remaining_distance_eta_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
@@ -123,6 +128,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   bool has_received_map_{false};
   bool has_received_route_{false};
 
+  Pose goal_pose_;
+  double remaining_distance_;
+  route_handler::EstimatedTimeOfArrival eta_;
+
   std::mutex mutex_pd_;       // mutex for planner_data_
   std::mutex mutex_manager_;  // mutex for bt_manager_ or planner_manager_
   std::mutex mutex_map_;      // mutex for has_received_map_ and map_ptr_
@@ -177,6 +186,12 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;
 
+  /**
+   * @brief publish remaining distance and ETA
+   */
+  void publishRemainingDistanceETA(
+    const double & remaining_distance, const route_handler::EstimatedTimeOfArrival & eta) const;
+  
   /**
    * @brief publish reroute availability
    */

planning/behavior_path_planner/launch/behavior_path_planner.launch.xml

@@ -34,6 +34,7 @@
     <remap from="~/input/accel" to="/localization/acceleration"/>
     <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
     <remap from="~/output/path" to="path_with_lane_id"/>
+    <remap from="~/output/remaining_distance_eta" to="/planning/remaining_distance_eta"/>
     <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
     <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd"/>
     <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -62,6 +62,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
   // publisher
   path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
+  remaining_distance_eta_publisher_ = create_publisher<RemainingDistanceETA>("~/output/remaining_distance_eta", rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable());
   turn_signal_publisher_ =
     create_publisher<TurnIndicatorsCommand>("~/output/turn_indicators_cmd", 1);
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
@@ -385,6 +386,8 @@ void BehaviorPathPlannerNode::run()
       planner_manager_->reset();
       planner_data_->prev_modified_goal.reset();
     }
+
+    goal_pose_ = route_ptr->goal_pose;
   }
 
   const auto controlled_by_autoware_autonomously =
@@ -394,6 +397,16 @@ void BehaviorPathPlannerNode::run()
     planner_manager_->resetRootLanelet(planner_data_);
   }
 
+  if (planner_data_->route_handler->isHandlerReady()) {
+    remaining_distance_ = planner_data_->route_handler->getRemainingDistance(
+      planner_data_->self_odometry->pose.pose, goal_pose_);
+
+    eta_ = planner_data_->route_handler->getEstimatedTimeOfArrival(
+      remaining_distance_, planner_data_->self_odometry->twist.twist.linear);
+
+    publishRemainingDistanceETA(remaining_distance_, eta_);
+  }
+
   // run behavior planner
   const auto output = planner_manager_->run(planner_data_);
 
@@ -529,6 +542,18 @@ void BehaviorPathPlannerNode::publish_reroute_availability() const
   reroute_availability_publisher_->publish(is_reroute_available);
 }
 
+void BehaviorPathPlannerNode::publishRemainingDistanceETA(const double & remaining_distance, const route_handler::EstimatedTimeOfArrival & eta) const
+{
+  RemainingDistanceETA remaining_distance_eta;
+  remaining_distance_eta.remaining_distance = remaining_distance;
+  remaining_distance_eta.hours = eta.hours;
+  remaining_distance_eta.minutes = eta.minutes;
+  remaining_distance_eta.seconds = eta.seconds;
+
+  remaining_distance_eta_publisher_->publish(remaining_distance_eta);
+
+}
+
 void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data)
 {
   MarkerArray marker_array;

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -91,7 +91,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
 
 void MissionPlanner::remaining_distance_timer_callback_()
 {
-  planner_->calculateRemainingDistance(current_vehicle_pose, current_vehicle_velocity);
+  //planner_->calculateRemainingDistance(current_vehicle_pose, current_vehicle_velocity);
 }
 
 void MissionPlanner::check_initialization()