Merge pull request #254 from tier4/sync-upstream

chore: sync upstream

.github/CODEOWNERS

@@ -44,24 +44,24 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp @autowarefoundati
 common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 common/time_utils/** christopherj.ho@gmail.com @autowarefoundation/autoware-global-codeowners
 common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners
-common/tvm_utility/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners
+common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai @autowarefoundation/autoware-global-codeowners
 control/external_cmd_selector/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/joy_controller/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/lane_departure_checker/** kyoichi.sugahara@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/obstacle_collision_checker/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
-control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
+control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/pure_pursuit/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/shift_decider/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
-control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
+control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners
 evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners
 launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners
-launch/tier4_control_launch/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
+launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 launch/tier4_localization_launch/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners
 launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners
 launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
@@ -115,7 +115,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp @autow
 perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@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 yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/costmap_generator/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/freespace_planner/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
@@ -128,12 +128,13 @@ planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp @autowarefoundatio
 planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/planning_error_monitor/** yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/planning_evaluator/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/route_handler/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/rtc_interface/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/scenario_selector/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/static_centerline_optimizer/** takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/surround_obstacle_checker/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners
 sensing/geo_pos_conv/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners
 sensing/gnss_poser/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners
@@ -166,7 +167,7 @@ system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp @autowar
 system/topic_state_monitor/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
 system/velodyne_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners
 tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai @autowarefoundation/autoware-global-codeowners
-vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
 vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners

common/autoware_auto_perception_rviz_plugin/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <build_depend>autoware_cmake</build_depend>
+  <build_depend>libboost-dev</build_depend>
   <build_depend>qtbase5-dev</build_depend>
 
   <depend>autoware_auto_perception_msgs</depend>

common/kalman_filter/package.xml

@@ -14,6 +14,8 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <build_depend>autoware_cmake</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>eigen3_cmake_module</build_depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_lint_auto</test_depend>

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -15,23 +15,28 @@
 #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 
+#include "motion_utils/resample/resample_utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
+#include <functional>
 #include <string>
+#include <vector>
 
 namespace motion_utils
 {
+using geometry_msgs::msg::Pose;
+
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset = 0.0);
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0);
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset = 0.0);
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0);
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
-  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset = 0.0);
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0);
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
@@ -41,6 +46,46 @@ visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
 
 visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createVirtualWallMarkerFromPreviousPoses(
+  const std::vector<Pose> & stop_poses, const std::vector<Pose> & previous_poses,
+  const rclcpp::Time & now, int32_t id);
+
+class VirtualWallMarkerCreator
+{
+public:
+  virtual ~VirtualWallMarkerCreator() = default;
+
+  using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
+    const geometry_msgs::msg::Pose & pose, const std::string & module_name,
+    const rclcpp::Time & now, const int32_t id, const double longitudinal_offset)>;
+
+  using delete_wall_function =
+    std::function<visualization_msgs::msg::MarkerArray(const rclcpp::Time & now, const int32_t id)>;
+
+  visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
+    const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
+    int32_t id, const double longitudinal_offset = 0.0);
+
+  visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
+    const std::vector<Pose> & slow_down_poses, const std::string & module_name,
+    const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0);
+
+  visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
+    const std::vector<Pose> & dead_line_poses, const std::string & module_name,
+    const rclcpp::Time & now, int32_t id, const double longitudinal_offset = 0.0);
+
+private:
+  visualization_msgs::msg::MarkerArray handleVirtualWallMarker(
+    const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
+    int32_t id, create_wall_function function_create_wall_marker,
+    delete_wall_function function_delete_wall_marker,
+    std::vector<geometry_msgs::msg::Pose> & previous_poses, const double longitudinal_offset = 0.0);
+
+  std::vector<Pose> previous_stop_poses_;
+  std::vector<Pose> previous_slow_down_poses_;
+  std::vector<Pose> previous_dead_line_poses_;
+};
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -437,6 +437,39 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t
   return dist_sum;
 }
 
+/**
+ *  @brief Computes the partial sums of the elements in the sub-ranges of
+ *         the range [src_idx, dst_idx) and return these sum as vector
+ */
+template <class T>
+std::vector<double> calcSignedArcLengthPartialSum(
+  const T & points, const size_t src_idx, const size_t dst_idx)
+{
+  try {
+    validateNonEmpty(points);
+  } catch (const std::exception & e) {
+    std::cerr << e.what() << std::endl;
+    return {};
+  }
+
+  if (src_idx + 1 > dst_idx) {
+    auto copied = points;
+    std::reverse(copied.begin(), copied.end());
+    return calcSignedArcLengthPartialSum(points, dst_idx, src_idx);
+  }
+
+  std::vector<double> partial_dist;
+  partial_dist.reserve(dst_idx - src_idx);
+
+  double dist_sum = 0.0;
+  partial_dist.push_back(dist_sum);
+  for (size_t i = src_idx; i < dst_idx - 1; ++i) {
+    dist_sum += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1));
+    partial_dist.push_back(dist_sum);
+  }
+  return partial_dist;
+}
+
 /**
  * @brief calcSignedArcLength from point to index
  */

common/motion_utils/src/marker/marker_helper.cpp

@@ -16,13 +16,16 @@
 
 #include <string>
 
+using tier4_autoware_utils::appendMarkerArray;
 using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createDeletedDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
+using visualization_msgs::msg::MarkerArray;
 
 namespace
 {
+
 inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
   const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
   const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
@@ -128,4 +131,79 @@ visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
 {
   return createDeletedVirtualWallMarkerArray("dead_line_", now, id);
 }
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWallMarker(
+  const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
+  int32_t id, create_wall_function function_create_wall_marker,
+  delete_wall_function function_delete_wall_marker,
+  std::vector<geometry_msgs::msg::Pose> & previous_virtual_wall_poses,
+  const double longitudinal_offset)
+{
+  size_t id_to_create = id;
+  size_t id_to_delete = id;
+  visualization_msgs::msg::MarkerArray wall_marker;
+
+  if (poses.size() == 0) {
+    return wall_marker;
+  }
+
+  for (const auto & p : previous_virtual_wall_poses) {
+    const bool previous_stop_pose_is_in_stop_pose =
+      std::any_of(poses.begin(), poses.end(), [&](const geometry_msgs::msg::Pose & elem) {
+        std::vector<Pose> poses;
+        poses.push_back(p);
+        poses.push_back(elem);
+        return resample_utils::validate_points_duplication(poses);
+      });
+
+    if (!previous_stop_pose_is_in_stop_pose) {
+      appendMarkerArray(function_delete_wall_marker(now, id_to_delete), &wall_marker, now);
+    }
+    id_to_delete++;
+  }
+
+  for (const auto & p : poses) {
+    appendMarkerArray(
+      function_create_wall_marker(p, module_name, now, id_to_create++, longitudinal_offset),
+      &wall_marker);
+  }
+  previous_virtual_wall_poses = poses;
+  return wall_marker;
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker(
+  const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
+  int32_t id, const double longitudinal_offset)
+{
+  create_wall_function creator = motion_utils::createStopVirtualWallMarker;
+  delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker;
+
+  return handleVirtualWallMarker(
+    stop_poses, module_name, now, id, creator, deleter, previous_stop_poses_, longitudinal_offset);
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker(
+  const std::vector<Pose> & slow_down_poses, const std::string & module_name,
+  const rclcpp::Time & now, int32_t id, const double longitudinal_offset)
+{
+  create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker;
+  delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker;
+
+  return handleVirtualWallMarker(
+    slow_down_poses, module_name, now, id, creator, deleter, previous_slow_down_poses_,
+    longitudinal_offset);
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker(
+  const std::vector<Pose> & dead_line_poses, const std::string & module_name,
+  const rclcpp::Time & now, int32_t id, const double longitudinal_offset)
+{
+  create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker;
+  delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker;
+
+  return handleVirtualWallMarker(
+    dead_line_poses, module_name, now, id, creator, deleter, previous_dead_line_poses_,
+    longitudinal_offset);
+}
+
 }  // namespace motion_utils

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -39,6 +39,12 @@ std::string getModuleName(const uint8_t module_type)
     case Module::LANE_CHANGE_RIGHT: {
       return "lane_change_right";
     }
+    case Module::EXT_REQUEST_LANE_CHANGE_LEFT: {
+      return "ext_request_lane_change_left";
+    }
+    case Module::EXT_REQUEST_LANE_CHANGE_RIGHT: {
+      return "ext_request_lane_change_right";
+    }
     case Module::AVOIDANCE_LEFT: {
       return "avoidance_left";
     }
@@ -80,8 +86,10 @@ bool isPathChangeModule(const uint8_t module_type)
 {
   if (
     module_type == Module::LANE_CHANGE_LEFT || module_type == Module::LANE_CHANGE_RIGHT ||
-    module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT ||
-    module_type == Module::PULL_OVER || module_type == Module::PULL_OUT) {
+    module_type == Module::EXT_REQUEST_LANE_CHANGE_LEFT ||
+    module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || module_type == Module::AVOIDANCE_LEFT ||
+    module_type == Module::AVOIDANCE_RIGHT || module_type == Module::PULL_OVER ||
+    module_type == Module::PULL_OUT) {
     return true;
   }
   return false;

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -13,3 +13,4 @@
     max_longitudinal_deviation: 2.0
     max_yaw_deviation_deg: 60.0
     delta_yaw_threshold_for_closest_point: 1.570 #M_PI/2.0, delta yaw thres for closest point
+    min_braking_distance: 0.0

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -58,6 +58,7 @@ struct Param
   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;
   double ego_nearest_yaw_threshold;

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -109,8 +109,9 @@ Output LaneDepartureChecker::update(const Input & input)
     const auto & raw_abs_velocity = std::abs(input.current_odom->twist.twist.linear.x);
     const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity;
 
-    const auto braking_distance =
-      calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time);
+    const auto braking_distance = std::max(
+      param_.min_braking_distance,
+      calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time));
 
     output.resampled_trajectory = cutTrajectory(
       resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance);