Merge branch 'main' into chore/add_sensing_maintainers

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

@@ -38,7 +38,8 @@ class InterProcessPollingSubscriber
     noexec_subscription_options.callback_group = noexec_callback_group;
 
     subscriber_ = node->create_subscription<T>(
-      topic_name, rclcpp::QoS{1}, [node](const typename T::ConstSharedPtr msg) { assert(false); },
+      topic_name, rclcpp::QoS{1},
+      [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
       noexec_subscription_options);
   };
   bool updateLatestData()

perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp

@@ -41,7 +41,10 @@ class TrackerDebugger
     const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
   void startMeasurementTime(
     const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp);
+  void endMeasurementTime(const rclcpp::Time & now);
+  void startPublishTime(const rclcpp::Time & now);
   void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time);
+
   void setupDiagnostics();
   void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
   struct DEBUG_SETTINGS
@@ -56,12 +59,15 @@ class TrackerDebugger
 
 private:
   void loadParameters();
+  bool is_initialized_ = false;
   rclcpp::Node & node_;
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
     debug_tentative_objects_pub_;
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
   rclcpp::Time last_input_stamp_;
   rclcpp::Time stamp_process_start_;
+  rclcpp::Time stamp_process_end_;
+  rclcpp::Time stamp_publish_start_;
   rclcpp::Time stamp_publish_output_;
 };
 

perception/multi_object_tracker/src/debugger.cpp

@@ -108,16 +108,35 @@ void TrackerDebugger::startMeasurementTime(
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/input_latency_ms", input_latency_ms);
   }
+  // initialize debug time stamps
+  if (!is_initialized_) {
+    stamp_publish_output_ = now;
+    is_initialized_ = true;
+  }
+}
+
+void TrackerDebugger::endMeasurementTime(const rclcpp::Time & now)
+{
+  stamp_process_end_ = now;
+}
+
+void TrackerDebugger::startPublishTime(const rclcpp::Time & now)
+{
+  stamp_publish_start_ = now;
 }
 
 void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time)
 {
   const auto current_time = now;
-  // pipeline latency: time from sensor measurement to publish
+  // pipeline latency: time from sensor measurement to publish, used for 'checkDelay'
   pipeline_latency_ms_ = (current_time - last_input_stamp_).seconds() * 1e3;
-  if (debug_settings_.publish_processing_time) {
-    // processing time: time between input and publish
-    double processing_time_ms = (current_time - stamp_process_start_).seconds() * 1e3;
+  if (debug_settings_.publish_processing_time && is_initialized_) {
+    // processing latency: time between input and publish
+    double processing_latency_ms = ((current_time - stamp_process_start_).seconds()) * 1e3;
+    // processing time: only the time spent in the tracking processes
+    double processing_time_ms = ((current_time - stamp_publish_start_).seconds() +
+                                 (stamp_process_end_ - stamp_process_start_).seconds()) *
+                                1e3;
     // cycle time: time between two consecutive publish
     double cyclic_time_ms = (current_time - stamp_publish_output_).seconds() * 1e3;
     // measurement to tracked-object time: time from the sensor measurement to the publishing object
@@ -131,6 +150,8 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim
       "debug/cyclic_time_ms", cyclic_time_ms);
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/processing_time_ms", processing_time_ms);
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_latency_ms", processing_latency_ms);
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/meas_to_tracked_object_ms", measurement_to_object_ms);
   }

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -190,6 +190,10 @@ void MultiObjectTracker::onMeasurement(
     if (tracker) list_tracker_.push_back(tracker);
   }
 
+  // debugger time
+  debugger_->endMeasurementTime(this->now());
+
+  // Publish objects if the timer is not enabled
   if (publish_timer_ == nullptr) {
     publish(measurement_time);
   }
@@ -337,6 +341,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish(
 
 void MultiObjectTracker::publish(const rclcpp::Time & time) const
 {
+  debugger_->startPublishTime(this->now());
   const auto subscriber_count = tracked_objects_pub_->get_subscription_count() +
                                 tracked_objects_pub_->get_intra_process_subscription_count();
   if (subscriber_count < 1) {

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -287,7 +287,8 @@
           nominal_jerk: 0.5                             # [m/sss]
           max_deceleration: -1.5                        # [m/ss]
           max_jerk: 1.0                                 # [m/sss]
-          max_acceleration: 1.0                         # [m/ss]
+          max_acceleration: 0.5                         # [m/ss]
+          min_velocity_to_limit_max_acceleration: 2.78  # [m/ss]
 
       shift_line_pipeline:
         trim:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -143,6 +143,9 @@ struct AvoidanceParameters
   // To prevent large acceleration while avoidance.
   double max_acceleration{0.0};
 
+  // To prevent large acceleration while avoidance.
+  double min_velocity_to_limit_max_acceleration{0.0};
+
   // upper distance for envelope polygon expansion.
   double upper_distance_for_polygon_expansion{0.0};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::helper::avoidance
@@ -32,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::PlannerData;
 using motion_utils::calcDecelDistWithJerkAndAccConstraints;
+using motion_utils::calcLongitudinalOffsetPose;
+using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
+using tier4_autoware_utils::getPose;
 
 class AvoidanceHelper
 {
@@ -61,6 +65,11 @@ class AvoidanceHelper
 
   geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; }
 
+  geometry_msgs::msg::Point getEgoPosition() const
+  {
+    return data_->self_odometry->pose.pose.position;
+  }
+
   static size_t getConstraintsMapIndex(const double velocity, const std::vector<double> & values)
   {
     const auto itr = std::find_if(
@@ -262,6 +271,20 @@ class AvoidanceHelper
     return *itr;
   }
 
+  std::pair<double, double> getDistanceToAccelEndPoint(const PathWithLaneId & path)
+  {
+    updateAccelEndPoint(path);
+
+    if (!max_v_point_.has_value()) {
+      return std::make_pair(0.0, std::numeric_limits<double>::max());
+    }
+
+    const auto start_idx = data_->findEgoIndex(path.points);
+    const auto distance =
+      calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
+    return std::make_pair(distance, max_v_point_.value().second);
+  }
+
   double getFeasibleDecelDistance(
     const double target_velocity, const bool use_hard_constraints = true) const
   {
@@ -367,6 +390,56 @@ class AvoidanceHelper
     return true;
   }
 
+  void updateAccelEndPoint(const PathWithLaneId & path)
+  {
+    const auto & p = parameters_;
+    const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
+    if (a_now < 0.0) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (getEgoSpeed() < p->min_velocity_to_limit_max_acceleration) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (max_v_point_.has_value()) {
+      return;
+    }
+
+    const auto v0 = getEgoSpeed() + p->buf_slow_down_speed;
+
+    const auto t_neg_jerk = std::max(0.0, a_now - p->max_acceleration) / p->max_jerk;
+    const auto v_neg_jerk = v0 + a_now * t_neg_jerk + std::pow(t_neg_jerk, 2.0) / 2.0;
+    const auto x_neg_jerk = v0 * t_neg_jerk + a_now * std::pow(t_neg_jerk, 2.0) / 2.0 +
+                            p->max_jerk * std::pow(t_neg_jerk, 3.0) / 6.0;
+
+    const auto & v_max = data_->parameters.max_vel;
+    if (v_max < v_neg_jerk) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    const auto t_max_accel = (v_max - v_neg_jerk) / p->max_acceleration;
+    const auto x_max_accel =
+      v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0;
+
+    const auto point =
+      calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
+    if (point.has_value()) {
+      max_v_point_ = std::make_pair(point.value(), v_max);
+      return;
+    }
+
+    const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
+    const auto t_end =
+      (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
+    const auto v_end = v0 + p->max_acceleration * t_end;
+
+    max_v_point_ = std::make_pair(getPose(path.points.back()), v_end);
+  }
+
   void reset()
   {
     prev_reference_path_ = PathWithLaneId();
@@ -417,6 +490,8 @@ class AvoidanceHelper
   ShiftedPath prev_linear_shift_path_;
 
   lanelet::ConstLanelets prev_driving_lanes_;
+
+  std::optional<std::pair<Pose, double>> max_v_point_;
 };
 }  // namespace behavior_path_planner::helper::avoidance
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -313,6 +313,10 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.use_shorten_margin_immediately =
       getOrDeclareParameter<bool>(*node, ns + "use_shorten_margin_immediately");
 
+    if (p.policy_approval != "per_shift_line" && p.policy_approval != "per_avoidance_maneuver") {
+      throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'.");
+    }
+
     if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") {
       throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'.");
     }
@@ -330,6 +334,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.max_deceleration = getOrDeclareParameter<double>(*node, ns + "max_deceleration");
     p.max_jerk = getOrDeclareParameter<double>(*node, ns + "max_jerk");
     p.max_acceleration = getOrDeclareParameter<double>(*node, ns + "max_acceleration");
+    p.min_velocity_to_limit_max_acceleration =
+      getOrDeclareParameter<double>(*node, ns + "min_velocity_to_limit_max_acceleration");
   }
 
   // constraints (lateral)

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -233,6 +233,12 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void insertPrepareVelocity(ShiftedPath & shifted_path) const;
 
+  /**
+   * @brief insert max velocity in output path to limit acceleration.
+   * @param target path.
+   */
+  void insertAvoidanceVelocity(ShiftedPath & shifted_path) const;
+
   /**
    * @brief calculate stop distance based on object's overhang.
    * @param stop distance.