Merge branch 'main' into feat/ndt-diagnotics-exe-time

common/autoware_auto_geometry/CMakeLists.txt

@@ -12,6 +12,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/bounding_box.cpp
 )
 
+if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+  target_compile_definitions(${PROJECT_NAME} PRIVATE
+    DEFINE_LEGACY_FUNCTION
+  )
+endif()
+
 if(BUILD_TESTING)
   set(GEOMETRY_GTEST geometry_gtest)
   set(GEOMETRY_SRC test/src/test_geometry.cpp

common/autoware_auto_tf2/CMakeLists.txt

@@ -17,6 +17,11 @@ if(BUILD_TESTING)
     "tf2_geometry_msgs"
     "tf2_ros"
   )
+  if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+    target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE
+      DEFINE_LEGACY_FUNCTION
+    )
+  endif()
 endif()
 
 ament_auto_package()

common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp

@@ -45,7 +45,7 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
-
+#ifdef DEFINE_LEGACY_FUNCTION
 /*************/
 /** Point32 **/
 /*************/
@@ -94,6 +94,7 @@ inline void doTransform(
     t_out.points[i].z = static_cast<float>(v_out[2]);
   }
 }
+#endif
 
 /******************/
 /** Quaternion32 **/

localization/landmark_based_localizer/ar_tag_based_localizer/README.md

@@ -25,7 +25,8 @@ The positions and orientations of the AR-Tags are assumed to be written in the L
 | :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- |
 | `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose                                                                            |
 | `~/debug/result`                | `sensor_msgs::msg::Image`                       | [debug topic] Image in which marker detection results are superimposed on the input image |
-| `tf`                            | `geometry_msgs::msg::TransformStamped`          | [debug topic] TF from camera to detected tag                                              |
+| `/tf`                           | `geometry_msgs::msg::TransformStamped`          | [debug topic] TF from camera to detected tag                                              |
+| `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                                       |
 
 ## How to launch
 

localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp

@@ -48,6 +48,7 @@
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -98,6 +99,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   // Publishers
   image_transport::Publisher image_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_pub_;
 
   // Others
   aruco::MarkerDetector detector_;

localization/landmark_based_localizer/ar_tag_based_localizer/package.xml

@@ -13,6 +13,7 @@
 
   <depend>aruco</depend>
   <depend>cv_bridge</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>image_transport</depend>
   <depend>rclcpp</depend>

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp

@@ -129,6 +129,8 @@ bool ArTagBasedLocalizer::setup()
   image_pub_ = it_->advertise("~/debug/result", 1);
   pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "~/output/pose_with_covariance", qos_pub);
+  diag_pub_ =
+    this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", qos_pub);
 
   RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!");
   return true;
@@ -199,6 +201,32 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha
     out_msg.image = in_image;
     image_pub_.publish(out_msg.toImageMsg());
   }
+
+  const int detected_tags = markers.size();
+
+  diagnostic_msgs::msg::DiagnosticStatus diag_status;
+
+  if (detected_tags > 0) {
+    diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    diag_status.message = "AR tags detected. The number of tags: " + std::to_string(detected_tags);
+  } else {
+    diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status.message = "No AR tags detected.";
+  }
+
+  diag_status.name = "localization: " + std::string(this->get_name());
+  diag_status.hardware_id = this->get_name();
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "Number of Detected AR Tags";
+  key_value.value = std::to_string(detected_tags);
+  diag_status.values.push_back(key_value);
+
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = this->now();
+  diag_msg.status.push_back(diag_status);
+
+  diag_pub_->publish(diag_msg);
 }
 
 // wait for one camera info, then shut down that subscriber

planning/behavior_path_planner/CMakeLists.txt

@@ -58,6 +58,12 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/marker_utils/lane_change/debug.cpp
 )
 
+if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+  target_compile_definitions(behavior_path_planner_node PRIVATE
+    DEFINE_LEGACY_FUNCTION
+  )
+endif()
+
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
   ${EIGEN3_INCLUDE_DIR}
 )

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -168,11 +168,12 @@ class StartPlannerModule : public SceneModuleInterface
   lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const;
   std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
   void updatePullOutStatus();
+  void updateStatusAfterBackwardDriving();
   static bool isOverlappedWithLane(
     const lanelet::ConstLanelet & candidate_lanelet,
     const tier4_autoware_utils::LinearRing2d & vehicle_footprint);
   bool hasFinishedPullOut() const;
-  void checkBackFinished();
+  bool isBackwardDrivingComplete() const;
   bool isStopped();
   bool isStuck();
   bool hasFinishedCurrentPath();

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

@@ -676,10 +676,15 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
   const auto target_polygon =
     utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
-  const auto target_backward_polygon = utils::lane_change::createPolygon(
-    target_backward_lanes, 0.0, std::numeric_limits<double>::max());
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
+  std::vector<std::optional<lanelet::BasicPolygon2d>> target_backward_polygons;
+  for (const auto & target_backward_lane : target_backward_lanes) {
+    lanelet::ConstLanelets lanelet{target_backward_lane};
+    auto lane_polygon =
+      utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits<double>::max());
+    target_backward_polygons.push_back(lane_polygon);
+  }
 
   auto filtered_objects = objects;
 
@@ -732,10 +737,16 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
       }
     }
 
+    const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) {
+      return target_backward_polygon &&
+             boost::geometry::intersects(target_backward_polygon.value(), obj_polygon);
+    };
+
     // check if the object intersects with target backward lanes
     if (
-      target_backward_polygon &&
-      boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
+      !target_backward_polygons.empty() &&
+      std::any_of(
+        target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) {
       filtered_obj_indices.target_lane.push_back(i);
       continue;
     }

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -192,7 +192,10 @@ ModuleStatus StartPlannerModule::updateState()
     return ModuleStatus::SUCCESS;
   }
 
-  checkBackFinished();
+  if (isBackwardDrivingComplete()) {
+    updateStatusAfterBackwardDriving();
+    return ModuleStatus::SUCCESS;  // for breaking loop
+  }
 
   return current_state_;
 }
@@ -658,14 +661,28 @@ void StartPlannerModule::updatePullOutStatus()
   planWithPriority(
     start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority);
 
-  checkBackFinished();
-  if (!status_.back_finished) {
+  if (isBackwardDrivingComplete()) {
+    updateStatusAfterBackwardDriving();
+    current_state_ = ModuleStatus::SUCCESS;  // for breaking loop
+  } else {
     status_.backward_path = start_planner_utils::getBackwardPath(
       *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose,
       parameters_->backward_velocity);
   }
 }
 
+void StartPlannerModule::updateStatusAfterBackwardDriving()
+{
+  status_.back_finished = true;
+  // request start_planner approval
+  waitApproval();
+  // To enable approval of the forward path, the RTC status is removed.
+  removeRTCStatus();
+  for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) {
+    itr->second = generateUUID();
+  }
+}
+
 PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const
 {
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
@@ -787,9 +804,9 @@ bool StartPlannerModule::hasFinishedPullOut() const
   return has_finished;
 }
 
-void StartPlannerModule::checkBackFinished()
+bool StartPlannerModule::isBackwardDrivingComplete() const
 {
-  // check ego car is close enough to pull out start pose
+  // check ego car is close enough to pull out start pose and stopped
   const auto current_pose = planner_data_->self_odometry->pose.pose;
   const auto distance =
     tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose);
@@ -798,18 +815,12 @@ void StartPlannerModule::checkBackFinished()
   const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear);
   const bool is_stopped = ego_vel < parameters_->th_stopped_velocity;
 
-  if (!status_.back_finished && is_near && is_stopped) {
+  const bool back_finished = !status_.back_finished && is_near && is_stopped;
+  if (back_finished) {
     RCLCPP_INFO(getLogger(), "back finished");
-    status_.back_finished = true;
-
-    // request start_planner approval
-    waitApproval();
-    removeRTCStatus();
-    for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) {
-      itr->second = generateUUID();
-    }
-    current_state_ = ModuleStatus::SUCCESS;  // for breaking loop
   }
+
+  return back_finished;
 }
 
 bool StartPlannerModule::isStopped()

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -125,15 +125,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
 void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
 {
   const auto rh = planner_data_->route_handler_;
-  if (!opt_use_regulatory_element_) {
-    opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
-    std::ostringstream string_stream;
-    string_stream << "use crosswalk regulatory element: ";
-    string_stream << std::boolalpha << *opt_use_regulatory_element_;
-    RCLCPP_INFO_STREAM(logger_, string_stream.str());
-  }
 
-  const auto launch = [this, &path](const auto id) {
+  const auto launch = [this, &path](const auto id, const auto & use_regulatory_element) {
     if (isModuleRegistered(id)) {
       return;
     }
@@ -143,26 +136,23 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
     const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
 
     registerModule(std::make_shared<CrosswalkModule>(
-      node_, id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
+      node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_));
     generateUUID(id);
     updateRTCStatus(getUUID(id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
   };
 
-  if (*opt_use_regulatory_element_) {
-    const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
-      path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
+  const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
+    path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-    for (const auto & crosswalk : crosswalk_leg_elem_map) {
-      launch(crosswalk.first->id());
-    }
-  } else {
-    const auto crosswalk_lanelets = getCrosswalksOnPath(
-      planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
-      rh->getOverallGraphPtr());
+  for (const auto & crosswalk : crosswalk_leg_elem_map) {
+    launch(crosswalk.first->id(), true);
+  }
 
-    for (const auto & crosswalk : crosswalk_lanelets) {
-      launch(crosswalk.id());
-    }
+  const auto crosswalk_lanelets = getCrosswalksOnPath(
+    planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
+
+  for (const auto & crosswalk : crosswalk_lanelets) {
+    launch(crosswalk.id(), false);
   }
 }
 
@@ -173,17 +163,14 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
 
   std::set<int64_t> crosswalk_id_set;
 
-  if (*opt_use_regulatory_element_) {
-    const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
-      path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
+  crosswalk_id_set = getCrosswalkIdSetOnPath(
+    planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
 
-    for (const auto & crosswalk : crosswalk_leg_elem_map) {
-      crosswalk_id_set.insert(crosswalk.first->id());
-    }
-  } else {
-    crosswalk_id_set = getCrosswalkIdSetOnPath(
-      planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
-      rh->getOverallGraphPtr());
+  const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
+    path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
+
+  for (const auto & crosswalk : crosswalk_leg_elem_map) {
+    crosswalk_id_set.insert(crosswalk.first->id());
   }
 
   return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {

planning/behavior_velocity_crosswalk_module/src/manager.hpp

@@ -51,8 +51,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const PathWithLaneId & path) override;
-
-  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
 };
 
 class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>

planning/behavior_velocity_walkway_module/src/manager.cpp

@@ -45,7 +45,7 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
 {
   const auto rh = planner_data_->route_handler_;
 
-  const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) {
+  const auto launch = [this, &path](const auto & lanelet, const auto & use_regulatory_element) {
     const auto attribute =
       lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string(""));
     if (attribute != lanelet::AttributeValueString::Walkway) {

planning/behavior_velocity_walkway_module/src/manager.hpp

@@ -50,8 +50,6 @@ class WalkwayModuleManager : public SceneModuleManagerInterface
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const PathWithLaneId & path) override;
-
-  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
 };
 
 class WalkwayModulePlugin : public PluginWrapper<WalkwayModuleManager>