feat(remaining_dist_eta): removing functions usage from mission planner and default planner

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

feat(remaining_dist_eta): removing unneeded change in mission planner and default planner

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

feat(remaining_dist_eta): removing unneeded lines

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

feat(remaining_dist_eta): removing unneeded lines

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

Author: Ahmed Ebrahim

planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp

@@ -30,7 +30,6 @@ namespace mission_planner
 class PlannerPlugin
 {
 public:
-  using Pose = geometry_msgs::msg::Pose;
   using RoutePoints = std::vector<geometry_msgs::msg::Pose>;
   using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute;
   using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
@@ -41,7 +40,6 @@ class PlannerPlugin
   virtual void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) = 0;
   virtual bool ready() const = 0;
   virtual LaneletRoute plan(const RoutePoints & points) = 0;
-  virtual void calculateRemainingDistance(const Pose & current_vehicle_pose, const geometry_msgs::msg::Vector3 & current_vehicle_velocity) = 0;
   virtual MarkerArray visualize(const LaneletRoute & route) const = 0;
   virtual void updateRoute(const LaneletRoute & route) = 0;
   virtual void clearRoute() = 0;

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -158,41 +158,6 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node)
   param_.consider_no_drivable_lanes = node_->declare_parameter<bool>("consider_no_drivable_lanes");
   param_.check_footprint_inside_lanes =
     node_->declare_parameter<bool>("check_footprint_inside_lanes");
-  
-}
-
-void DefaultPlanner::calculateRemainingDistance(const Pose & current_vehicle_pose, const geometry_msgs::msg::Vector3 & current_vehicle_velocity)
-{
-  if (is_route_planned) {
-    const auto logger = node_->get_logger();
-    double remaining_distance = route_handler_.getRemainingDistance(current_vehicle_pose, goal_pose_);
-    
-    double current_veh_vel_mag = std::sqrt(current_vehicle_velocity.x * current_vehicle_velocity.x + current_vehicle_velocity.y + current_vehicle_velocity.y);
-    RCLCPP_INFO_STREAM(logger, "current_vehicle_velocity.x = " << current_vehicle_velocity.x);
-    RCLCPP_INFO_STREAM(logger, "current_vehicle_velocity.y = " << current_vehicle_velocity.y);
-    RCLCPP_INFO_STREAM(logger, "current_veh_vel_mag = " << current_veh_vel_mag);
-    
-    // double eta = remaining_distance / current_veh_vel_mag;
-     route_handler::EstimatedTimeOfArrival eta = route_handler_.getEstimatedTimeOfArrival(remaining_distance, current_vehicle_velocity);
-    //uint8_t  seconds = route_handler_.getEstimatedTimeOfArrival(remaining_distance, current_vehicle_velocity).seconds;
-    //const auto eta = route_handler_.getEsatimatedTimeOfArrival(remaining_distance, current_vehicle_velocity);
-
-    RCLCPP_INFO_STREAM(logger, "Remaining Distance = " << remaining_distance << " m");
-    RCLCPP_INFO_STREAM(logger, "ETA = " << unsigned(eta.hours) << "H : " << unsigned(eta.minutes) << "M : " << unsigned(eta.seconds) << "S");
-  //   double remaining_time = remaining_distance / current_veh_vel_mag;
-  //   RCLCPP_INFO_STREAM(logger, "remaining_time = " << remaining_time);
-  // double hours = static_cast<uint8_t>(remaining_time / 3600);
-  // RCLCPP_INFO_STREAM(logger, "hours = " << hours);
-  // remaining_time = std::fmod(remaining_time, 3600);
-  // RCLCPP_INFO_STREAM(logger, "remaining_time = " << remaining_time);
-  // double minutes = static_cast<int>(remaining_time / 60);
-  // RCLCPP_INFO_STREAM(logger, "minutes = " << minutes);
-  // double seconds = static_cast<int>(fmod(remaining_time, 60));
-  // RCLCPP_INFO_STREAM(logger, "seconds = " << seconds);
-  // RCLCPP_INFO_STREAM(logger, "hours = " << eta.hours);
-  // RCLCPP_INFO_STREAM(logger, "minutes = " << eta.minutes);
-  // RCLCPP_INFO_STREAM(logger, "seconds = " << eta.seconds);
-  }
 }
 
 void DefaultPlanner::initialize(rclcpp::Node * node)
@@ -475,9 +440,6 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
   const auto refined_goal = refine_goal_height(goal_pose, route_sections);
   RCLCPP_DEBUG(logger, "Goal Pose Z : %lf", refined_goal.position.z);
 
-  is_route_planned = true;
-  goal_pose_ = refined_goal;
-
   // The header is assigned by mission planner.
   route_msg.start_pose = points.front();
   route_msg.goal_pose = refined_goal;

planning/mission_planner/src/lanelet2_plugins/default_planner.hpp

@@ -51,7 +51,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
   MarkerArray visualize(const LaneletRoute & route) const override;
   MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const;
   vehicle_info_util::VehicleInfo vehicle_info_;
-  void calculateRemainingDistance(const Pose & current_vehicle_pose, const geometry_msgs::msg::Vector3 & current_vehicle_velocity) override;
 
 private:
   using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
@@ -63,8 +62,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
   lanelet::ConstLanelets road_lanelets_;
   lanelet::ConstLanelets shoulder_lanelets_;
   route_handler::RouteHandler route_handler_;
-  bool is_route_planned{false};
-  Pose goal_pose_;
 
   DefaultPlannerParameters param_;
 
@@ -74,7 +71,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
 
   void initialize_common(rclcpp::Node * node);
   void map_callback(const HADMapBin::ConstSharedPtr msg);
-  void calculateRemainingDistance(Pose & current_vehicle_pose);
 
   /**
    * @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -84,16 +84,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   const auto period = rclcpp::Rate(10).period();
   data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); });
 
-  remaining_distance_timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&MissionPlanner::remaining_distance_timer_callback_, this));
-
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
 
-void MissionPlanner::remaining_distance_timer_callback_()
-{
-  //planner_->calculateRemainingDistance(current_vehicle_pose, current_vehicle_velocity);
-}
-
 void MissionPlanner::check_initialization()
 {
   auto logger = get_logger();
@@ -120,8 +113,6 @@ void MissionPlanner::check_initialization()
 void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg)
 {
   odometry_ = msg;
-  current_vehicle_pose = odometry_->pose.pose;
-  current_vehicle_velocity = odometry_->twist.twist.linear;
 
   // NOTE: Do not check in the other states as goal may change.
   if (state_.state == RouteState::SET) {

planning/mission_planner/src/mission_planner/mission_planner.hpp

@@ -124,16 +124,12 @@ class MissionPlanner : public rclcpp::Node
 
   rclcpp::TimerBase::SharedPtr data_check_timer_;
   void check_initialization();
-  rclcpp::TimerBase::SharedPtr remaining_distance_timer_;
-  void remaining_distance_timer_callback_();
 
   double reroute_time_threshold_;
   double minimum_reroute_length_;
   bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route);
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
-  Pose current_vehicle_pose;
-  geometry_msgs::msg::Vector3 current_vehicle_velocity;
 };
 
 }  // namespace mission_planner