Merge branch 'main' into fix/centerpoint-unused-depend

control/autoware_joy_controller/include/autoware/joy_controller/joy_controller.hpp

@@ -33,7 +33,6 @@
 #include <tier4_external_api_msgs/srv/engage.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
 
-#include <algorithm>
 #include <memory>
 #include <string>
 
@@ -110,7 +109,6 @@ class AutowareJoyControllerNode : public rclcpp::Node
   autoware_control_msgs::msg::Control prev_control_command_;
   tier4_external_api_msgs::msg::ControlCommand prev_external_control_command_;
   GearShiftType prev_shift_ = tier4_external_api_msgs::msg::GearShift::NONE;
-  TurnSignalType prev_turn_signal_ = tier4_external_api_msgs::msg::TurnSignal::NONE;
   GateModeType prev_gate_mode_ = tier4_control_msgs::msg::GateMode::AUTO;
 
   // Timer

control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/g29_joy_converter.hpp

@@ -17,6 +17,8 @@
 
 #include "autoware/joy_controller/joy_converter/joy_converter_base.hpp"
 
+#include <cstdlib>
+
 namespace autoware::joy_controller
 {
 class G29JoyConverter : public JoyConverterBase
@@ -27,7 +29,7 @@ class G29JoyConverter : public JoyConverterBase
   float accel() const
   {
     constexpr float eps = 0.0000001;
-    if (std::fabs(AccelPedal()) < eps) {
+    if (std::abs(AccelPedal()) < eps) {
       return 0.0f;
     }
     return (AccelPedal() + 1.0f) / 2;
@@ -36,7 +38,7 @@ class G29JoyConverter : public JoyConverterBase
   float brake() const
   {
     constexpr float eps = 0.0000001;
-    if (std::fabs(BrakePedal()) < eps) {
+    if (std::abs(BrakePedal()) < eps) {
       return 0.0f;
     }
     return (BrakePedal() + 1.0f) / 2;

control/autoware_joy_controller/include/autoware/joy_controller/joy_converter/joy_converter_base.hpp

@@ -17,8 +17,6 @@
 
 #include <sensor_msgs/msg/joy.hpp>
 
-#include <algorithm>
-
 namespace autoware::joy_controller
 {
 class JoyConverterBase
@@ -49,6 +47,8 @@ class JoyConverterBase
 
   virtual bool vehicle_engage() const = 0;
   virtual bool vehicle_disengage() const = 0;
+
+  virtual ~JoyConverterBase() = default;
 };
 }  // namespace autoware::joy_controller
 

control/autoware_joy_controller/src/joy_controller_node.cpp

@@ -23,7 +23,6 @@
 #include <algorithm>
 #include <memory>
 #include <string>
-#include <utility>
 
 namespace
 {
@@ -404,8 +403,8 @@ void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency)
   request->emergency = emergency;
 
   client_emergency_stop_->async_send_request(
-    request, [this, emergency](
-               rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedFuture result) {
+    request,
+    [this](rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedFuture result) {
       auto response = result.get();
       if (tier4_api_utils::is_success(response->status)) {
         RCLCPP_INFO(get_logger(), "service succeeded");

localization/ekf_localizer/README.md

@@ -186,10 +186,12 @@ Note that, although the dimension gets larger since the analytical expansion can
 - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`.
 - The timestamp of the Pose/Twist topic is beyond the delay compensation range.
 - The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation.
+- The covariance ellipse is bigger than threshold `warn_ellipse_size` for long axis or `warn_ellipse_size_lateral_direction` for lateral_direction.
 
 ### The conditions that result in an ERROR state
 
 - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`.
+- The covariance ellipse is bigger than threshold `error_ellipse_size` for long axis or `error_ellipse_size_lateral_direction` for lateral_direction.
 
 ## Known issues
 

localization/ekf_localizer/config/ekf_localizer.param.yaml

@@ -39,6 +39,11 @@
       pose_no_update_count_threshold_error: 100
       twist_no_update_count_threshold_warn: 50
       twist_no_update_count_threshold_error: 100
+      ellipse_scale: 3.0
+      error_ellipse_size: 1.5
+      warn_ellipse_size: 1.2
+      error_ellipse_size_lateral_direction: 0.3
+      warn_ellipse_size_lateral_direction: 0.25
 
     misc:
       # for velocity measurement limitation (Set 0.0 if you want to ignore)

localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp

@@ -33,6 +33,9 @@ diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate(
 diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate(
   const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
   const double mahalanobis_distance, const double mahalanobis_distance_threshold);
+diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse(
+  const std::string & name, const double curr_size, const double warn_threshold,
+  const double error_threshold);
 
 diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
   const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array);

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -216,7 +216,8 @@ class EKFLocalizer : public rclcpp::Node
   /**
    * @brief publish diagnostics message
    */
-  void publish_diagnostics(const rclcpp::Time & current_time);
+  void publish_diagnostics(
+    const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time);
 
   /**
    * @brief update simple 1d filter

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp

@@ -57,6 +57,13 @@ class HyperParameters
       node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_warn")),
     twist_no_update_count_threshold_error(
       node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_error")),
+    ellipse_scale(node->declare_parameter<double>("diagnostics.ellipse_scale")),
+    error_ellipse_size(node->declare_parameter<double>("diagnostics.error_ellipse_size")),
+    warn_ellipse_size(node->declare_parameter<double>("diagnostics.warn_ellipse_size")),
+    error_ellipse_size_lateral_direction(
+      node->declare_parameter<double>("diagnostics.error_ellipse_size_lateral_direction")),
+    warn_ellipse_size_lateral_direction(
+      node->declare_parameter<double>("diagnostics.warn_ellipse_size_lateral_direction")),
     threshold_observable_velocity_mps(
       node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
   {
@@ -86,6 +93,12 @@ class HyperParameters
   const size_t pose_no_update_count_threshold_error;
   const size_t twist_no_update_count_threshold_warn;
   const size_t twist_no_update_count_threshold_error;
+  double ellipse_scale;
+  double error_ellipse_size;
+  double warn_ellipse_size;
+  double error_ellipse_size_lateral_direction;
+  double warn_ellipse_size_lateral_direction;
+
   const double threshold_observable_velocity_mps;
 };
 

localization/ekf_localizer/package.xml

@@ -28,6 +28,7 @@
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>kalman_filter</depend>
+  <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json

@@ -24,6 +24,31 @@
           "type": "integer",
           "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times",
           "default": 100
+        },
+        "ellipse_scale": {
+          "type": "number",
+          "description": "The scale factor to apply the error ellipse size",
+          "default": 3.0
+        },
+        "error_ellipse_size": {
+          "type": "number",
+          "description": "The long axis size of the error ellipse to trigger a ERROR state",
+          "default": 1.5
+        },
+        "warn_ellipse_size": {
+          "type": "number",
+          "description": "The long axis size of the error ellipse to trigger a WARN state",
+          "default": 1.2
+        },
+        "error_ellipse_size_lateral_direction": {
+          "type": "number",
+          "description": "The lateral direction size of the error ellipse to trigger a ERROR state",
+          "default": 0.3
+        },
+        "warn_ellipse_size_lateral_direction": {
+          "type": "number",
+          "description": "The lateral direction size of the error ellipse to trigger a WARN state",
+          "default": 0.25
         }
       },
       "required": [

localization/ekf_localizer/src/diagnostics.cpp

@@ -139,6 +139,39 @@ diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate(
   return stat;
 }
 
+diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse(
+  const std::string & name, const double curr_size, const double warn_threshold,
+  const double error_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = name + "_size";
+  key_value.value = std::to_string(curr_size);
+  stat.values.push_back(key_value);
+
+  key_value.key = name + "_warn_threshold";
+  key_value.value = std::to_string(warn_threshold);
+  stat.values.push_back(key_value);
+
+  key_value.key = name + "_error_threshold";
+  key_value.value = std::to_string(error_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (curr_size >= warn_threshold) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + name + " is large";
+  }
+  if (curr_size >= error_threshold) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "[ERROR]" + name + " is large";
+  }
+
+  return stat;
+}
+
 // The highest level within the stat_array will be reflected in the merged_stat.
 // When all stat_array entries are 'OK,' the message of merged_stat will be "OK"
 diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -17,6 +17,7 @@
 #include "ekf_localizer/diagnostics.hpp"
 #include "ekf_localizer/string.hpp"
 #include "ekf_localizer/warning_message.hpp"
+#include "localization_util/covariance_ellipse.hpp"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>
@@ -148,7 +149,7 @@ void EKFLocalizer::timer_callback()
   if (!is_activated_) {
     warning_->warn_throttle(
       "The node is not activated. Provide initial pose to pose_initializer", 2000);
-    publish_diagnostics(current_time);
+    publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time);
     return;
   }
 
@@ -241,7 +242,7 @@ void EKFLocalizer::timer_callback()
 
   /* publish ekf result */
   publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist);
-  publish_diagnostics(current_time);
+  publish_diagnostics(current_ekf_pose, current_time);
 }
 
 /*
@@ -390,7 +391,8 @@ void EKFLocalizer::publish_estimate_result(
   pub_odom_->publish(odometry);
 }
 
-void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time)
+void EKFLocalizer::publish_diagnostics(
+  const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time)
 {
   std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;
 
@@ -418,6 +420,18 @@ void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time)
     diag_status_array.push_back(check_measurement_mahalanobis_gate(
       "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
       params_.twist_gate_dist));
+
+    geometry_msgs::msg::PoseWithCovariance pose_cov;
+    pose_cov.pose = current_ekf_pose.pose;
+    pose_cov.covariance = ekf_module_->get_current_pose_covariance();
+    const autoware::localization_util::Ellipse ellipse =
+      autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
+    diag_status_array.push_back(check_covariance_ellipse(
+      "cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size,
+      params_.error_ellipse_size));
+    diag_status_array.push_back(check_covariance_ellipse(
+      "cov_ellipse_lateral_direction", ellipse.size_lateral_direction,
+      params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction));
   }
 
   diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;

localization/pose_instability_detector/src/pose_instability_detector.cpp

@@ -354,6 +354,9 @@ PoseInstabilityDetector::clip_out_necessary_twist(
     start_twist.header.stamp = start_time;
     result_deque.push_front(start_twist);
   } else {
+    if (result_deque.size() < 2) {
+      return result_deque;
+    }
     // If the first element is earlier than start_time, interpolate the first element
     rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp);
     rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp);
@@ -380,6 +383,9 @@ PoseInstabilityDetector::clip_out_necessary_twist(
     end_twist.header.stamp = end_time;
     result_deque.push_back(end_twist);
   } else {
+    if (result_deque.size() < 2) {
+      return result_deque;
+    }
     // If the last element is later than end_time, interpolate the last element
     rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp);
     rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp);

localization/pose_instability_detector/test/test.cpp

@@ -163,6 +163,64 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small)  // NOL
   EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN);
 }
 
+TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_data_comes)  // NOLINT
+{
+  // [Condition] There is no twist_msg between the two target odometry_msgs.
+  // Normally this doesn't seem to happen.
+  // As far as I can think, this happens when the odometry msg stops (so the next timer callback
+  // will refer to the same odometry msg, and the timestamp difference will be calculated as 0)
+  // This test case shows that an error occurs when two odometry msgs come in close succession and
+  // there is no other odometry msg.
+  // Referring again, this doesn't normally seem to happen in usual operation.
+
+  builtin_interfaces::msg::Time timestamp{};
+
+  // send the twist message1
+  timestamp.sec = 0;
+  timestamp.nanosec = 0;
+  helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0);
+
+  // send the first odometry message after the first twist message
+  timestamp.sec = 0;
+  timestamp.nanosec = 5e8 + 1;
+  helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0);
+
+  // process the above message (by timer_callback)
+  helper_->received_diagnostic_array_flag = false;
+  while (!helper_->received_diagnostic_array_flag) {
+    executor_.spin_some();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // send the second odometry message before the second twist message
+  timestamp.sec = 0;
+  timestamp.nanosec = 5e8 + 1e7;
+  helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0);
+
+  // send the twist message2
+  timestamp.sec = 1;
+  timestamp.nanosec = 0;
+  helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0);
+
+  // process the above messages (by timer_callback)
+  helper_->received_diagnostic_array_flag = false;
+  while (!helper_->received_diagnostic_array_flag) {
+    executor_.spin_some();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // provoke timer callback again
+  helper_->received_diagnostic_array_flag = false;
+  while (!helper_->received_diagnostic_array_flag) {
+    executor_.spin_some();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // This test is OK if pose_instability_detector does not crash. The diagnostics status is not
+  // checked.
+  SUCCEED();
+}
+
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -40,7 +40,6 @@ using autoware::behavior_path_planner::LaneChangeModuleType;
 using autoware::behavior_path_planner::ObjectInfo;
 using autoware::behavior_path_planner::Point2d;
 using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea;
-namespace utils = autoware::behavior_path_planner::utils;
 
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp

@@ -38,7 +38,6 @@ namespace autoware::behavior_velocity_planner
 {
 namespace bg = boost::geometry;
 using object_recognition_utils::convertLabelToString;
-namespace planning_utils = autoware::behavior_velocity_planner::planning_utils;
 
 RunOutModule::RunOutModule(
   const int64_t module_id, const std::shared_ptr<const PlannerData> & planner_data,

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp

@@ -35,7 +35,6 @@ namespace autoware::behavior_velocity_planner
 {
 using autoware::universe_utils::getOrDeclareParameter;
 using lanelet::autoware::VirtualTrafficLight;
-namespace planning_utils = autoware::behavior_velocity_planner::planning_utils;
 
 VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterface(node, getModuleName())