Merge pull request #17 from autowarefoundation/main

[pull] main from autowarefoundation:main

.github/CODEOWNERS

@@ -154,6 +154,8 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon
 perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
 perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp
 perception/traffic_light_visualization/** yukihiro.saito@tier4.jp
+planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp zulfaqar.azmi@tier4.jp
+planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp
 planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
 planning/behavior_path_planner_common/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
 planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -155,7 +155,6 @@ class EKFLocalizer : public rclcpp::Node
 
   const HyperParameters params_;
 
-  double ekf_rate_;
   double ekf_dt_;
 
   /* process noise variance for discrete model */

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -30,6 +30,7 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 
 #include <memory>
+#include <vector>
 
 struct EKFDiagnosticInfo
 {
@@ -76,12 +77,15 @@ class EKFModule
   std::array<double, 36> getCurrentPoseCovariance() const;
   std::array<double, 36> getCurrentTwistCovariance() const;
 
+  size_t find_closest_delay_time_index(double target_value) const;
+  void accumulate_delay_time(const double dt);
+
   void predictWithDelay(const double dt);
   bool measurementUpdatePose(
-    const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr,
+    const PoseWithCovariance & pose, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & pose_diag_info);
   bool measurementUpdateTwist(
-    const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr,
+    const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & twist_diag_info);
   geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay(
     const PoseWithCovariance & pose, const double delay_time);
@@ -91,6 +95,7 @@ class EKFModule
 
   std::shared_ptr<Warning> warning_;
   const int dim_x_;
+  std::vector<double> accumulated_delay_times_;
   const HyperParameters params_;
 };
 

localization/ekf_localizer/include/ekf_localizer/warning_message.hpp

@@ -17,10 +17,9 @@
 
 #include <string>
 
-std::string poseDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt);
+std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold);
 std::string twistDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt);
+  const double delay_time, const double delay_time_threshold);
 std::string poseDelayTimeWarningMessage(const double delay_time);
 std::string twistDelayTimeWarningMessage(const double delay_time);
 std::string mahalanobisWarningMessage(const double distance, const double max_distance);

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
 : rclcpp::Node(node_name, node_options),
   warning_(std::make_shared<Warning>(this)),
   params_(this),
-  ekf_rate_(params_.ekf_rate),
   ekf_dt_(params_.ekf_dt),
   pose_queue_(params_.pose_smoothing_steps),
   twist_queue_(params_.twist_smoothing_steps)
@@ -109,9 +108,22 @@ void EKFLocalizer::updatePredictFrequency()
     if (get_clock()->now() < *last_predict_time_) {
       warning_->warn("Detected jump back in time");
     } else {
-      ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds();
-      DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_);
-      ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);
+      /* Measure dt */
+      ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds();
+      DEBUG_INFO(
+        get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_);
+
+      if (ekf_dt_ > 10.0) {
+        ekf_dt_ = 10.0;
+        RCLCPP_WARN(
+          get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_);
+      } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) {
+        RCLCPP_WARN(
+          get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_);
+      }
+
+      /* Register dt and accumulate time delay */
+      ekf_module_->accumulate_delay_time(ekf_dt_);
 
       /* Update discrete proc_cov*/
       proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
@@ -165,7 +177,7 @@ void EKFLocalizer::timerCallback()
     const size_t n = pose_queue_.size();
     for (size_t i = 0; i < n; ++i) {
       const auto pose = pose_queue_.pop_increment_age();
-      bool is_updated = ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_);
+      bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_);
       if (is_updated) {
         pose_is_updated = true;
 
@@ -200,8 +212,7 @@ void EKFLocalizer::timerCallback()
     const size_t n = twist_queue_.size();
     for (size_t i = 0; i < n; ++i) {
       const auto twist = twist_queue_.pop_increment_age();
-      bool is_updated =
-        ekf_module_->measurementUpdateTwist(*twist, ekf_dt_, t_curr, twist_diag_info_);
+      bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_);
       if (is_updated) {
         twist_is_updated = true;
       }

localization/ekf_localizer/src/ekf_module.cpp

@@ -38,6 +38,7 @@
 EKFModule::EKFModule(std::shared_ptr<Warning> warning, const HyperParameters params)
 : warning_(std::move(warning)),
   dim_x_(6),  // x, y, yaw, yaw_bias, vx, wz
+  accumulated_delay_times_(params.extend_state_step, 1.0E15),
   params_(params)
 {
   Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1);
@@ -131,6 +132,48 @@ double EKFModule::getYawBias() const
   return kalman_filter_.getLatestX()(IDX::YAWB);
 }
 
+size_t EKFModule::find_closest_delay_time_index(double target_value) const
+{
+  // If target_value is too large, return last index + 1
+  if (target_value > accumulated_delay_times_.back()) {
+    return accumulated_delay_times_.size();
+  }
+
+  auto lower = std::lower_bound(
+    accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value);
+
+  // If the lower bound is the first element, return its index.
+  // If the lower bound is beyond the last element, return the last index.
+  // If else, take the closest element.
+  if (lower == accumulated_delay_times_.begin()) {
+    return 0;
+  } else if (lower == accumulated_delay_times_.end()) {
+    return accumulated_delay_times_.size() - 1;
+  } else {
+    // Compare the target with the lower bound and the previous element.
+    auto prev = lower - 1;
+    bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value);
+
+    // Return the index of the closer element.
+    return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev)
+                             : std::distance(accumulated_delay_times_.begin(), lower);
+  }
+}
+
+void EKFModule::accumulate_delay_time(const double dt)
+{
+  // Shift the delay times to the right.
+  std::copy_backward(
+    accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1,
+    accumulated_delay_times_.end());
+
+  // Add the new delay time to all elements.
+  accumulated_delay_times_.front() = 0.0;
+  for (auto & delay_time : accumulated_delay_times_) {
+    delay_time += dt;
+  }
+}
+
 void EKFModule::predictWithDelay(const double dt)
 {
   const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();
@@ -147,8 +190,7 @@ void EKFModule::predictWithDelay(const double dt)
 }
 
 bool EKFModule::measurementUpdatePose(
-  const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr,
-  EKFDiagnosticInfo & pose_diag_info)
+  const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info)
 {
   if (pose.header.frame_id != params_.pose_frame_id) {
     warning_->warnThrottle(
@@ -170,14 +212,15 @@ bool EKFModule::measurementUpdatePose(
 
   delay_time = std::max(delay_time, 0.0);
 
-  const int delay_step = std::roundf(delay_time / dt);
+  const int delay_step = static_cast<int>(find_closest_delay_time_index(delay_time));
 
   pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time);
-  pose_diag_info.delay_time_threshold = params_.extend_state_step * dt;
+  pose_diag_info.delay_time_threshold = accumulated_delay_times_.back();
   if (delay_step >= params_.extend_state_step) {
     pose_diag_info.is_passed_delay_gate = false;
     warning_->warnThrottle(
-      poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000);
+      poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold),
+      2000);
     return false;
   }
 
@@ -243,7 +286,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela
 }
 
 bool EKFModule::measurementUpdateTwist(
-  const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr,
+  const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
   EKFDiagnosticInfo & twist_diag_info)
 {
   if (twist.header.frame_id != "base_link") {
@@ -262,14 +305,16 @@ bool EKFModule::measurementUpdateTwist(
   }
   delay_time = std::max(delay_time, 0.0);
 
-  const int delay_step = std::roundf(delay_time / dt);
+  const int delay_step = static_cast<int>(find_closest_delay_time_index(delay_time));
 
   twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time);
-  twist_diag_info.delay_time_threshold = params_.extend_state_step * dt;
+  twist_diag_info.delay_time_threshold = accumulated_delay_times_.back();
   if (delay_step >= params_.extend_state_step) {
     twist_diag_info.is_passed_delay_gate = false;
     warning_->warnThrottle(
-      twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000);
+      twistDelayStepWarningMessage(
+        twist_diag_info.delay_time, twist_diag_info.delay_time_threshold),
+      2000);
     return false;
   }
 

localization/ekf_localizer/src/warning_message.cpp

@@ -18,22 +18,20 @@
 
 #include <string>
 
-std::string poseDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt)
+std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold)
 {
   const std::string s =
     "Pose delay exceeds the compensation limit, ignored. "
-    "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]";
-  return fmt::format(s, delay_time, extend_state_step * ekf_dt);
+    "delay: {:.3f}[s], limit: {:.3f}[s]";
+  return fmt::format(s, delay_time, delay_time_threshold);
 }
 
-std::string twistDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt)
+std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold)
 {
   const std::string s =
     "Twist delay exceeds the compensation limit, ignored. "
-    "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]";
-  return fmt::format(s, delay_time, extend_state_step * ekf_dt);
+    "delay: {:.3f}[s], limit: {:.3f}[s]";
+  return fmt::format(s, delay_time, delay_time_threshold);
 }
 
 std::string poseDelayTimeWarningMessage(const double delay_time)

localization/ekf_localizer/test/test_warning_message.cpp

@@ -21,17 +21,17 @@
 TEST(PoseDelayStepWarningMessage, SmokeTest)
 {
   EXPECT_STREQ(
-    poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(),
+    poseDelayStepWarningMessage(6.0, 4.0).c_str(),
     "Pose delay exceeds the compensation limit, ignored. "
-    "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]");
+    "delay: 6.000[s], limit: 4.000[s]");
 }
 
 TEST(TwistDelayStepWarningMessage, SmokeTest)
 {
   EXPECT_STREQ(
-    twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(),
+    twistDelayStepWarningMessage(10.0, 6.0).c_str(),
     "Twist delay exceeds the compensation limit, ignored. "
-    "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]");
+    "delay: 10.000[s], limit: 6.000[s]");
 }
 
 TEST(PoseDelayTimeWarningMessage, SmokeTest)

planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_path_avoidance_by_lane_change_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/scene.cpp
+  src/manager.cpp
+  src/interface.cpp
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gmock(test_${PROJECT_NAME}
+    test/test_behavior_path_planner_node_interface.cpp
+  )
+
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+
+  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp

@@ -0,0 +1,33 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_
+#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_
+
+#include "behavior_path_avoidance_module/data_structs.hpp"
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+struct AvoidanceByLCParameters : public AvoidanceParameters
+{
+  // execute only when the target object longitudinal distance is larger than this param.
+  double execute_object_longitudinal_margin{0.0};
+
+  // execute only when lane change end point is before the object.
+  bool execute_only_when_lane_change_finish_before_object{false};
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp

@@ -0,0 +1,48 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_
+#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_
+
+#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
+#include "behavior_path_avoidance_by_lane_change_module/scene.hpp"
+#include "behavior_path_planner/scene_module/lane_change/interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+namespace behavior_path_planner
+{
+class AvoidanceByLaneChangeInterface : public LaneChangeInterface
+{
+public:
+  AvoidanceByLaneChangeInterface(
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<LaneChangeParameters> & parameters,
+    const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
+
+  bool isExecutionRequested() const override;
+
+protected:
+  void updateRTCStatus(const double start_distance, const double finish_distance) override;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_