Merge branch 'autowarefoundation:main' into dojo-ros-config-joy_contr…

…oller

.github/CODEOWNERS

@@ -179,6 +179,7 @@ sensing/vehicle_velocity_converter/** [email protected] @autowarefoundation/
 simulator/dummy_perception_publisher/** [email protected] @autowarefoundation/autoware-global-codeowners
 simulator/fault_injection/** [email protected] @autowarefoundation/autoware-global-codeowners
 simulator/simple_planning_simulator/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
+system/autoware_auto_msgs_adapter/** [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 system/bluetooth_monitor/** [email protected] @autowarefoundation/autoware-global-codeowners
 system/component_state_monitor/** [email protected] [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners
 system/default_ad_api/** [email protected] [email protected] [email protected] [email protected] [email protected] @autowarefoundation/autoware-global-codeowners

.pre-commit-config.yaml

@@ -78,6 +78,12 @@ repos:
         args: [--quiet]
         exclude: .cu
 
+  - repo: https://github.com/python-jsonschema/check-jsonschema
+    rev: 0.23.2
+    hooks:
+      - id: check-metaschema
+        files: ^.+/schema/.*schema\.json$
+
   - repo: local
     hooks:
       - id: prettier-svg

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -75,6 +75,24 @@ struct Route
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct NormalRoute
+{
+  using Message = autoware_planning_msgs::msg::LaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/normal_route";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct MrmRoute
+{
+  using Message = autoware_planning_msgs::msg::LaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/mrm_route";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
 struct Trajectory
 {
   using Message = autoware_auto_planning_msgs::msg::Trajectory;

localization/ndt_scan_matcher/src/debug.cpp

@@ -31,6 +31,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers(
   marker.action = visualization_msgs::msg::Marker::ADD;
   marker.scale = scale;
   marker.id = i;
+  marker.lifetime = rclcpp::Duration::from_seconds(10.0);  // 10.0 is the lifetime in seconds.
 
   marker.ns = "initial_pose_transform_probability_color_marker";
   marker.pose = particle.initial_pose;

perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp

@@ -63,6 +63,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   lanelet::ConstLanelets getIntersectedLanelets(
     const LinearRing2d &, const lanelet::ConstLanelets &);
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
+  geometry_msgs::msg::Polygon setFootprint(
+    const autoware_auto_perception_msgs::msg::DetectedObject &);
 };
 
 }  // namespace object_lanelet_filter

perception/detected_object_validation/object-lanelet-filter.md

@@ -2,7 +2,7 @@
 
 ## Purpose
 
-The `object_lanelet_filter` is a node that filters detected object by using vector map.  
+The `object_lanelet_filter` is a node that filters detected object by using vector map.
 The objects only inside of the vector map will be published.
 
 ## Inner-workings / Algorithms
@@ -39,7 +39,7 @@ The objects only inside of the vector map will be published.
 
 ## Assumptions / Known limits
 
-Filtering is performed based on the shape polygon of the object.
+The lanelet filter is performed based on the shape polygon and bounding box of the objects.
 
 ## (Optional) Error detection and handling
 

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -89,7 +89,7 @@ void ObjectLaneletFilterNode::objectCallback(
 
   int index = 0;
   for (const auto & object : transformed_objects.objects) {
-    const auto & footprint = object.shape.footprint;
+    const auto footprint = setFootprint(object);
     const auto & label = object.classification.front().label;
     if (filter_target_.isTarget(label)) {
       Polygon2d polygon;
@@ -110,13 +110,39 @@ void ObjectLaneletFilterNode::objectCallback(
   object_pub_->publish(output_object_msg);
 }
 
+geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
+  const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
+{
+  geometry_msgs::msg::Polygon footprint;
+  if (detected_object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
+    const auto object_size = detected_object.shape.dimensions;
+    const double x_front = object_size.x / 2.0;
+    const double x_rear = -object_size.x / 2.0;
+    const double y_left = object_size.y / 2.0;
+    const double y_right = -object_size.y / 2.0;
+
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_front).y(y_left).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_front).y(y_right).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_rear).y(y_right).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_rear).y(y_left).z(0.0));
+  } else {
+    footprint = detected_object.shape.footprint;
+  }
+  return footprint;
+}
+
 LinearRing2d ObjectLaneletFilterNode::getConvexHull(
   const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects)
 {
   MultiPoint2d candidate_points;
   for (const auto & object : detected_objects.objects) {
     const auto & pos = object.kinematics.pose_with_covariance.pose.position;
-    for (const auto & p : object.shape.footprint.points) {
+    const auto footprint = setFootprint(object);
+    for (const auto & p : footprint.points) {
       candidate_points.emplace_back(p.x + pos.x, p.y + pos.y);
     }
   }

perception/ground_segmentation/CMakeLists.txt

@@ -59,4 +59,44 @@ rclcpp_components_register_node(ground_segmentation
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  test/data
+  config
 )
+
+# Resolve system dependency on yaml-cpp, which apparently does not
+# provide a CMake find_package() module.
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+find_path(YAML_CPP_INCLUDE_DIR
+          NAMES yaml_cpp.h
+          PATHS ${YAML_CPP_INCLUDE_DIRS})
+find_library(YAML_CPP_LIBRARY
+            NAMES YAML_CPP
+            PATHS ${YAML_CPP_LIBRARY_DIRS})
+
+link_directories(${YAML_CPP_LIBRARY_DIRS})
+
+if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+  add_definitions(-DHAVE_NEW_YAMLCPP)
+endif()
+
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+  ament_add_ros_isolated_gtest(test_ray_ground_filter
+    test/test_ray_ground_filter.cpp
+  )
+
+  target_link_libraries(test_ray_ground_filter
+    ground_segmentation
+  )
+
+  ament_add_ros_isolated_gtest(test_scan_ground_filter
+    test/test_scan_ground_filter.cpp
+  )
+
+  target_link_libraries(test_scan_ground_filter
+    ground_segmentation
+    ${YAML_CPP_LIBRARIES}
+  )
+endif()

perception/ground_segmentation/config/ground_segmentation.param.yaml

@@ -0,0 +1,31 @@
+/**:
+  ros__parameters:
+    additional_lidars: []
+    ransac_input_topics: []
+    use_single_frame_filter: False
+    use_time_series_filter: True
+
+    common_crop_box_filter:
+      parameters:
+        min_x: -50.0
+        max_x: 100.0
+        min_y: -50.0
+        max_y: 50.0
+        max_z: 2.5  # recommended 2.5 for non elevation_grid_mode
+        min_z: -2.5 # recommended 0.0 for non elevation_grid_mode
+        negative: False
+
+    common_ground_filter:
+      plugin: "ground_segmentation::ScanGroundFilterComponent"
+      parameters:
+        global_slope_max_angle_deg: 10.0
+        local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
+        split_points_distance_tolerance: 0.2
+        use_virtual_ground_point: True
+        split_height_distance: 0.2
+        non_ground_height_threshold: 0.20
+        grid_size_m: 0.1
+        grid_mode_switch_radius: 20.0
+        gnd_grid_buffer_size: 4
+        detection_range_z_max: 2.5
+        elevation_grid_mode: true

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -38,6 +38,8 @@
 #include <string>
 #include <vector>
 
+class ScanGroundFilterTest;
+
 namespace ground_segmentation
 {
 using vehicle_info_util::VehicleInfo;
@@ -255,6 +257,9 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   explicit ScanGroundFilterComponent(const rclcpp::NodeOptions & options);
+
+  // for test
+  friend ScanGroundFilterTest;
 };
 }  // namespace ground_segmentation
 

perception/ground_segmentation/package.xml

@@ -29,7 +29,9 @@
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
   <depend>vehicle_info_util</depend>
+  <depend>yaml-cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>