remove trajectory deviation

Author: zulfaqar-azmi-t4

control/autoware_lane_departure_checker/README.md

@@ -57,7 +57,6 @@ This package includes the following features:
 ### Output
 
 - [`diagnostic_updater`] lane_departure : Update diagnostic level when ego vehicle is out of lane.
-- [`diagnostic_updater`] trajectory_deviation : Update diagnostic level when ego vehicle deviates from trajectory.
 
 ## Parameters
 

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp

@@ -119,7 +119,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node
   diagnostic_updater::Updater updater_{this};
 
   void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void checkTrajectoryDeviation(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   // Visualization
   visualization_msgs::msg::MarkerArray createMarkerArray() const;

control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -151,8 +151,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
 
   updater_.add("lane_departure", this, &LaneDepartureCheckerNode::checkLaneDeparture);
 
-  updater_.add("trajectory_deviation", this, &LaneDepartureCheckerNode::checkTrajectoryDeviation);
-
   // Timer
   const auto period_ns = rclcpp::Rate(node_param_.update_rate).period();
   timer_ = rclcpp::create_timer(
@@ -312,17 +310,6 @@ void LaneDepartureCheckerNode::onTimer()
   updater_.force_update();
   processing_time_map["Node: updateDiagnostics"] = stop_watch.toc(true);
 
-  {
-    const auto & deviation = output_.trajectory_deviation;
-    debug_publisher_.publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
-      "deviation/lateral", deviation.lateral);
-    debug_publisher_.publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
-      "deviation/yaw", deviation.yaw);
-    debug_publisher_.publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
-      "deviation/yaw_deg", rad2deg(deviation.yaw));
-  }
-  processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true);
-
   debug_publisher_.publish<visualization_msgs::msg::MarkerArray>(
     std::string("marker_array"), createMarkerArray());
   processing_time_map["Node: publishDebugMarker"] = stop_watch.toc(true);
@@ -406,48 +393,6 @@ void LaneDepartureCheckerNode::checkLaneDeparture(
   stat.summary(level, msg);
 }
 
-void LaneDepartureCheckerNode::checkTrajectoryDeviation(
-  diagnostic_updater::DiagnosticStatusWrapper & stat)
-{
-  using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus;
-  using ControlModeStatus = autoware_vehicle_msgs::msg::ControlModeReport;
-  using OperationModeStatus = autoware_adapi_v1_msgs::msg::OperationModeState;
-
-  int8_t level = DiagStatus::OK;
-
-  if (std::abs(output_.trajectory_deviation.lateral) >= param_.max_lateral_deviation) {
-    level = DiagStatus::ERROR;
-  }
-
-  if (std::abs(output_.trajectory_deviation.longitudinal) >= param_.max_longitudinal_deviation) {
-    level = DiagStatus::ERROR;
-  }
-
-  if (std::abs(rad2deg(output_.trajectory_deviation.yaw)) >= param_.max_yaw_deviation_deg) {
-    level = DiagStatus::ERROR;
-  }
-
-  std::string msg = "OK";
-  if (
-    level == DiagStatus::ERROR && operation_mode_->mode == OperationModeStatus::AUTONOMOUS &&
-    control_mode_->mode == ControlModeStatus::AUTONOMOUS) {
-    msg = "trajectory deviation is too large";
-  } else {
-    level = DiagStatus::OK;
-  }
-
-  stat.addf("max lateral deviation", "%.3f", param_.max_lateral_deviation);
-  stat.addf("lateral deviation", "%.3f", output_.trajectory_deviation.lateral);
-
-  stat.addf("max longitudinal deviation", "%.3f", param_.max_longitudinal_deviation);
-  stat.addf("longitudinal deviation", "%.3f", output_.trajectory_deviation.longitudinal);
-
-  stat.addf("max yaw deviation", "%.3f", param_.max_yaw_deviation_deg);
-  stat.addf("yaw deviation", "%.3f", rad2deg(output_.trajectory_deviation.yaw));
-
-  stat.summary(level, msg);
-}
-
 visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray() const
 {
   using autoware_utils::create_default_marker;

control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp

@@ -31,10 +31,6 @@ Param init(rclcpp::Node & node)
   p.resample_interval = get_or_declare_parameter<double>(node, "resample_interval");
   p.max_deceleration = get_or_declare_parameter<double>(node, "max_deceleration");
   p.delay_time = get_or_declare_parameter<double>(node, "delay_time");
-  p.max_lateral_deviation = get_or_declare_parameter<double>(node, "max_lateral_deviation");
-  p.max_longitudinal_deviation =
-    get_or_declare_parameter<double>(node, "max_longitudinal_deviation");
-  p.max_yaw_deviation_deg = get_or_declare_parameter<double>(node, "max_yaw_deviation_deg");
   p.min_braking_distance = get_or_declare_parameter<double>(node, "min_braking_distance");
   p.ego_nearest_dist_threshold =
     get_or_declare_parameter<double>(node, "ego_nearest_dist_threshold");

planning/autoware_boundary_departure_checker/include/autoware/boundary_departure_checker/parameters.hpp

@@ -45,9 +45,6 @@ struct Param
   double resample_interval{};
   double max_deceleration{};
   double delay_time{};
-  double max_lateral_deviation{};
-  double max_longitudinal_deviation{};
-  double max_yaw_deviation_deg{};
   double min_braking_distance{};
   // nearest search to ego
   double ego_nearest_dist_threshold{};
@@ -72,7 +69,6 @@ struct Output
   bool will_leave_lane{};
   bool is_out_of_lane{};
   bool will_cross_boundary{};
-  PoseDeviation trajectory_deviation{};
   lanelet::ConstLanelets candidate_lanelets;
   TrajectoryPoints resampled_trajectory;
   std::vector<LinearRing2d> vehicle_footprints;

planning/autoware_boundary_departure_checker/include/autoware/boundary_departure_checker/utils.hpp

@@ -108,18 +108,6 @@ LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprin
 std::vector<LinearRing2d> createVehiclePassingAreas(
   const std::vector<LinearRing2d> & vehicle_footprints);
 
-/**
- * @brief calculate the deviation of the given pose from the nearest pose on the trajectory
- * @param trajectory target trajectory
- * @param pose vehicle pose
- * @param dist_threshold distance threshold used for searching for first nearest index to given pose
- * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose
- * @return deviation of the given pose from the trajectory
- */
-PoseDeviation calcTrajectoryDeviation(
-  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
-  const double yaw_threshold);
-
 /**
  * @brief calculate the maximum search length for boundaries considering the vehicle dimensions
  * @param trajectory target trajectory

planning/autoware_boundary_departure_checker/src/boundary_departure_checker.cpp

@@ -69,11 +69,6 @@ Output LaneDepartureChecker::update(const Input & input)
 
   autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 
-  output.trajectory_deviation = utils::calcTrajectoryDeviation(
-    *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold,
-    param_.ego_nearest_yaw_threshold);
-  output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true);
-
   {
     constexpr double min_velocity = 0.01;
     const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x);

planning/autoware_boundary_departure_checker/src/utils.cpp

@@ -223,15 +223,6 @@ std::vector<LinearRing2d> createVehiclePassingAreas(
   return areas;
 }
 
-PoseDeviation calcTrajectoryDeviation(
-  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
-  const double yaw_threshold)
-{
-  const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
-    trajectory.points, pose, dist_threshold, yaw_threshold);
-  return autoware_utils::calc_pose_deviation(trajectory.points.at(nearest_idx).pose, pose);
-}
-
 double calcMaxSearchLengthForBoundaries(
   const Trajectory & trajectory, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info)
 {