Merge branch 'main' into traffic-light-classifier

common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp

@@ -52,7 +52,7 @@ struct NodeInterface
        {ServiceLog::CLIENT_RESPONSE, "client exit"},
        {ServiceLog::ERROR_UNREADY, "client unready"},
        {ServiceLog::ERROR_TIMEOUT, "client timeout"}});
-    RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name);
+    RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name);
 
     ServiceLog msg;
     msg.stamp = node->now();

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

@@ -17,6 +17,7 @@
 
 #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp"
 #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp"
+#include "std_msgs/msg/header.hpp"
 
 #include <vector>
 
@@ -34,7 +35,8 @@ namespace motion_utils
  * points larger than the capacity. (Tier IV)
  */
 autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory);
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header = std_msgs::msg::Header{});
 
 /**
  * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to

common/motion_utils/src/resample/resample.cpp

@@ -486,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
   autoware_auto_planning_msgs::msg::Path resampled_path;
   resampled_path.header = input_path.header;
   resampled_path.left_bound = input_path.left_bound;
-  resampled_path.right_bound = resampled_path.right_bound;
+  resampled_path.right_bound = input_path.right_bound;
   resampled_path.points.resize(interpolated_pose.size());
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
     autoware_auto_planning_msgs::msg::PathPoint path_point;

common/motion_utils/src/trajectory/conversion.cpp

@@ -30,9 +30,11 @@ namespace motion_utils
  * points larger than the capacity. (Tier IV)
  */
 autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory)
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header)
 {
   autoware_auto_planning_msgs::msg::Trajectory output{};
+  output.header = header;
   for (const auto & pt : trajectory) output.points.push_back(pt);
   return output;
 }

common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp

@@ -41,20 +41,20 @@ class Client
     const typename ServiceT::Request::SharedPtr & request,
     const std::chrono::nanoseconds & timeout = std::chrono::seconds(2))
   {
-    RCLCPP_INFO(logger_, "client request");
+    RCLCPP_DEBUG(logger_, "client request");
 
     if (!client_->service_is_ready()) {
-      RCLCPP_INFO(logger_, "client available");
+      RCLCPP_DEBUG(logger_, "client available");
       return {response_error("Internal service is not available."), nullptr};
     }
 
     auto future = client_->async_send_request(request);
     if (future.wait_for(timeout) != std::future_status::ready) {
-      RCLCPP_INFO(logger_, "client timeout");
+      RCLCPP_DEBUG(logger_, "client timeout");
       return {response_error("Internal service has timed out."), nullptr};
     }
 
-    RCLCPP_INFO(logger_, "client response");
+    RCLCPP_DEBUG(logger_, "client response");
     return {response_success(), future.get()};
   }
 

common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

@@ -263,9 +263,9 @@ void ManualController::onClickEnableButton()
   {
     auto req = std::make_shared<EngageSrv::Request>();
     req->engage = true;
-    RCLCPP_INFO(raw_node_->get_logger(), "client request");
+    RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
     if (!client_engage_->service_is_ready()) {
-      RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable");
+      RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
       return;
     }
     client_engage_->async_send_request(

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -171,15 +171,15 @@ public Q_SLOTS:  // NOLINT for Qt
   {
     auto req = std::make_shared<typename T::Request>();
 
-    RCLCPP_INFO(raw_node_->get_logger(), "client request");
+    RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
 
     if (!client->service_is_ready()) {
-      RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable");
+      RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
       return;
     }
 
     client->async_send_request(req, [this](typename rclcpp::Client<T>::SharedFuture result) {
-      RCLCPP_INFO(
+      RCLCPP_DEBUG(
         raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code,
         result.get()->status.message.c_str());
     });

map/map_height_fitter/src/map_height_fitter.cpp

@@ -101,19 +101,19 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
   req->area.center_y = point.y;
   req->area.radius = 50;
 
-  RCLCPP_INFO(logger, "Send request to map_loader");
+  RCLCPP_DEBUG(logger, "Send request to map_loader");
   auto future = cli_map_->async_send_request(req);
   auto status = future.wait_for(std::chrono::seconds(1));
   while (status != std::future_status::ready) {
-    RCLCPP_INFO(logger, "waiting response");
+    RCLCPP_DEBUG(logger, "waiting response");
     if (!rclcpp::ok()) {
       return false;
     }
     status = future.wait_for(std::chrono::seconds(1));
   }
 
   const auto res = future.get();
-  RCLCPP_INFO(
+  RCLCPP_DEBUG(
     logger, "Loaded partial pcd map from map_loader (grid size: %lu)",
     res->new_pointcloud_with_ids.size());
 
@@ -168,7 +168,7 @@ std::optional<Point> MapHeightFitter::Impl::fit(const Point & position, const st
   const auto logger = node_->get_logger();
   tf2::Vector3 point(position.x, position.y, position.z);
 
-  RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
+  RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
 
   if (cli_map_) {
     if (!get_partial_point_cloud_map(position)) {
@@ -193,7 +193,7 @@ std::optional<Point> MapHeightFitter::Impl::fit(const Point & position, const st
     return std::nullopt;
   }
 
-  RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
+  RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
 
   Point result;
   result.x = point.getX();

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -59,6 +59,10 @@
         max_time_for_object_lat_shift: 0.0 # [s]
         lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]
 
+        max_ego_lat_acc: 0.3        # [m/ss]
+        max_ego_lat_jerk: 0.3       # [m/sss]
+        delay_time_ego_shift: 1.0   # [s]
+
         # for same directional object
         overtaking_object:
           max_time_to_collision: 40.0 # [s]

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -35,6 +35,25 @@
 #include <utility>
 #include <vector>
 
+namespace
+{
+template <typename T>
+bool isInVector(const T & val, const std::vector<T> & vec)
+{
+  return std::find(vec.begin(), vec.end(), val) != vec.end();
+}
+
+template <typename T, typename S>
+std::vector<T> getAllKeys(const std::unordered_map<T, S> & map)
+{
+  std::vector<T> keys;
+  for (const auto & pair : map) {
+    keys.push_back(pair.first);
+  }
+  return keys;
+}
+}  // namespace
+
 namespace behavior_path_planner
 {
 using autoware_auto_perception_msgs::msg::PredictedPath;
@@ -97,6 +116,10 @@ struct DynamicAvoidanceParameters
   double max_time_for_lat_shift{0.0};
   double lpf_gain_for_lat_avoid_to_offset{0.0};
 
+  double max_ego_lat_acc{0.0};
+  double max_ego_lat_jerk{0.0};
+  double delay_time_ego_shift{0.0};
+
   double max_time_to_collision_overtaking_object{0.0};
   double start_duration_to_avoid_overtaking_object{0.0};
   double end_duration_to_avoid_overtaking_object{0.0};
@@ -113,6 +136,11 @@ struct TimeWhileCollision
   double time_to_end_collision;
 };
 
+struct LatFeasiblePaths
+{
+  std::vector<geometry_msgs::msg::Point> left_path;
+  std::vector<geometry_msgs::msg::Point> right_path;
+};
 class DynamicAvoidanceModule : public SceneModuleInterface
 {
 public:
@@ -151,6 +179,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     bool is_collision_left{false};
     bool should_be_avoided{false};
     std::vector<PathPointWithLaneId> ref_path_points_for_obj_poly;
+    LatFeasiblePaths ego_lat_feasible_paths;
 
     void update(
       const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
@@ -172,14 +201,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     {
     }
     int max_count_{0};
-    int min_count_{0};
+    int min_count_{0};  // TODO(murooka): The sign needs to be opposite?
 
     void initialize() { current_uuids_.clear(); }
     void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object)
     {
       // add/update object
       if (object_map_.count(uuid) != 0) {
+        const auto prev_object = object_map_.at(uuid);
         object_map_.at(uuid) = object;
+        // TODO(murooka) refactor this. Counter can be moved to DynamicObject,
+        //               and TargetObjectsManager can be removed.
+        object_map_.at(uuid).ego_lat_feasible_paths = prev_object.ego_lat_feasible_paths;
       } else {
         object_map_.emplace(uuid, object);
       }
@@ -195,14 +228,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface
       current_uuids_.push_back(uuid);
     }
 
-    void finalize()
+    void finalize(const LatFeasiblePaths & ego_lat_feasible_paths)
     {
       // decrease counter for not updated uuids
       std::vector<std::string> not_updated_uuids;
       for (const auto & object : object_map_) {
-        if (
-          std::find(current_uuids_.begin(), current_uuids_.end(), object.first) ==
-          current_uuids_.end()) {
+        if (!isInVector(object.first, current_uuids_)) {
           not_updated_uuids.push_back(object.first);
         }
       }
@@ -214,45 +245,46 @@ class DynamicAvoidanceModule : public SceneModuleInterface
         }
       }
 
-      // remove objects whose counter is lower than threshold
-      std::vector<std::string> obsolete_uuids;
+      // update valid object uuids and its variable
       for (const auto & counter : counter_map_) {
-        if (counter.second < min_count_) {
-          obsolete_uuids.push_back(counter.first);
+        if (!isInVector(counter.first, valid_object_uuids_) && max_count_ <= counter.second) {
+          valid_object_uuids_.push_back(counter.first);
+          object_map_.at(counter.first).ego_lat_feasible_paths = ego_lat_feasible_paths;
         }
       }
-      for (const auto & obsolete_uuid : obsolete_uuids) {
-        counter_map_.erase(obsolete_uuid);
-        object_map_.erase(obsolete_uuid);
+      valid_object_uuids_.erase(
+        std::remove_if(
+          valid_object_uuids_.begin(), valid_object_uuids_.end(),
+          [&](const auto & uuid) {
+            return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_;
+          }),
+        valid_object_uuids_.end());
+
+      // remove objects whose counter is lower than threshold
+      const auto counter_map_keys = getAllKeys(counter_map_);
+      for (const auto & key : counter_map_keys) {
+        if (counter_map_.at(key) < min_count_) {
+          counter_map_.erase(key);
+          object_map_.erase(key);
+        }
       }
     }
     std::vector<DynamicAvoidanceObject> getValidObjects() const
     {
-      std::vector<DynamicAvoidanceObject> objects;
-      for (const auto & object : object_map_) {
-        if (counter_map_.count(object.first) != 0) {
-          if (max_count_ <= counter_map_.at(object.first)) {
-            objects.push_back(object.second);
-          }
+      std::vector<DynamicAvoidanceObject> valid_objects;
+      for (const auto & valid_object_uuid : valid_object_uuids_) {
+        if (object_map_.count(valid_object_uuid) == 0) {
+          std::cerr
+            << "[DynamicAvoidance] Internal calculation has an error when getting valid objects."
+            << std::endl;
+          continue;
         }
+        valid_objects.push_back(object_map_.at(valid_object_uuid));
       }
-      return objects;
-    }
-    std::optional<DynamicAvoidanceObject> getValidObject(const std::string & uuid) const
-    {
-      // add/update object
-      if (counter_map_.count(uuid) == 0) {
-        return std::nullopt;
-      }
-      if (counter_map_.at(uuid) < max_count_) {
-        return std::nullopt;
-      }
-      if (object_map_.count(uuid) == 0) {
-        return std::nullopt;
-      }
-      return object_map_.at(uuid);
+
+      return valid_objects;
     }
-    void updateObject(
+    void updateObjectVariables(
       const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
       const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
       const bool should_be_avoided,
@@ -269,6 +301,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     // NOTE: positive is for meeting entry condition, and negative is for exiting.
     std::unordered_map<std::string, int> counter_map_;
     std::unordered_map<std::string, DynamicAvoidanceObject> object_map_;
+    std::vector<std::string> valid_object_uuids_{};
   };
 
   struct DecisionWithReason
@@ -319,6 +352,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
   bool isLabelTargetObstacle(const uint8_t label) const;
   void updateTargetObjects();
+  LatFeasiblePaths generateLateralFeasiblePaths(
+    const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const;
   void updateRefPathBeforeLaneChange(const std::vector<PathPointWithLaneId> & ego_ref_path_points);
   bool willObjectCutIn(
     const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,