chore: sync upstream

.github/sync-files.yaml

@@ -97,3 +97,4 @@
         sd "/edit/main/docs/" "/edit/main/" {source}
         sd "docs_dir: .*" "docs_dir: ." {source}
         sd "assets/(\w+)" "docs/assets/\$1" {source}
+    - source: docs/assets/js/mathjax.js

common/tier4_planning_rviz_plugin/src/path/display.cpp

@@ -53,23 +53,21 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail(
   const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr)
 {
   const size_t size = msg_ptr->points.size();
-  if (size > lane_id_obj_ptrs_.size()) {
-    for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) {
-      std::unique_ptr<Ogre::SceneNode> node_ptr;
-      node_ptr.reset(scene_node_->createChildSceneNode());
-      auto text_ptr =
-        std::make_unique<rviz_rendering::MovableText>("not initialized", "Liberation Sans", 0.1);
-      text_ptr->setVisible(false);
-      text_ptr->setTextAlignment(
-        rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
-      node_ptr->attachObject(text_ptr.get());
-      lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr)));
-    }
-  } else {
-    for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) {
-      scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get());
-    }
-    lane_id_obj_ptrs_.resize(size);
+  // clear previous text
+  for (const auto & [node_ptr, text_ptr] : lane_id_obj_ptrs_) {
+    scene_node_->removeChild(node_ptr.get());
+  }
+  lane_id_obj_ptrs_.clear();
+  for (std::size_t i = 0; i < size; i++) {
+    std::unique_ptr<Ogre::SceneNode> node_ptr;
+    node_ptr.reset(scene_node_->createChildSceneNode());
+    auto text_ptr =
+      std::make_unique<rviz_rendering::MovableText>("not initialized", "Liberation Sans", 0.1);
+    text_ptr->setVisible(false);
+    text_ptr->setTextAlignment(
+      rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+    node_ptr->attachObject(text_ptr.get());
+    lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr)));
   }
 }
 

control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp

@@ -56,6 +56,7 @@ class DebugValues
     PITCH_RAW_TRAJ_DEG = 27,
     STOP_DIST = 28,
     FF_SCALE = 29,
+    ACC_CMD_FF = 30,
     SIZE  // this is the number of enum elements
   };
 

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -946,7 +946,8 @@ double PidLongitudinalController::applyVelocityFeedback(
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0));
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1));
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2));
-  m_debug_values.setValues(DebugValues::TYPE::FF_SCALE, ff_acc);
+  m_debug_values.setValues(DebugValues::TYPE::FF_SCALE, ff_scale);
+  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FF, ff_acc);
 
   return feedback_acc;
 }

docs/assets/js/mathjax.js

@@ -0,0 +1,16 @@
+window.MathJax = {
+  tex: {
+    inlineMath: [["\\(", "\\)"]],
+    displayMath: [["\\[", "\\]"]],
+    processEscapes: true,
+    processEnvironments: true,
+  },
+  options: {
+    ignoreHtmlClass: ".*|",
+    processHtmlClass: "arithmatex",
+  },
+};
+
+document$.subscribe(() => {
+  MathJax.typesetPromise();
+});

mkdocs.yaml

@@ -45,7 +45,9 @@ extra_css:
   - https://use.fontawesome.com/releases/v5.15.4/css/all.css
 
 extra_javascript:
-  - https://cdn.jsdelivr.net/npm/mathjax@2/MathJax.js?config=TeX-AMS-MML_HTMLorMML
+  - docs/assets/js/mathjax.js
+  - https://polyfill.io/v3/polyfill.min.js?features=es6
+  - https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js
 
 plugins:
   - awesome-pages
@@ -73,7 +75,8 @@ markdown_extensions:
   - plantuml_markdown:
       server: http://www.plantuml.com/plantuml
       format: svg
-  - pymdownx.arithmatex
+  - pymdownx.arithmatex:
+      generic: true
   - pymdownx.details
   - pymdownx.emoji:
       emoji_index: !!python/name:materialx.emoji.twemoji

planning/behavior_path_planner/include/behavior_path_planner/marker_util/lane_change/debug.hpp

@@ -39,5 +39,8 @@ MarkerArray showPolygon(
   const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
 MarkerArray showPolygonPose(
   const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray createLaneChangingVirtualWallMarker(
+  const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
+  const rclcpp::Time & now, const std::string & ns);
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTIL__LANE_CHANGE__DEBUG_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp

@@ -91,6 +91,8 @@ class LaneChangeInterface : public SceneModuleInterface
 
   void setData(const std::shared_ptr<const PlannerData> & data) override;
 
+  MarkerArray getModuleVirtualWall() override;
+
 protected:
   std::shared_ptr<LaneChangeParameters> parameters_;
 
@@ -105,6 +107,7 @@ class LaneChangeInterface : public SceneModuleInterface
   void updateSteeringFactorPtr(
     const CandidateOutput & output, const LaneChangePath & selected_path) const;
 
+  mutable MarkerArray virtual_wall_marker_;
   mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
 
   std::unique_ptr<PathWithLaneId> prev_approved_path_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -25,6 +25,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 #include <rtc_interface/rtc_interface.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -33,6 +34,7 @@
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
+#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
 #include <algorithm>
 #include <limits>
@@ -356,6 +358,9 @@ class SceneModuleInterface
       appendMarkerArray(virtual_wall, &markers);
     }
 
+    const auto module_specific_wall = getModuleVirtualWall();
+    appendMarkerArray(module_specific_wall, &markers);
+
     pub_virtual_wall_->publish(markers);
 
     resetWallPoses();
@@ -378,6 +383,8 @@ class SceneModuleInterface
 
   MarkerArray getDebugMarkers() const { return debug_marker_; }
 
+  virtual MarkerArray getModuleVirtualWall() { return MarkerArray(); }
+
   ModuleStatus getCurrentStatus() const { return current_state_; }
 
   StopReason getStopReason() const { return stop_reason_; }

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp

@@ -154,6 +154,9 @@ class SceneModuleManagerInterface
         appendMarkerArray(virtual_wall, &markers);
       }
 
+      const auto module_specific_wall = m->getModuleVirtualWall();
+      appendMarkerArray(module_specific_wall, &markers);
+
       m->resetWallPoses();
     }
 
@@ -203,6 +206,10 @@ class SceneModuleManagerInterface
 
   bool isSimultaneousExecutableAsApprovedModule() const
   {
+    if (registered_modules_.empty()) {
+      return enable_simultaneous_execution_as_approved_module_;
+    }
+
     return std::all_of(
       registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) {
         return module->isSimultaneousExecutableAsApprovedModule();
@@ -211,6 +218,10 @@ class SceneModuleManagerInterface
 
   bool isSimultaneousExecutableAsCandidateModule() const
   {
+    if (registered_modules_.empty()) {
+      return enable_simultaneous_execution_as_candidate_module_;
+    }
+
     return std::all_of(
       registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) {
         return module->isSimultaneousExecutableAsCandidateModule();

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp

@@ -32,6 +32,8 @@ struct LaneChangePath
   PathWithLaneId path{};
   lanelet::ConstLanelets reference_lanelets{};
   lanelet::ConstLanelets target_lanelets{};
+  Pose lane_changing_start{};
+  Pose lane_changing_end{};
   ShiftedPath shifted_path{};
   ShiftLine shift_line{};
   double acceleration{0.0};

planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp

@@ -19,7 +19,9 @@
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <visualization_msgs/msg/detail/marker__struct.hpp>
 
+#include <cstdint>
 #include <cstdlib>
 #include <iomanip>
 #include <string>
@@ -235,4 +237,34 @@ MarkerArray showPolygonPose(
 
   return marker_array;
 }
+
+MarkerArray createLaneChangingVirtualWallMarker(
+  const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
+  const rclcpp::Time & now, const std::string & ns)
+{
+  int32_t id{0};
+  MarkerArray marker_array{};
+  marker_array.markers.reserve(2);
+  {
+    auto wall_marker = createDefaultMarker(
+      "map", now, ns + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE,
+      createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(0.0, 1.0, 0.0, 0.5));
+    wall_marker.pose = lane_changing_pose;
+    wall_marker.pose.position.z += 1.0;
+    marker_array.markers.push_back(wall_marker);
+  }
+
+  {
+    auto text_marker = createDefaultMarker(
+      "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+    text_marker.pose = lane_changing_pose;
+    text_marker.pose.position.z += 2.0;
+    text_marker.text = module_name;
+    marker_array.markers.push_back(text_marker);
+  }
+
+  return marker_array;
+}
+
 }  // namespace marker_utils::lane_change_markers

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -14,11 +14,15 @@
 
 #include "behavior_path_planner/scene_module/lane_change/interface.hpp"
 
+#include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/module_status.hpp"
+#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
 #include <algorithm>
 #include <limits>
 #include <memory>
@@ -305,6 +309,26 @@ std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_arra
   return std::make_shared<LaneChangeDebugMsgArray>(lane_change_debug_msg_array_);
 }
 
+MarkerArray LaneChangeInterface::getModuleVirtualWall()
+{
+  using marker_utils::lane_change_markers::createLaneChangingVirtualWallMarker;
+  MarkerArray marker;
+  if (isWaitingApproval() || current_state_ != ModuleStatus::RUNNING) {
+    return marker;
+  }
+  const auto & start_pose = module_type_->getLaneChangePath().lane_changing_start;
+  const auto start_marker =
+    createLaneChangingVirtualWallMarker(start_pose, name(), clock_->now(), "lane_change_start");
+
+  const auto & end_pose = module_type_->getLaneChangePath().lane_changing_end;
+  const auto end_marker =
+    createLaneChangingVirtualWallMarker(end_pose, name(), clock_->now(), "lane_change_end");
+  marker.markers.reserve(start_marker.markers.size() + end_marker.markers.size());
+  appendMarkerArray(start_marker, &marker);
+  appendMarkerArray(end_marker, &marker);
+  return marker;
+}
+
 void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output)
 {
   const auto steering_factor_direction = std::invoke([&]() {

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -254,10 +254,10 @@ std::optional<LaneChangePath> constructCandidatePath(
       .get_child("constructCandidatePath"),
     "prepare_length: %f, lane_change: %f", prepare_length, lane_changing_length);
 
-  const PathPointWithLaneId & lane_changing_end_point = target_segment.points.front();
-  const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose;
+  candidate_path.lane_changing_start = prepare_segment.points.back().point.pose;
+  candidate_path.lane_changing_end = target_segment.points.front().point.pose;
   const auto lane_change_end_idx =
-    motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose);
+    motion_utils::findNearestIndex(shifted_path.path.points, candidate_path.lane_changing_end);
 
   if (!lane_change_end_idx) {
     RCLCPP_ERROR_STREAM(

planning/behavior_velocity_blind_spot_module/src/debug.cpp

@@ -116,7 +116,7 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createVirtualWallMarkerArr
   if (!isActivated() && !is_over_pass_judge_line_) {
     appendMarkerArray(
       virtual_wall_marker_creator_->createStopVirtualWallMarker(
-        {debug_data_.virtual_wall_pose}, "blind_spot", now),
+        {debug_data_.virtual_wall_pose}, "blind_spot", now, 0.0, std::to_string(module_id_) + "_"),
       &wall_marker, now);
   }
   return wall_marker;

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -100,6 +100,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.max_vehicle_velocity_for_rss =
     node.declare_parameter<double>(ns + ".occlusion.max_vehicle_velocity_for_rss");
   ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
+  ip.occlusion.pub_debug_grid = node.declare_parameter<bool>(ns + ".occlusion.pub_debug_grid");
 }
 
 void IntersectionModuleManager::launchNewModules(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1256,26 +1256,24 @@ bool IntersectionModule::isOcclusionCleared(
   }
   debug_data_.nearest_occlusion_point = nearest_occlusion_point;
 
-  cv::Mat distance_grid_heatmap;
-  cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
-  /*
-  cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL);
-  cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap);
-  cv::waitKey(1);
-  */
-  grid_map::GridMap occlusion_grid({"elevation"});
-  occlusion_grid.setFrameId("map");
-  occlusion_grid.setGeometry(
-    grid_map::Length(width * resolution, height * resolution), resolution,
-    grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2));
-  cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE);
-  cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE);
-  grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(
-    distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */,
-    origin.z + distance_max /* elevation for 255 */);
-  grid_map::GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(
-    distance_grid_heatmap, "color", occlusion_grid);
-  occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid));
+  if (planner_param_.occlusion.pub_debug_grid) {
+    cv::Mat distance_grid_heatmap;
+    cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
+    grid_map::GridMap occlusion_grid({"elevation"});
+    occlusion_grid.setFrameId("map");
+    occlusion_grid.setGeometry(
+      grid_map::Length(width * resolution, height * resolution), resolution,
+      grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2));
+    cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE);
+    cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE);
+    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(
+      distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */,
+      origin.z + distance_max /* elevation for 255 */);
+    grid_map::GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(
+      distance_grid_heatmap, "color", occlusion_grid);
+    occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid));
+  }
+
   if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) {
     return true;
   } else {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -123,6 +123,7 @@ class IntersectionModule : public SceneModuleInterface
       double min_vehicle_brake_for_rss;
       double max_vehicle_velocity_for_rss;
       double denoise_kernel;
+      bool pub_debug_grid;
     } occlusion;
   };
 

planning/behavior_velocity_no_stopping_area_module/src/debug.cpp

@@ -174,7 +174,8 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createVirtualWallMark
     stop_poses.push_back(p_front);
   }
   appendMarkerArray(
-    virtual_wall_marker_creator_->createStopVirtualWallMarker(stop_poses, "no_stopping_area", now),
+    virtual_wall_marker_creator_->createStopVirtualWallMarker(
+      stop_poses, "no_stopping_area", now, 0.0, std::to_string(module_id_) + "_"),
     &wall_marker, now);
 
   return wall_marker;

planning/behavior_velocity_occlusion_spot_module/src/debug.cpp

@@ -221,14 +221,13 @@ MarkerArray OcclusionSpotModule::createVirtualWallMarkerArray()
   MarkerArray wall_marker;
   std::string module_name = "occlusion_spot";
   std::vector<Pose> slow_down_poses;
-  size_t module_id = 0;
   for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) {
     const auto p_front =
       calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0);
     slow_down_poses.push_back(p_front);
     appendMarkerArray(
       virtual_wall_marker_creator_->createSlowDownVirtualWallMarker(
-        slow_down_poses, module_name, current_time, module_id),
+        slow_down_poses, module_name, current_time),
       &wall_marker, current_time);
   }
   return wall_marker;