chore: sync upstream

common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp

@@ -28,7 +28,7 @@ void setFormatDate(QLabel * line, double time)
   char buffer[128];
   auto seconds = static_cast<time_t>(time);
   strftime(buffer, sizeof(buffer), "%Y-%m-%d-%H-%M-%S", localtime(&seconds));
-  line->setText(QString("- ") + QString(buffer) + QString(".mp4"));
+  line->setText(QString("- ") + QString(buffer));
 }
 
 AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent)
@@ -104,7 +104,8 @@ void AutowareScreenCapturePanel::onRateChanged() {}
 
 void AutowareScreenCapturePanel::onClickScreenCapture()
 {
-  const std::string time_text = "capture/" + ros_time_label_->text().toStdString();
+  const std::string time_text =
+    "capture/" + file_name_prefix_->text().toStdString() + ros_time_label_->text().toStdString();
   getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot(
     time_text + ".png");
 }

evaluator/planning_evaluator/src/planning_evaluator_node.cpp

@@ -131,7 +131,9 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m
   }
 
   auto start = now();
-  stamps_.push_back(traj_msg->header.stamp);
+  if (!output_file_str_.empty()) {
+    stamps_.push_back(traj_msg->header.stamp);
+  }
 
   DiagnosticArray metrics_msg;
   metrics_msg.header.stamp = now();
@@ -141,7 +143,10 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m
       continue;
     }
 
-    metric_stats_[static_cast<size_t>(metric)].push_back(*metric_stat);
+    if (!output_file_str_.empty()) {
+      metric_stats_[static_cast<size_t>(metric)].push_back(*metric_stat);
+    }
+
     if (metric_stat->count() > 0) {
       metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
     }

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -149,15 +149,14 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   TurnSignalDecider turn_signal_decider_;
 
-  std::mutex mutex_pd_;  // mutex for planner_data_
-  std::mutex mutex_bt_;  // mutex for bt_manager_
+  std::mutex mutex_pd_;       // mutex for planner_data_
+  std::mutex mutex_manager_;  // mutex for bt_manager_ or planner_manager_
+  std::mutex mutex_map_;      // mutex for has_received_map_ and map_ptr_
+  std::mutex mutex_route_;    // mutex for has_received_route_ and route_ptr_
 
   // setup
   bool isDataReady();
 
-  // update planner data
-  std::shared_ptr<PlannerData> createLatestPlannerData();
-
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr_;
   std::shared_ptr<SideShiftParameters> side_shift_param_ptr_;
@@ -195,7 +194,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
    * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point.
    */
   PathWithLaneId modifyPathForSmoothGoalConnection(
-    const PathWithLaneId & path) const;  // (TODO) move to util
+    const PathWithLaneId & path,
+    const std::shared_ptr<PlannerData> & planner_data) const;  // (TODO) move to util
   OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
 
   /**
@@ -206,8 +206,15 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief extract path from behavior tree output
    */
+#ifdef USE_OLD_ARCHITECTURE
+  PathWithLaneId::SharedPtr getPath(
+    const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> & planner_data,
+    const std::shared_ptr<BehaviorTreeManager> & bt_manager);
+#else
   PathWithLaneId::SharedPtr getPath(
-    const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);
+    const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> & planner_data,
+    const std::shared_ptr<PlannerManager> & planner_manager);
+#endif
 
   /**
    * @brief skip smooth goal connection
@@ -242,27 +249,34 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief publish debug messages
    */
-  void publishSceneModuleDebugMsg();
+#ifdef USE_OLD_ARCHITECTURE
+  void publishSceneModuleDebugMsg(
+    const std::shared_ptr<SceneModuleVisitor> & debug_messages_data_ptr);
+#endif
 
   /**
    * @brief publish path candidate
    */
 #ifdef USE_OLD_ARCHITECTURE
   void publishPathCandidate(
-    const std::vector<std::shared_ptr<SceneModuleInterface>> & scene_modules);
+    const std::vector<std::shared_ptr<SceneModuleInterface>> & scene_modules,
+    const std::shared_ptr<PlannerData> & planner_data);
 #else
   void publishPathCandidate(
-    const std::vector<std::shared_ptr<SceneModuleManagerInterface>> & managers);
+    const std::vector<std::shared_ptr<SceneModuleManagerInterface>> & managers,
+    const std::shared_ptr<PlannerData> & planner_data);
 
   void publishPathReference(
-    const std::vector<std::shared_ptr<SceneModuleManagerInterface>> & managers);
+    const std::vector<std::shared_ptr<SceneModuleManagerInterface>> & managers,
+    const std::shared_ptr<PlannerData> & planner_data);
 #endif
 
   /**
    * @brief convert path with lane id to path for publish path candidate
    */
   Path convertToPath(
-    const std::shared_ptr<PathWithLaneId> & path_candidate_ptr, const bool is_ready);
+    const std::shared_ptr<PathWithLaneId> & path_candidate_ptr, const bool is_ready,
+    const std::shared_ptr<PlannerData> & planner_data);
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp

@@ -497,6 +497,7 @@ struct DebugData
   AvoidLineArray quantized;
   AvoidLineArray trim_small_shift;
   AvoidLineArray trim_similar_grad_shift_second;
+  AvoidLineArray trim_similar_grad_shift_third;
   AvoidLineArray trim_momentary_return;
   AvoidLineArray trim_too_sharp_shift;
   std::vector<double> pos_shift;

planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp

@@ -54,10 +54,9 @@ bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
 
-std::pair<double, double> calcLaneChangingSpeedAndDistance(
-  const double velocity, const double shift_length, const double deceleration,
-  const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param,
-  const LaneChangeParameters & lc_param);
+double calcLaneChangingDistance(
+  const double lane_changing_velocity, const double shift_length, const double min_total_lc_len,
+  const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param);
 
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
@@ -113,8 +112,8 @@ ShiftLine getLaneChangingShiftLine(
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double target_lane_length,
-  const double lane_changing_distance, const double min_total_lane_changing_distance,
-  const double forward_path_length, const double resample_interval, const bool is_goal_in_route);
+  const double lane_changing_distance, const double forward_path_length,
+  const double resample_interval, const bool is_goal_in_route);
 
 PathWithLaneId getPrepareSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
@@ -128,8 +127,8 @@ PathWithLaneId getPrepareSegment(
 
 PathWithLaneId getLaneChangingSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
-  const double forward_path_length, const double arc_length_from_target,
-  const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end,
+  const double forward_path_length, const Pose & lane_changing_start_pose,
+  const double target_lane_length, const double lane_changing_distance,
   const double lane_changing_speed, const double total_required_min_dist);
 
 bool isEgoWithinOriginalLane(

planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp

@@ -134,8 +134,7 @@ class PathShifter
   static double calcLongitudinalDistFromJerk(
     const double lateral, const double jerk, const double velocity);
 
-  static double calcShiftTimeFromJerkAndJerk(
-    const double lateral, const double jerk, const double acc);
+  static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc);
 
   static double calcJerkFromLatLonDistance(
     const double lateral, const double longitudinal, const double velocity);