Merge branch 'main' into fix/simple_planning_sim_manual

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -195,6 +195,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -220,6 +220,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -73,6 +73,7 @@
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 
@@ -104,6 +105,7 @@
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 
@@ -133,6 +135,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -61,6 +61,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -18,6 +18,7 @@
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="object_recognition_detection_obstacle_pointcloud_based_validator_param_path"/>
+  <arg name="object_recognition_detection_detection_by_tracker_param"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
@@ -174,6 +175,7 @@
           <arg name="outlier_param_path" value="$(var object_recognition_detection_outlier_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="radar_lanelet_filtering_range_param" value="$(var object_recognition_detection_radar_lanelet_filtering_range_param)"/>
+          <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>

localization/landmark_based_localizer/README.md

@@ -23,11 +23,11 @@ This calculated ego pose is passed to the EKF, where it is fused with the twist
 
 ![node diagram](./doc_image/node_diagram.drawio.svg)
 
-### `landmark_parser`
+### `landmark_manager`
 
 The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`.
 
-The `landmark_parser` is a utility package to load landmarks from the map.
+The `landmark_manager` is a utility package to load landmarks from the map.
 
 - Translation : The center of the four vertices of the landmark
 - Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3.
@@ -43,7 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc
 
 ## Map specifications
 
-For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_parser` can interpret.
+For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret.
 
 The four vertices of a landmark are defined counterclockwise.
 

localization/landmark_based_localizer/ar_tag_based_localizer/package.xml

@@ -17,7 +17,7 @@
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
   <depend>image_transport</depend>
-  <depend>landmark_parser</depend>
+  <depend>landmark_manager</depend>
   <depend>lanelet2_extension</depend>
   <depend>localization_util</depend>
   <depend>rclcpp</depend>

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp

@@ -148,8 +148,13 @@ bool ArTagBasedLocalizer::setup()
 
 void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
 {
-  landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger());
-  const MarkerArray marker_msg = convert_to_marker_array_msg(landmark_map_);
+  const std::vector<landmark_manager::Landmark> landmarks =
+    landmark_manager::parse_landmarks(msg, "apriltag_16h5", this->get_logger());
+  for (const landmark_manager::Landmark & landmark : landmarks) {
+    landmark_map_[landmark.id] = landmark.pose;
+  }
+
+  const MarkerArray marker_msg = landmark_manager::convert_landmarks_to_marker_array_msg(landmarks);
   marker_pub_->publish(marker_msg);
 }
 

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp

@@ -45,7 +45,7 @@
 #ifndef AR_TAG_BASED_LOCALIZER_HPP_
 #define AR_TAG_BASED_LOCALIZER_HPP_
 
-#include "landmark_parser/landmark_parser_core.hpp"
+#include "landmark_manager/landmark_manager.hpp"
 
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>

localization/landmark_based_localizer/landmark_parser/CMakeLists.txt → localization/landmark_based_localizer/landmark_manager/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(landmark_parser)
+project(landmark_manager)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -12,8 +12,8 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
-ament_auto_add_library(landmark_parser
-  src/landmark_parser_core.cpp
+ament_auto_add_library(landmark_manager
+  src/landmark_manager.cpp
 )
 
 ament_auto_package(

localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp → localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp

@@ -12,23 +12,35 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_
-#define LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_
+#ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
+#define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include <map>
 #include <string>
+#include <vector>
 
-std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
+namespace landmark_manager
+{
+
+struct Landmark
+{
+  std::string id;
+  geometry_msgs::msg::Pose pose;
+};
+
+std::vector<Landmark> parse_landmarks(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
   const std::string & target_subtype, const rclcpp::Logger & logger);
 
-visualization_msgs::msg::MarkerArray convert_to_marker_array_msg(
-  const std::map<std::string, geometry_msgs::msg::Pose> & landmarks);
+visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg(
+  const std::vector<Landmark> & landmarks);
+
+}  // namespace landmark_manager
 
-#endif  // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_
+#endif  // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_

localization/landmark_based_localizer/landmark_parser/package.xml → localization/landmark_based_localizer/landmark_manager/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>landmark_parser</name>
+  <name>landmark_manager</name>
   <version>0.0.0</version>
-  <description>The landmark_parser package</description>
+  <description>The landmark_manager package</description>
   <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
   <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
   <maintainer email="[email protected]">Yamato Ando</maintainer>
@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>rclcpp</depend>
+  <depend>tf2_eigen</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp → localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp

@@ -12,16 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "landmark_parser/landmark_parser_core.hpp"
+#include "landmark_manager/landmark_manager.hpp"
 
 #include "lanelet2_extension/utility/message_conversion.hpp"
 
 #include <Eigen/Core>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/primitives/Polygon.h>
 
-std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
+namespace landmark_manager
+{
+
+std::vector<Landmark> parse_landmarks(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
   const std::string & target_subtype, const rclcpp::Logger & logger)
 {
@@ -32,7 +36,7 @@ std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
   lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared<lanelet::LaneletMap>()};
   lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr);
 
-  std::map<std::string, geometry_msgs::msg::Pose> landmark_map;
+  std::vector<Landmark> landmarks;
 
   for (const auto & poly : lanelet_map_ptr->polygonLayer) {
     const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")};
@@ -93,8 +97,8 @@ std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
     pose.orientation.z = q.z();
     pose.orientation.w = q.w();
 
-    // Add to map
-    landmark_map[marker_id] = pose;
+    // Add
+    landmarks.push_back(Landmark{marker_id, pose});
     RCLCPP_INFO_STREAM(logger, "id: " << marker_id);
     RCLCPP_INFO_STREAM(
       logger,
@@ -104,11 +108,11 @@ std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
                      << pose.orientation.z << ", " << pose.orientation.w);
   }
 
-  return landmark_map;
+  return landmarks;
 }
 
-visualization_msgs::msg::MarkerArray convert_to_marker_array_msg(
-  const std::map<std::string, geometry_msgs::msg::Pose> & landmarks)
+visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg(
+  const std::vector<Landmark> & landmarks)
 {
   int32_t id = 0;
   visualization_msgs::msg::MarkerArray marker_array;
@@ -152,3 +156,5 @@ visualization_msgs::msg::MarkerArray convert_to_marker_array_msg(
   }
   return marker_array;
 }
+
+}  // namespace landmark_manager

perception/detection_by_tracker/CMakeLists.txt

@@ -27,6 +27,7 @@ include_directories(
 # Generate exe file
 set(DETECTION_BY_TRACKER_SRC
   src/detection_by_tracker_core.cpp
+  src/utils.cpp
 )
 
 ament_auto_add_library(detection_by_tracker_node SHARED
@@ -45,4 +46,5 @@ rclcpp_components_register_node(detection_by_tracker_node
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

perception/detection_by_tracker/config/detection_by_tracker.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    tracker_ignore_label:
+      UNKNOWN : true
+      CAR : false
+      TRUCK : false
+      BUS : false
+      TRAILER : false
+      MOTORCYCLE : false
+      BICYCLE : false
+      PEDESTRIAN : false

perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp

@@ -39,14 +39,15 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include "utils/utils.hpp"
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <deque>
 #include <map>
 #include <memory>
 #include <vector>
-
 class TrackerHandler
 {
 private:
@@ -81,7 +82,7 @@ class DetectionByTracker : public rclcpp::Node
   std::map<uint8_t, int> max_search_distance_for_merger_;
   std::map<uint8_t, int> max_search_distance_for_divider_;
 
-  bool ignore_unknown_tracker_;
+  utils::TrackerIgnoreLabel tracker_ignore_;
 
   void setMaxSearchRange();
 

perception/detection_by_tracker/include/utils/utils.hpp

@@ -0,0 +1,36 @@
+// Copyright 2023 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 UTILS__UTILS_HPP_
+#define UTILS__UTILS_HPP_
+
+#include <cstdint>
+
+namespace utils
+{
+struct TrackerIgnoreLabel
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+  bool isIgnore(const uint8_t label) const;
+};  // struct TrackerIgnoreLabel
+}  // namespace utils
+
+#endif  // UTILS__UTILS_HPP_

perception/detection_by_tracker/launch/detection_by_tracker.launch.xml

@@ -3,10 +3,11 @@
   <arg name="input/tracked_objects" default="/perception/object_recognition/tracking/objects"/>
   <arg name="input/initial_objects" default="/perception/object_recognition/detection/clustering/objects_with_feature"/>
   <arg name="output" default="objects"/>
-
+  <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   <node pkg="detection_by_tracker" exec="detection_by_tracker" name="detection_by_tracker_node" output="screen">
     <remap from="~/input/tracked_objects" to="$(var input/tracked_objects)"/>
     <remap from="~/input/initial_objects" to="$(var input/initial_objects)"/>
     <remap from="~/output" to="$(var output)"/>
+    <param from="$(var detection_by_tracker_param_path)"/>
   </node>
 </launch>