Merge branch 'main' into fix/multi-object-tracker-instability-fix

Author: technolojin

map/map_height_fitter/CMakeLists.txt

@@ -14,4 +14,7 @@ ament_auto_add_executable(node
   src/node.cpp
 )
 
-ament_auto_package(INSTALL_TO_SHARE launch)
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config)

map/map_height_fitter/README.md

@@ -4,8 +4,18 @@ This library fits the given point with the ground of the point cloud map.
 The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`.
 The node using this library must use multi thread executor.
 
-| Interface    | Local Name         | Description                              |
-| ------------ | ------------------ | ---------------------------------------- |
-| Parameter    | map_loader_name    | The point cloud map loader name.         |
-| Subscription | ~/pointcloud_map   | The topic name to load the whole map     |
-| Client       | ~/partial_map_load | The service name to load the partial map |
+## Parameters
+
+{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }}
+
+## Topic subscription
+
+| Topic Name       | Description                                                                                  |
+| ---------------- | -------------------------------------------------------------------------------------------- |
+| ~/pointcloud_map | The topic containing the whole pointcloud map (only used when `enable_partial_load = false`) |
+
+## Service client
+
+| Service Name       | Description                         |
+| ------------------ | ----------------------------------- |
+| ~/partial_map_load | The service to load the partial map |

map/map_height_fitter/config/map_height_fitter.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    map_loader_name: "/map/pointcloud_map_loader"

map/map_height_fitter/launch/map_height_fitter.launch.xml

@@ -1,8 +1,10 @@
 <launch>
+  <arg name="param_path" default="$(find-pkg-share map_height_fitter)/config/map_height_fitter.param.yaml"/>
+
   <group>
     <push-ros-namespace namespace="map"/>
     <node pkg="map_height_fitter" exec="node" name="map_height_fitter">
-      <param name="map_loader_name" value="/map/pointcloud_map_loader"/>
+      <param from="$(var param_path)"/>
       <remap from="~/pointcloud_map" to="/map/pointcloud_map"/>
       <remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
     </node>

map/map_height_fitter/schema/map_height_fitter.schema.json

@@ -0,0 +1,33 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for map_height_fitter",
+  "type": "object",
+  "definitions": {
+    "map_height_fitter": {
+      "type": "object",
+      "properties": {
+        "map_loader_name": {
+          "type": "string",
+          "description": "Node name of the map loader from which this map_height_fitter will retrieve its parameters",
+          "default": "/map/pointcloud_map_loader"
+        }
+      },
+      "required": ["map_loader_name"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/map_height_fitter"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -37,6 +37,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 
+#include <atomic>
 #include <deque>
 #include <limits>
 #include <memory>
@@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
+  ~GoalPlannerModule()
+  {
+    if (lane_parking_timer_) {
+      lane_parking_timer_->cancel();
+    }
+    if (freespace_parking_timer_) {
+      freespace_parking_timer_->cancel();
+    }
+
+    while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) {
+      const std::string running_callbacks = std::invoke([&]() {
+        if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) {
+          return "lane parking and freespace parking";
+        }
+        if (is_lane_parking_cb_running_) {
+          return "lane parking";
+        }
+        return "freespace parking";
+      });
+      RCLCPP_INFO_THROTTLE(
+        getLogger(), *clock_, 1000, "Waiting for %s callback to finish...",
+        running_callbacks.c_str());
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    RCLCPP_INFO_THROTTLE(
+      getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished");
+  }
+
   void updateModuleParams(const std::any & parameters) override
   {
     parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
@@ -330,6 +360,18 @@ class GoalPlannerModule : public SceneModuleInterface
   CandidateOutput planCandidate() const override { return CandidateOutput{}; }
 
 private:
+  // Flag class for managing whether a certain callback is running in multi-threading
+  class ScopedFlag
+  {
+  public:
+    explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
+
+    ~ScopedFlag() { flag_.store(false); }
+
+  private:
+    std::atomic<bool> & flag_;
+  };
+
   /*
    * state transitions and plan function used in each state
    *
@@ -403,10 +445,12 @@ class GoalPlannerModule : public SceneModuleInterface
   // pre-generate lane parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr lane_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
+  std::atomic<bool> is_lane_parking_cb_running_;
 
   // generate freespace parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
+  std::atomic<bool> is_freespace_parking_cb_running_;
 
   // debug
   mutable GoalPlannerDebugData debug_data_;

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -62,6 +62,8 @@ GoalPlannerModule::GoalPlannerModule(
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
   thread_safe_data_{mutex_, clock_},
+  is_lane_parking_cb_running_{false},
+  is_freespace_parking_cb_running_{false},
   debug_stop_pose_with_info_{&stop_pose_}
 {
   LaneDepartureChecker lane_departure_checker{};
@@ -171,6 +173,8 @@ bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const
 // generate pull over candidate paths
 void GoalPlannerModule::onTimer()
 {
+  const ScopedFlag flag(is_lane_parking_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
@@ -287,6 +291,8 @@ void GoalPlannerModule::onTimer()
 
 void GoalPlannerModule::onFreespaceParkingTimer()
 {
+  const ScopedFlag flag(is_freespace_parking_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -35,6 +35,7 @@
 
 #include <tf2/utils.h>
 
+#include <atomic>
 #include <deque>
 #include <memory>
 #include <string>
@@ -83,6 +84,21 @@ class StartPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
+  ~StartPlannerModule()
+  {
+    if (freespace_planner_timer_) {
+      freespace_planner_timer_->cancel();
+    }
+
+    while (is_freespace_planner_cb_running_.load()) {
+      RCLCPP_INFO_THROTTLE(
+        getLogger(), *clock_, 1000, "Waiting for freespace planner callback to finish...");
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    RCLCPP_INFO_THROTTLE(getLogger(), *clock_, 1000, "freespace planner callback finished");
+  }
+
   void updateModuleParams(const std::any & parameters) override
   {
     parameters_ = std::any_cast<std::shared_ptr<StartPlannerParameters>>(parameters);
@@ -135,6 +151,18 @@ class StartPlannerModule : public SceneModuleInterface
   bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
 
 private:
+  // Flag class for managing whether a certain callback is running in multi-threading
+  class ScopedFlag
+  {
+  public:
+    explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
+
+    ~ScopedFlag() { flag_.store(false); }
+
+  private:
+    std::atomic<bool> & flag_;
+  };
+
   bool canTransitSuccessState() override;
 
   bool canTransitFailureState() override { return false; }
@@ -202,6 +230,8 @@ class StartPlannerModule : public SceneModuleInterface
   std::unique_ptr<PullOutPlannerBase> freespace_planner_;
   rclcpp::TimerBase::SharedPtr freespace_planner_timer_;
   rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_;
+  std::atomic<bool> is_freespace_planner_cb_running_;
+
   // TODO(kosuke55)
   // Currently, we only do lock when updating a member of status_.
   // However, we need to ensure that the value does not change when referring to it.

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -57,7 +57,8 @@ StartPlannerModule::StartPlannerModule(
     objects_of_interest_marker_interface_ptr_map)
 : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
-  vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}
+  vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
+  is_freespace_planner_cb_running_{false}
 {
   lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
   lane_departure_checker_->setVehicleInfo(vehicle_info_);
@@ -88,6 +89,8 @@ StartPlannerModule::StartPlannerModule(
 
 void StartPlannerModule::onFreespacePlannerTimer()
 {
+  const ScopedFlag flag(is_freespace_planner_cb_running_);
+
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
@@ -1362,7 +1365,6 @@ bool StartPlannerModule::planFreespacePath()
 void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
   if (status_.planner_type == PlannerType::FREESPACE) {
-    std::cerr << "Freespace planner updated drivable area." << std::endl;
     const double drivable_area_margin = planner_data_->parameters.vehicle_width;
     output.drivable_area_info.drivable_margin =
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;