Merge pull request #699 from tier4/sync-upstream

chore: sync upstream

.github/workflows/openai-pr-reviewer.yaml

@@ -6,6 +6,10 @@ permissions:
 
 on:
   pull_request:
+    types:
+      - opened
+      - synchronize
+      - labeled
   pull_request_review_comment:
     types: [created]
 
@@ -16,7 +20,13 @@ concurrency:
   cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }}
 
 jobs:
+  prevent-no-label-execution:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
+    with:
+      label: openai-pr-reviewer
   review:
+    needs: prevent-no-label-execution
+    if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
     runs-on: ubuntu-latest
     steps:
       - uses: fluxninja/openai-pr-reviewer@latest

common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp

@@ -0,0 +1,36 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+#define AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/dynamic_object_array.hpp>
+
+namespace autoware_ad_api::perception
+{
+
+struct DynamicObjectArray
+{
+  using Message = autoware_adapi_v1_msgs::msg::DynamicObjectArray;
+  static constexpr char name[] = "/api/perception/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::perception
+
+#endif  // AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_

common/component_interface_specs/include/component_interface_specs/perception.hpp

@@ -0,0 +1,36 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+#define COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+
+namespace perception_interface
+{
+
+struct ObjectRecognition
+{
+  using Message = autoware_auto_perception_msgs::msg::PredictedObjects;
+  static constexpr char name[] = "/perception/object_recognition/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace perception_interface
+
+#endif  // COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -14,7 +14,7 @@
   <arg name="pose_initializer_param_path"/>
   <arg name="eagleye_param_path"/>
 
-  <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud" description="The topic will be used in the localization util module"/>
+  <arg name="input_pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
   <arg name="use_pointcloud_container" default="true" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
 

launch/tier4_localization_launch/launch/util/util.launch.py

@@ -34,7 +34,7 @@ def load_composable_node_param(param_path):
         plugin="pointcloud_preprocessor::CropBoxFilterComponent",
         name="crop_box_filter_measurement_range",
         remappings=[
-            ("input", LaunchConfiguration("input/pointcloud")),
+            ("input", LaunchConfiguration("input_pointcloud")),
             ("output", "measurement_range/pointcloud"),
         ],
         parameters=[

launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py

@@ -65,7 +65,7 @@ def create_normal_pipeline(self):
                     ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
                     ("map_loader_service", "/map/get_differential_pointcloud_map"),
-                    ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+                    ("kinematic_state", "/localization/kinematic_state"),
                 ],
                 parameters=[
                     {
@@ -122,7 +122,7 @@ def create_down_sample_pipeline(self):
                     ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
                     ("map_loader_service", "/map/get_differential_pointcloud_map"),
-                    ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+                    ("kinematic_state", "/localization/kinematic_state"),
                 ],
                 parameters=[
                     {

perception/compare_map_segmentation/CMakeLists.txt

@@ -46,8 +46,7 @@ ament_target_dependencies(compare_map_segmentation
   sensor_msgs
   tier4_autoware_utils
   autoware_map_msgs
-  component_interface_specs
-  component_interface_utils
+  nav_msgs
 )
 
 if(OPENMP_FOUND)

perception/compare_map_segmentation/README.md

@@ -61,12 +61,11 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_
 
 #### Input
 
-| Name                                 | Type                                            | Description                                                    |
-| ------------------------------------ | ----------------------------------------------- | -------------------------------------------------------------- |
-| `~/input/points`                     | `sensor_msgs::msg::PointCloud2`                 | reference points                                               |
-| `~/input/map`                        | `sensor_msgs::msg::PointCloud2`                 | map (in case static map loading)                               |
-| `~/pose_with_covariance`             | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading)         |
-| `/localization/initialization_state` | `localization_interface::InitializationState`   | Ego-vehicle pose initialization state (in dynamic map loading) |
+| Name                            | Type                            | Description                                            |
+| ------------------------------- | ------------------------------- | ------------------------------------------------------ |
+| `~/input/points`                | `sensor_msgs::msg::PointCloud2` | reference points                                       |
+| `~/input/map`                   | `sensor_msgs::msg::PointCloud2` | map (in case static map loading)                       |
+| `/localization/kinematic_state` | `nav_msgs::msg::Odometry`       | current ego-vehicle pose (in case dynamic map loading) |
 
 #### Output
 

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp

@@ -15,12 +15,11 @@
 #ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_
 #define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_
 
-#include <component_interface_specs/localization.hpp>
-#include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_map_msgs/srv/get_differential_point_cloud_map.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <pcl/filters/voxel_grid.h>
@@ -154,16 +153,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   };
 
   typedef typename std::map<std::string, struct MapGridVoxelInfo> VoxelGridDict;
-  using InitializationState = localization_interface::InitializationState;
 
   /** \brief Map to hold loaded map grid id and it's voxel filter */
   VoxelGridDict current_voxel_grid_dict_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_kinematic_state_;
 
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-    sub_estimated_pose_;
-  component_interface_utils::Subscription<InitializationState>::SharedPtr
-    sub_pose_initializer_state_;
-  InitializationState::Message initialization_state_;
   std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
   std::optional<geometry_msgs::msg::Point> last_updated_position_ = std::nullopt;
   rclcpp::TimerBase::SharedPtr map_update_timer_;
@@ -200,8 +194,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
     std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group);
-  void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose);
-  void onPoseInitializerCallback(const InitializationState::Message::ConstSharedPtr msg);
+  void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose);
 
   void timer_callback();
   bool should_update_map() const;

perception/compare_map_segmentation/package.xml

@@ -18,10 +18,9 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_map_msgs</depend>
-  <depend>component_interface_specs</depend>
-  <depend>component_interface_utils</depend>
   <depend>grid_map_pcl</depend>
   <depend>grid_map_ros</depend>
+  <depend>nav_msgs</depend>
   <depend>pcl_conversions</depend>
   <depend>pointcloud_preprocessor</depend>
   <depend>rclcpp</depend>

perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp

@@ -301,13 +301,8 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
   map_loader_radius_ = node->declare_parameter<double>("map_loader_radius");
   auto main_sub_opt = rclcpp::SubscriptionOptions();
   main_sub_opt.callback_group = main_callback_group;
-
-  const auto localization_node = component_interface_utils::NodeAdaptor(node);
-  localization_node.init_sub(
-    sub_pose_initializer_state_, this, &VoxelGridDynamicMapLoader::onPoseInitializerCallback);
-
-  sub_estimated_pose_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "pose_with_covariance", rclcpp::QoS{1},
+  sub_kinematic_state_ = node->create_subscription<nav_msgs::msg::Odometry>(
+    "kinematic_state", rclcpp::QoS{1},
     std::bind(&VoxelGridDynamicMapLoader::onEstimatedPoseCallback, this, std::placeholders::_1),
     main_sub_opt);
   RCLCPP_INFO(logger_, "VoxelGridDynamicMapLoader initialized.\n");
@@ -327,18 +322,7 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
     node, node->get_clock(), period_ns, std::bind(&VoxelGridDynamicMapLoader::timer_callback, this),
     timer_callback_group_);
 }
-void VoxelGridDynamicMapLoader::onPoseInitializerCallback(
-  const InitializationState::Message::ConstSharedPtr msg)
-{
-  initialization_state_.state = msg->state;
-  if (msg->state != InitializationState::Message::INITIALIZED) {
-    current_position_ = std::nullopt;
-    last_updated_position_ = std::nullopt;
-    RCLCPP_INFO(logger_, "Initializing pose... Reset the position of Vehicle");
-  }
-}
-void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(
-  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg)
+void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg)
 {
   current_position_ = msg->pose.pose.position;
 }
@@ -417,9 +401,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
 }
 void VoxelGridDynamicMapLoader::timer_callback()
 {
-  if (
-    current_position_ == std::nullopt ||
-    initialization_state_.state != InitializationState::Message::INITIALIZED) {
+  if (current_position_ == std::nullopt) {
     return;
   }
   if (last_updated_position_ == std::nullopt) {

perception/euclidean_cluster/launch/euclidean_cluster.launch.py

@@ -57,7 +57,7 @@ def load_composable_node_param(param_path):
             ("map", LaunchConfiguration("input_map")),
             ("output", "compare_map_filtered/pointcloud"),
             ("map_loader_service", "/map/get_differential_pointcloud_map"),
-            ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+            ("kinematic_state", "/localization/kinematic_state"),
         ],
         parameters=[
             {

perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py

@@ -47,7 +47,7 @@ def load_composable_node_param(param_path):
             ("map", LaunchConfiguration("input_map")),
             ("output", "map_filter/pointcloud"),
             ("map_loader_service", "/map/get_differential_pointcloud_map"),
-            ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"),
+            ("kinematic_state", "/localization/kinematic_state"),
         ],
         parameters=[load_composable_node_param("compare_map_param_path")],
     )

perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -191,38 +191,39 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         max_iou = iou + iou_x + iou_y;
       }
     }
-    bool is_roi_label_known = feature_obj.object.classification.front().label !=
-                              autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
-    bool is_roi_existence_prob_higher =
-      output_cluster_msg.feature_objects.at(index).object.existence_probability <=
-      feature_obj.object.existence_probability;
-    if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) {
-      output_cluster_msg.feature_objects.at(index).object.classification =
-        feature_obj.object.classification;
-
-      // Update existence_probability for fused objects
-      if (
-        output_cluster_msg.feature_objects.at(index).object.existence_probability <
-        min_roi_existence_prob_) {
-        output_cluster_msg.feature_objects.at(index).object.existence_probability =
-          min_roi_existence_prob_;
+    if (!output_cluster_msg.feature_objects.empty()) {
+      bool is_roi_label_known = feature_obj.object.classification.front().label !=
+                                autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+      bool is_roi_existence_prob_higher =
+        output_cluster_msg.feature_objects.at(index).object.existence_probability <=
+        feature_obj.object.existence_probability;
+      if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) {
+        output_cluster_msg.feature_objects.at(index).object.classification =
+          feature_obj.object.classification;
+
+        // Update existence_probability for fused objects
+        if (
+          output_cluster_msg.feature_objects.at(index).object.existence_probability <
+          min_roi_existence_prob_) {
+          output_cluster_msg.feature_objects.at(index).object.existence_probability =
+            min_roi_existence_prob_;
+        }
       }
-    }
 
-    // fuse with unknown roi
-
-    if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) {
-      output_cluster_msg.feature_objects.at(index).object.classification =
-        feature_obj.object.classification;
-      // Update existence_probability for fused objects
-      if (
-        output_cluster_msg.feature_objects.at(index).object.existence_probability <
-        min_roi_existence_prob_) {
-        output_cluster_msg.feature_objects.at(index).object.existence_probability =
-          min_roi_existence_prob_;
+      // fuse with unknown roi
+
+      if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) {
+        output_cluster_msg.feature_objects.at(index).object.classification =
+          feature_obj.object.classification;
+        // Update existence_probability for fused objects
+        if (
+          output_cluster_msg.feature_objects.at(index).object.existence_probability <
+          min_roi_existence_prob_) {
+          output_cluster_msg.feature_objects.at(index).object.existence_probability =
+            min_roi_existence_prob_;
+        }
       }
     }
-
     debug_image_rois.push_back(feature_obj.feature.roi);
   }
 

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -227,7 +227,7 @@ lanelet::ConstLanelets getRightLineSharingLanelets(
   for (auto & candidate : right_lane_candidates) {
     // exclude self lanelet
     if (candidate == current_lanelet) continue;
-    // if candidate has linestring as leftbound, assign it to output
+    // if candidate has linestring as left bound, assign it to output
     if (candidate.leftBound() == current_lanelet.rightBound()) {
       output_lanelets.push_back(candidate);
     }
@@ -254,7 +254,7 @@ lanelet::ConstLanelets getLeftLineSharingLanelets(
   for (auto & candidate : left_lane_candidates) {
     // exclude self lanelet
     if (candidate == current_lanelet) continue;
-    // if candidate has linestring as rightbound, assign it to output
+    // if candidate has linestring as right bound, assign it to output
     if (candidate.rightBound() == current_lanelet.leftBound()) {
       output_lanelets.push_back(candidate);
     }
@@ -287,7 +287,7 @@ lanelet::routing::LaneletPaths getPossiblePathsForIsolatedLanelet(
   lanelet::ConstLanelets possible_lanelets;
   possible_lanelets.push_back(lanelet);
   lanelet::routing::LaneletPaths possible_paths;
-  // need to init path with constlanelets
+  // need to initialize path with constant lanelets
   lanelet::routing::LaneletPath possible_path(possible_lanelets);
   possible_paths.push_back(possible_path);
   return possible_paths;