Merge branch 'beta/v0.29.0' into fix/correct_radar_lanlet_filter_para…

…m_path

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -147,7 +147,7 @@
   <!-- Diagnostic Converter -->
   <group if="$(var launch_diagnostic_converter)">
     <node name="diagnostic_converter" exec="diagnostic_converter" pkg="diagnostic_converter" output="screen">
-      <param name="diagnostic_topics" value="[/planning/planning_evaluator/metrics, /diagnostic/control_evaluator/metrics, /diagnostics_err]"/>
+      <param name="diagnostic_topics" value="[/planning/planning_evaluator/metrics, /control/control_evaluator/metrics, /diagnostics_err]"/>
     </node>
   </group>
 

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -78,7 +78,6 @@ class MultiObjectTracker : public rclcpp::Node
   // publish timer
   rclcpp::TimerBase::SharedPtr publish_timer_;
   rclcpp::Time last_published_time_;
-  double publisher_period_;
 
   // internal states
   std::string world_frame_id_;  // tracking frame

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -153,9 +153,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   // If the delay compensation is enabled, the timer is used to publish the output at the correct
   // time.
   if (enable_delay_compensation) {
-    publisher_period_ = 1.0 / publish_rate;    // [s]
-    constexpr double timer_multiplier = 20.0;  // 20 times frequent for publish timing check
-    const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period();
+    const auto timer_period = rclcpp::Rate(publish_rate).period();
     publish_timer_ = rclcpp::create_timer(
       this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
   }
@@ -210,35 +208,13 @@ void MultiObjectTracker::onTrigger()
   std::vector<std::pair<uint, DetectedObjects>> objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (!is_objects_ready) return;
-
   onMessage(objects_list);
-  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
-
-  // Publish objects if the timer is not enabled
-  if (publish_timer_ == nullptr) {
-    // if the delay compensation is disabled, publish the objects in the latest time
-    publish(latest_time);
-  } else {
-    // Publish if the next publish time is close
-    const double minimum_publish_interval = publisher_period_ * 0.70;  // 70% of the period
-    const rclcpp::Time publish_time = this->now();
-    if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) {
-      checkAndPublish(publish_time);
-    }
-  }
 }
 
 void MultiObjectTracker::onTimer()
 {
   const rclcpp::Time current_time = this->now();
 
-  // Check the publish period
-  const auto elapsed_time = (current_time - last_published_time_).seconds();
-  // If the elapsed time is over the period, publish objects with prediction
-  constexpr double maximum_latency_ratio = 1.11;  // 11% margin
-  const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
-  if (elapsed_time < maximum_publish_latency) return;
-
   // get objects from the input manager and run process
   ObjectsList objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);

planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp

@@ -61,8 +61,10 @@ class RTCInterface
   bool isActivated(const UUID & uuid) const;
   bool isRegistered(const UUID & uuid) const;
   bool isRTCEnabled(const UUID & uuid) const;
+  bool isTerminated(const UUID & uuid) const;
   void lockCommandUpdate();
   void unlockCommandUpdate();
+  void print() const;
 
 private:
   void onCooperateCommandService(

planning/autoware_rtc_interface/src/rtc_interface.cpp

@@ -20,7 +20,7 @@ namespace
 {
 using tier4_rtc_msgs::msg::Module;
 
-std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
+std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & uuid)
 {
   std::stringstream ss;
   for (auto i = 0; i < 16; ++i) {
@@ -29,6 +29,34 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
   return ss.str();
 }
 
+std::string command_to_string(const uint8_t type)
+{
+  if (type == tier4_rtc_msgs::msg::Command::ACTIVATE) {
+    return "ACTIVATE";
+  } else if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) {
+    return "DEACTIVATE";
+  }
+
+  throw std::domain_error("invalid rtc command.");
+}
+
+std::string state_to_string(const uint8_t type)
+{
+  if (type == tier4_rtc_msgs::msg::State::WAITING_FOR_EXECUTION) {
+    return "WAITING_FOR_EXECUTION";
+  } else if (type == tier4_rtc_msgs::msg::State::RUNNING) {
+    return "RUNNING";
+  } else if (type == tier4_rtc_msgs::msg::State::ABORTING) {
+    return "ABORTING";
+  } else if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) {
+    return "SUCCEEDED";
+  } else if (type == tier4_rtc_msgs::msg::State::FAILED) {
+    return "FAILED";
+  }
+
+  throw std::domain_error("invalid rtc state.");
+}
+
 Module getModuleType(const std::string & module_name)
 {
   Module module;
@@ -158,14 +186,14 @@ std::vector<CooperateResponse> RTCInterface::validateCooperateCommands(
       } else {
         RCLCPP_WARN_STREAM(
           getLogger(), "[validateCooperateCommands] uuid : "
-                         << to_string(command.uuid)
+                         << uuid_to_string(command.uuid)
                          << " state is not WAITING_FOR_EXECUTION or RUNNING. state : "
                          << itr->state.type << std::endl);
         response.success = false;
       }
     } else {
       RCLCPP_WARN_STREAM(
-        getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid)
+        getLogger(), "[validateCooperateCommands] uuid : " << uuid_to_string(command.uuid)
                                                            << " is not found." << std::endl);
       response.success = false;
     }
@@ -262,7 +290,7 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid)
 
   RCLCPP_WARN_STREAM(
     getLogger(),
-    "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl);
+    "[removeCooperateStatus] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
 }
 
 void RTCInterface::removeStoredCommand(const UUID & uuid)
@@ -313,7 +341,7 @@ bool RTCInterface::isActivated(const UUID & uuid) const
   }
 
   RCLCPP_WARN_STREAM(
-    getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl);
+    getLogger(), "[isActivated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
   return false;
 }
 
@@ -338,7 +366,23 @@ bool RTCInterface::isRTCEnabled(const UUID & uuid) const
   }
 
   RCLCPP_WARN_STREAM(
-    getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl);
+    getLogger(), "[isRTCEnabled] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
+  return is_auto_mode_enabled_;
+}
+
+bool RTCInterface::isTerminated(const UUID & uuid) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  const auto itr = std::find_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [uuid](auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    return itr->state.type == State::SUCCEEDED || itr->state.type == State::FAILED;
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(), "[isTerminated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
   return is_auto_mode_enabled_;
 }
 
@@ -363,4 +407,16 @@ bool RTCInterface::isLocked() const
   return is_locked_;
 }
 
+void RTCInterface::print() const
+{
+  RCLCPP_INFO_STREAM(getLogger(), "---print rtc cooperate statuses---" << std::endl);
+  for (const auto status : registered_status_.statuses) {
+    RCLCPP_INFO_STREAM(
+      getLogger(), "uuid:" << uuid_to_string(status.uuid)
+                           << " command:" << command_to_string(status.command_status.type)
+                           << std::boolalpha << " safe:" << status.safe
+                           << " state:" << state_to_string(status.state.type) << std::endl);
+  }
+}
+
 }  // namespace autoware::rtc_interface

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.policy_ambiguous_vehicle = getOrDeclareParameter<std::string>(*node, ns + "policy");
+    p.wait_and_see_target_behaviors =
+      getOrDeclareParameter<std::vector<std::string>>(*node, ns + "wait_and_see.target_behaviors");
+    p.wait_and_see_th_closest_distance =
+      getOrDeclareParameter<double>(*node, ns + "wait_and_see.th_closest_distance");
     p.time_threshold_for_ambiguous_vehicle =
       getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
     p.distance_threshold_for_ambiguous_vehicle =

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -114,10 +114,8 @@ void AvoidanceByLaneChange::updateSpecialData()
                    : Direction::RIGHT;
   }
 
-  utils::static_obstacle_avoidance::updateRegisteredObject(
-    registered_objects_, avoidance_data_.target_objects, p);
-  utils::static_obstacle_avoidance::compensateDetectionLost(
-    registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects);
+  utils::static_obstacle_avoidance::compensateLostTargetObjects(
+    registered_objects_, avoidance_data_, clock_.now(), planner_data_, p);
 
   std::sort(
     avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(),

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -335,8 +335,15 @@ class SceneModuleInterface
   {
     return std::any_of(
       rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) {
-        return rtc.second->isRegistered(uuid_map_.at(rtc.first)) &&
-               rtc.second->isActivated(uuid_map_.at(rtc.first));
+        if (!rtc.second->isRegistered(uuid_map_.at(rtc.first))) {
+          return false;
+        }
+
+        if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) {
+          return true;
+        }
+
+        return rtc.second->isActivated(uuid_map_.at(rtc.first));
       });
   }
 
@@ -345,7 +352,8 @@ class SceneModuleInterface
     return std::any_of(
       rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) {
         return rtc.second->isRegistered(uuid_map_.at(rtc.first)) &&
-               !rtc.second->isActivated(uuid_map_.at(rtc.first));
+               !rtc.second->isActivated(uuid_map_.at(rtc.first)) &&
+               !rtc.second->isTerminated(uuid_map_.at(rtc.first));
       });
   }
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md

@@ -401,6 +401,15 @@ else (\n no)
 #00FFB1 :IGNORE;
 stop
 endif
+if(Is UNKNOWN objects?) then (yes)
+if(isSatisfiedWithUnknownTypeObjectCodition) then (yes)
+#FF006C :AVOID;
+stop
+else (\n no)
+#00FFB1 :IGNORE;
+stop
+endif
+else (\n no)
 if(Is vehicle type?) then (yes)
 if(isSatisfiedWithVehicleCodition()) then (yes)
 else (\n no)
@@ -618,6 +627,18 @@ title Filter vehicle which is obviously an avoidance target
 start
 
 partition isObviousAvoidanceTarget() {
+if(Is object within freespace?) then (yes)
+if(Is object on ego lane?) then (no)
+if(Is object stopping longer than threshold?) then (yes)
+#FF006C :return true;
+stop
+else (\n no)
+endif
+else (\n yes)
+endif
+else (\n no)
+endif
+
 if(Is object within intersection?) then (yes)
 else (\n no)
 if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes)

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml

@@ -138,8 +138,11 @@
 
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
-          enable: true                                  # [-]
-          closest_distance_to_wait_and_see: 10.0        # [m]
+          # policy for ego behavior for ambiguous vehicle.
+          # "auto"   : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically.
+          # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode.
+          # "ignore" : never avoid it.
+          policy: "auto"                                # [-]
           condition:
             th_stopped_time: 3.0                        # [s]
             th_moving_distance: 1.0                     # [m]
@@ -149,11 +152,18 @@
             crosswalk:
               front_distance: 30.0                      # [m]
               behind_distance: 30.0                     # [m]
+          wait_and_see:
+            target_behaviors: ["MERGING", "DEVIATING"]  # [-]
+            th_closest_distance: 10.0                   # [m]
 
         # params for filtering objects that are in intersection
         intersection:
           yaw_deviation: 0.349                          # [rad] (default 20.0deg)
 
+        freespace:
+          condition:
+            th_stopped_time: 5.0                        # [-]
+
       # For safety check
       safety_check:
         # safety check target type

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

@@ -108,7 +108,7 @@ struct AvoidanceParameters
   bool enable_cancel_maneuver{false};
 
   // enable avoidance for all parking vehicle
-  bool enable_avoidance_for_ambiguous_vehicle{false};
+  std::string policy_ambiguous_vehicle{"ignore"};
 
   // enable yield maneuver.
   bool enable_yield_maneuver{false};
@@ -191,8 +191,12 @@ struct AvoidanceParameters
   // minimum road shoulder width. maybe 0.5 [m]
   double object_check_min_road_shoulder_width{0.0};
 
+  // time threshold for vehicle in freespace.
+  double freespace_condition_th_stopped_time{0.0};
+
   // force avoidance
-  double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0};
+  std::vector<std::string> wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"};
+  double wait_and_see_th_closest_distance{0.0};
   double time_threshold_for_ambiguous_vehicle{0.0};
   double distance_threshold_for_ambiguous_vehicle{0.0};
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp

@@ -47,6 +47,11 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
 MarkerArray createOtherObjectsMarkerArray(
   const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose);
 
+MarkerArray createAmbiguousObjectsMarkerArray(
+  const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy);
+
+MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data);
+
 MarkerArray createDebugMarkerArray(
   const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
   const std::shared_ptr<AvoidanceParameters> & parameters);