chore: sync upstream

.cspell-partial.json

@@ -3,7 +3,6 @@
     "**/perception/**",
     "**/planning/**",
     "**/sensing/**",
-    "**/control/**",
     "perception/bytetrack/lib/**"
   ],
   "ignoreRegExpList": [],

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -379,6 +379,14 @@ class MPC
     const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
     const Odometry & current_kinematics) const;
 
+  /**
+   * @brief calculate steering rate limit along with the target trajectory
+   * @param reference_trajectory The reference trajectory.
+   * @param current_velocity current velocity of ego.
+   */
+  VectorXd calcSteerRateLimitOnTrajectory(
+    const MPCTrajectory & trajectory, const double current_velocity) const;
+
   //!< @brief logging with warn and return false
   template <typename... Args>
   inline bool fail_warn_throttle(Args &&... args) const

control/mpc_lateral_controller/src/mpc.cpp

@@ -359,9 +359,6 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
 
   MatrixXd x_curr = x0_orig;
   double mpc_curr_time = start_time;
-  // for (const auto & tt : traj.relative_time) {
-  //   std::cerr << "traj.relative_time = " << tt << std::endl;
-  // }
   for (size_t i = 0; i < m_input_buffer.size(); ++i) {
     double k, v = 0.0;
     try {
@@ -573,47 +570,16 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
     A(i, i - 1) = -1.0;
   }
 
-  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
-  const auto get_adaptive_steer_rate_lim = [&](const double curvature, const double velocity) {
-    if (is_vehicle_stopped) {
-      return std::numeric_limits<double>::max();
-    }
-
-    double steer_rate_lim_by_curvature = m_steer_rate_lim_map_by_curvature.back().second;
-    for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_curvature) {
-      if (std::abs(curvature) <= steer_rate_lim_info.first) {
-        steer_rate_lim_by_curvature = steer_rate_lim_info.second;
-        break;
-      }
-    }
-
-    double steer_rate_lim_by_velocity = m_steer_rate_lim_map_by_velocity.back().second;
-    for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_velocity) {
-      if (std::abs(velocity) <= steer_rate_lim_info.first) {
-        steer_rate_lim_by_velocity = steer_rate_lim_info.second;
-        break;
-      }
-    }
-
-    return std::min(steer_rate_lim_by_curvature, steer_rate_lim_by_velocity);
-  };
-
+  // steering angle limit
   VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // min steering angle
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
 
-  VectorXd lbA(DIM_U_N);
-  VectorXd ubA(DIM_U_N);
-  for (int i = 0; i < DIM_U_N; ++i) {
-    const double adaptive_steer_rate_lim =
-      get_adaptive_steer_rate_lim(traj.smooth_k.at(i), traj.vx.at(i));
-    const double adaptive_delta_steer_lim = adaptive_steer_rate_lim * prediction_dt;
-    lbA(i) = -adaptive_delta_steer_lim;
-    ubA(i) = adaptive_delta_steer_lim;
-  }
-  const double adaptive_steer_rate_lim =
-    get_adaptive_steer_rate_lim(traj.smooth_k.at(0), traj.vx.at(0));
-  lbA(0, 0) = m_raw_steer_cmd_prev - adaptive_steer_rate_lim * m_ctrl_period;
-  ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period;
+  // steering angle rate limit
+  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
+  VectorXd ubA = steer_rate_limits * prediction_dt;
+  VectorXd lbA = -steer_rate_limits * prediction_dt;
+  ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
+  lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period;
 
   auto t_start = std::chrono::system_clock::now();
   bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
@@ -761,6 +727,58 @@ double MPC::calcDesiredSteeringRate(
   return steer_rate;
 }
 
+VectorXd MPC::calcSteerRateLimitOnTrajectory(
+  const MPCTrajectory & trajectory, const double current_velocity) const
+{
+  const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
+    std::vector<double> reference, limits;
+    for (const auto & p : steer_rate_limit_map) {
+      reference.push_back(p.first);
+      limits.push_back(p.second);
+    }
+
+    // If the speed is out of range of the reference, apply zero-order hold.
+    if (current <= reference.front()) {
+      return limits.front();
+    }
+    if (current >= reference.back()) {
+      return limits.back();
+    }
+
+    // Apply linear interpolation
+    for (size_t i = 0; i < reference.size() - 1; ++i) {
+      if (reference.at(i) <= current && current <= reference.at(i + 1)) {
+        auto ratio =
+          (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5);
+        ratio = std::clamp(ratio, 0.0, 1.0);
+        const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i));
+        return interp;
+      }
+    }
+
+    std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command "
+                 "filter is not working. Please check the code."
+              << std::endl;
+    return reference.back();
+  };
+
+  // when the vehicle is stopped, no steering rate limit.
+  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
+  if (is_vehicle_stopped) {
+    return VectorXd::Zero(m_param.prediction_horizon);
+  }
+
+  // calculate steering rate limit
+  VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
+  for (int i = 0; i < m_param.prediction_horizon; ++i) {
+    const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i));
+    const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i));
+    steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity);
+  }
+
+  return steer_rate_limits;
+}
+
 bool MPC::isValid(const MPCMatrix & m) const
 {
   if (

control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp

@@ -72,12 +72,10 @@ bool QPSolverOSQP::solve(
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful
   int status_polish = std::get<2>(result);
-  if (status_polish == -1) {
-    RCLCPP_WARN(logger_, "osqp status_polish = %d (unsuccessful)", status_polish);
-    return false;
-  }
-  if (status_polish == 0) {
-    RCLCPP_WARN(logger_, "osqp status_polish = %d (unperformed)", status_polish);
+  if (status_polish == -1 || status_polish == 0) {
+    const auto s = (status_polish == 0) ? "Polish process is not performed in osqp."
+                                        : "Polish process failed in osqp.";
+    RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s);
     return true;
   }
   return true;

control/mpc_lateral_controller/test/test_mpc.cpp

@@ -252,9 +252,8 @@ TEST_F(MPCTest, OsqpCalculate)
   AckermannLateralCommand ctrl_cmd;
   Trajectory pred_traj;
   Float32MultiArrayStamped diag;
-  // with OSQP this function returns false despite finding correct solutions
   const auto odom = makeOdometry(pose_zero, default_velocity);
-  EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
+  EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag));
   EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f);
 }

launch/tier4_map_launch/launch/map.launch.py

@@ -64,7 +64,6 @@ def launch_setup(context, *args, **kwargs):
         name="lanelet2_map_loader",
         remappings=[
             ("output/lanelet2_map", "vector_map"),
-            ("input/map_projector_info", "map_projector_type"),
         ],
         parameters=[
             {

localization/ekf_localizer/config/ekf_localizer.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     show_debug_info: false
-    enable_yaw_bias_estimation: True
+    enable_yaw_bias_estimation: true
     predict_frequency: 50.0
     tf_rate: 50.0
     extend_state_step: 50

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -15,6 +15,8 @@
 #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
 #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
 
+#include <component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
@@ -41,9 +43,11 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
     const rclcpp::Time & now);
 
 private:
-  void on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg);
+  using MapProjectorInfo = map_interface::MapProjectorInfo;
 
-  rclcpp::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_type_;
+  void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg);
+
+  component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_type_;
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
 };
 

map/map_loader/package.xml

@@ -16,6 +16,8 @@
 
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_map_msgs</depend>
+  <depend>component_interface_specs</depend>
+  <depend>component_interface_utils</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -51,16 +51,22 @@
 Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options)
 : Node("lanelet2_map_loader", options)
 {
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+
   // subscription
-  sub_map_projector_type_ = create_subscription<MapProjectorInfo>(
-    "input/map_projector_info", rclcpp::QoS{1}.transient_local(),
-    [this](const MapProjectorInfo::ConstSharedPtr msg) { on_map_projector_info(msg); });
+  adaptor.init_sub(
+    sub_map_projector_type_,
+    [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); });
+
+  declare_parameter("lanelet2_map_path", "");
+  declare_parameter("center_line_resolution", 5.0);
 }
 
-void Lanelet2MapLoaderNode::on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg)
+void Lanelet2MapLoaderNode::on_map_projector_info(
+  const MapProjectorInfo::Message::ConstSharedPtr msg)
 {
-  const auto lanelet2_filename = declare_parameter("lanelet2_map_path", "");
-  const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0);
+  const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string();
+  const auto center_line_resolution = get_parameter("center_line_resolution").as_double();
 
   // load map from file
   const auto map =

map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp

@@ -17,7 +17,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "tier4_map_msgs/msg/map_projector_info.hpp"
+#include <component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
 
 #include <string>
 
@@ -29,7 +30,8 @@ class MapProjectionLoader : public rclcpp::Node
   MapProjectionLoader();
 
 private:
-  rclcpp::Publisher<tier4_map_msgs::msg::MapProjectorInfo>::SharedPtr publisher_;
+  using MapProjectorInfo = map_interface::MapProjectorInfo;
+  component_interface_utils::Publisher<MapProjectorInfo>::SharedPtr publisher_;
 };
 
 #endif  // MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_

map/map_projection_loader/launch/map_projection_loader.launch.xml

@@ -3,7 +3,6 @@
   <arg name="lanelet2_map_path" description="Path to the lanelet2 map file"/>
 
   <node pkg="map_projection_loader" exec="map_projection_loader" name="map_projection_loader" output="screen">
-    <remap from="~/map_projector_info" to="/map/map_projector_type"/>
     <param name="map_projector_info_path" value="$(var map_projector_info_path)"/>
     <param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
   </node>

map/map_projection_loader/package.xml

@@ -12,6 +12,8 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>component_interface_specs</depend>
+  <depend>component_interface_utils</depend>
   <depend>lanelet2_extension</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

map/map_projection_loader/src/map_projection_loader.cpp

@@ -16,6 +16,8 @@
 
 #include "map_projection_loader/load_info_from_lanelet2_map.hpp"
 
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
 #include <yaml-cpp/yaml.h>
 
 #include <fstream>
@@ -64,7 +66,7 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader")
   }
 
   // Publish the message
-  publisher_ = this->create_publisher<tier4_map_msgs::msg::MapProjectorInfo>(
-    "~/map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local());
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_pub(publisher_);
   publisher_->publish(msg);
 }

map/map_projection_loader/test/test_node_load_local_from_yaml.test.py

@@ -110,7 +110,7 @@ def test_node_link(self):
         # Create subscription to map_projector_info topic
         subscription = self.test_node.create_subscription(
             MapProjectorInfo,
-            "/map_projection_loader/map_projector_info",
+            "/map/map_projector_type",
             self.callback,
             custom_qos_profile,
         )
@@ -125,7 +125,7 @@ def test_node_link(self):
             get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
         )
         with open(map_projection_info_path) as f:
-            yaml_data = yaml.load(f)
+            yaml_data = yaml.load(f, Loader=yaml.FullLoader)
 
         # Test if message received
         self.assertIsNotNone(

map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py

@@ -110,7 +110,7 @@ def test_node_link(self):
         # Create subscription to map_projector_info topic
         subscription = self.test_node.create_subscription(
             MapProjectorInfo,
-            "/map_projection_loader/map_projector_info",
+            "/map/map_projector_type",
             self.callback,
             custom_qos_profile,
         )
@@ -125,7 +125,7 @@ def test_node_link(self):
             get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
         )
         with open(map_projection_info_path) as f:
-            yaml_data = yaml.load(f)
+            yaml_data = yaml.load(f, Loader=yaml.FullLoader)
 
         # Test if message received
         self.assertIsNotNone(

map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py

@@ -110,7 +110,7 @@ def test_node_link(self):
         # Create subscription to map_projector_info topic
         subscription = self.test_node.create_subscription(
             MapProjectorInfo,
-            "/map_projection_loader/map_projector_info",
+            "/map/map_projector_type",
             self.callback,
             custom_qos_profile,
         )
@@ -125,7 +125,7 @@ def test_node_link(self):
             get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
         )
         with open(map_projection_info_path) as f:
-            yaml_data = yaml.load(f)
+            yaml_data = yaml.load(f, Loader=yaml.FullLoader)
 
         # Test if message received
         self.assertIsNotNone(

planning/behavior_path_planner/CMakeLists.txt

@@ -31,6 +31,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/utils.cpp
   src/utils/path_utils.cpp
   src/utils/path_safety_checker/safety_check.cpp
+  src/utils/path_safety_checker/objects_filtering.cpp
   src/utils/avoidance/utils.cpp
   src/utils/lane_change/utils.cpp
   src/utils/side_shift/util.cpp
@@ -43,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/start_planner/util.cpp
   src/utils/start_planner/shift_pull_out.cpp
   src/utils/start_planner/geometric_pull_out.cpp
+  src/utils/start_planner/freespace_pull_out.cpp
   src/utils/path_shifter/path_shifter.cpp
   src/utils/drivable_area_expansion/drivable_area_expansion.cpp
   src/utils/drivable_area_expansion/map_utils.cpp