fix(autoware_lane_departure_checker): not to show error message "trajectory deviation is too large" during manual driving (#8404)

* update: update not to show error message "trajectory deviation is too large" during manual driving

Signed-off-by: T-Kimura-MM <tkc1110bs@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: T-Kimura-MM <tkc1110bs@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: T-Kimura-MM

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

@@ -23,9 +23,11 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_vehicle_msgs/msg/control_mode_report.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -78,6 +80,12 @@ class LaneDepartureCheckerNode : public rclcpp::Node
     this, "~/input/reference_trajectory"};
   autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> sub_predicted_trajectory_{
     this, "~/input/predicted_trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    autoware_adapi_v1_msgs::msg::OperationModeState>
+    sub_operation_mode_{this, "/api/operation_mode/state"};
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    autoware_vehicle_msgs::msg::ControlModeReport>
+    sub_control_mode_{this, "/vehicle/status/control_mode"};
 
   // Data Buffer
   nav_msgs::msg::Odometry::ConstSharedPtr current_odom_;
@@ -91,13 +99,17 @@ class LaneDepartureCheckerNode : public rclcpp::Node
   lanelet::ConstLanelets route_lanelets_;
   Trajectory::ConstSharedPtr reference_trajectory_;
   Trajectory::ConstSharedPtr predicted_trajectory_;
+  autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_;
+  autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;
 
   // Callback
   void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
   void onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg);
   void onRoute(const LaneletRoute::ConstSharedPtr msg);
   void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg);
   void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);
+  void onOperationMode(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
+  void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
 
   // Publisher
   autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};

control/autoware_lane_departure_checker/package.xml

@@ -13,12 +13,14 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_universe_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
+  <depend>autoware_vehicle_msgs</depend>
   <depend>boost</depend>
   <depend>diagnostic_updater</depend>
   <depend>eigen</depend>

control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -216,6 +216,16 @@ bool LaneDepartureCheckerNode::isDataReady()
     return false;
   }
 
+  if (!operation_mode_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for operation_mode msg...");
+    return false;
+  }
+
+  if (!control_mode_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for control_mode msg...");
+    return false;
+  }
+
   return true;
 }
 
@@ -260,6 +270,8 @@ void LaneDepartureCheckerNode::onTimer()
   route_ = sub_route_.takeData();
   reference_trajectory_ = sub_reference_trajectory_.takeData();
   predicted_trajectory_ = sub_predicted_trajectory_.takeData();
+  operation_mode_ = sub_operation_mode_.takeData();
+  control_mode_ = sub_control_mode_.takeData();
 
   const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData();
   if (lanelet_map_bin_msg) {
@@ -426,6 +438,9 @@ 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) {
@@ -441,8 +456,12 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation(
   }
 
   std::string msg = "OK";
-  if (level == DiagStatus::ERROR) {
+  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);