feat(radar_object_tracker): fix lanelet based filter and add test (#7047)

YoshiRi · web-flow · commit 2e2cf7722d99 · 2024-05-17T18:32:33.000+09:00
* feat: change lanelet filtering method to remove unintened objects output

Signed-off-by: yoshiri &lt;yoshiyoshidetteiu@gmail.com&gt;

* refactor: move util-like function to util folder

Signed-off-by: yoshiri &lt;yoshiyoshidetteiu@gmail.com&gt;

* feat: add test

Signed-off-by: yoshiri &lt;yoshiyoshidetteiu@gmail.com&gt;

---------

Signed-off-by: yoshiri &lt;yoshiyoshidetteiu@gmail.com&gt;
diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt
@@ -13,18 +13,23 @@ find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(nlohmann_json REQUIRED) # for debug
 find_package(glog REQUIRED)
+find_package(ament_cmake_gtest REQUIRED)
 
 include_directories(
   SYSTEM
     ${EIGEN3_INCLUDE_DIR}
 )
 
+ament_auto_add_library(radar_object_tracker_utils SHARED
+  src/utils/utils.cpp
+  src/utils/radar_object_tracker_utils.cpp
+)
+
 ament_auto_add_library(radar_object_tracker_node SHARED
   src/radar_object_tracker_node/radar_object_tracker_node.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/linear_motion_tracker.cpp
   src/tracker/model/constant_turn_rate_motion_tracker.cpp
-  src/utils/utils.cpp
   src/data_association/data_association.cpp
   src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
 )
@@ -34,6 +39,7 @@ target_link_libraries(radar_object_tracker_node
   yaml-cpp
   nlohmann_json::nlohmann_json # for debug
   glog::glog
+  radar_object_tracker_utils
 )
 
 rclcpp_components_register_node(radar_object_tracker_node
@@ -42,7 +48,21 @@ rclcpp_components_register_node(radar_object_tracker_node
 )
 
 # testing
-# todo
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_radar_object_tracker_utils
+    test/test_radar_object_tracker_utils.cpp
+    src/utils/radar_object_tracker_utils.cpp
+  )
+  target_include_directories(test_radar_object_tracker_utils PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+  )
+  target_link_libraries(test_radar_object_tracker_utils
+    radar_object_tracker_utils
+  )
+endif()
 
 # Package
 ament_auto_package(
diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp
@@ -0,0 +1,77 @@
+// Copyright 2024 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 RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
+#define RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
+
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+
+#include <geometry_msgs/msg/transform.hpp>
+
+#include <boost/optional.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
+
+#include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
+
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+
+#include <string>
+#include <utility>
+namespace radar_object_tracker_utils
+{
+
+boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
+  const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
+  const std::string & target_frame_id, const rclcpp::Time & time);
+
+bool isDuplicated(
+  const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
+  const lanelet::ConstLanelets & lanelets);
+
+bool checkCloseLaneletCondition(
+  const std::pair<double, lanelet::Lanelet> & lanelet,
+  const autoware_auto_perception_msgs::msg::TrackedObject & object,
+  const double max_distance_from_lane, const double max_angle_diff_from_lane);
+
+lanelet::ConstLanelets getClosestValidLanelets(
+  const autoware_auto_perception_msgs::msg::TrackedObject & object,
+  const lanelet::LaneletMapPtr & lanelet_map_ptr, const double max_distance_from_lane,
+  const double max_angle_diff_from_lane);
+
+bool hasValidVelocityDirectionToLanelet(
+  const autoware_auto_perception_msgs::msg::TrackedObject & object,
+  const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity);
+
+}  // namespace radar_object_tracker_utils
+
+#endif  // RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml
@@ -33,6 +33,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp
@@ -14,6 +14,7 @@
 
 #include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp"
 
+#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp"
 #include "radar_object_tracker/utils/utils.hpp"
 
 #include <Eigen/Core>
@@ -36,158 +37,6 @@
 #include <vector>
 #define EIGEN_MPL2_ONLY
 
-namespace
-{
-boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
-  const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
-  const std::string & target_frame_id, const rclcpp::Time & time)
-{
-  try {
-    // check if the frames are ready
-    std::string errstr;  // This argument prevents error msg from being displayed in the terminal.
-    if (!tf_buffer.canTransform(
-          target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) {
-      return boost::none;
-    }
-
-    geometry_msgs::msg::TransformStamped self_transform_stamped;
-    self_transform_stamped = tf_buffer.lookupTransform(
-      /*target*/ target_frame_id, /*src*/ source_frame_id, time,
-      rclcpp::Duration::from_seconds(0.5));
-    return self_transform_stamped.transform;
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what());
-    return boost::none;
-  }
-}
-
-// check if lanelet is close enough to the target lanelet
-bool isDuplicated(
-  const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
-  const lanelet::ConstLanelets & lanelets)
-{
-  const double CLOSE_LANELET_THRESHOLD = 0.1;
-  for (const auto & lanelet : lanelets) {
-    const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back();
-    const auto lanelet_end_p = lanelet.centerline2d().back();
-    const double dist = std::hypot(
-      target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y());
-    if (dist < CLOSE_LANELET_THRESHOLD) {
-      return true;
-    }
-  }
-
-  return false;
-}
-
-// check if the lanelet is valid for object tracking
-bool checkCloseLaneletCondition(
-  const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object,
-  const double max_distance_from_lane, const double max_angle_diff_from_lane)
-{
-  // Step1. If we only have one point in the centerline, we will ignore the lanelet
-  if (lanelet.second.centerline().size() <= 1) {
-    return false;
-  }
-
-  // Step2. Check if the obstacle is inside or close enough to the lanelet
-  lanelet::BasicPoint2d search_point(
-    object.kinematics.pose_with_covariance.pose.position.x,
-    object.kinematics.pose_with_covariance.pose.position.y);
-  if (!lanelet::geometry::inside(lanelet.second, search_point)) {
-    const auto distance = lanelet.first;
-    if (distance > max_distance_from_lane) {
-      return false;
-    }
-  }
-
-  // Step3. Calculate the angle difference between the lane angle and obstacle angle
-  const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
-  const double lane_yaw = lanelet::utils::getLaneletAngle(
-    lanelet.second, object.kinematics.pose_with_covariance.pose.position);
-  const double delta_yaw = object_yaw - lane_yaw;
-  const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
-  const double abs_norm_delta = std::fabs(normalized_delta_yaw);
-
-  // Step4. Check if the closest lanelet is valid, and add all
-  // of the lanelets that are below max_dist and max_delta_yaw
-  const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x;
-  const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0;
-  if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) {
-    return true;
-  }
-
-  return false;
-}
-
-lanelet::ConstLanelets getClosestValidLanelets(
-  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
-  const double max_distance_from_lane, const double max_angle_diff_from_lane)
-{
-  // obstacle point
-  lanelet::BasicPoint2d search_point(
-    object.kinematics.pose_with_covariance.pose.position.x,
-    object.kinematics.pose_with_covariance.pose.position.y);
-
-  // nearest lanelet
-  std::vector<std::pair<double, lanelet::Lanelet>> surrounding_lanelets =
-    lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10);
-
-  // No Closest Lanelets
-  if (surrounding_lanelets.empty()) {
-    return {};
-  }
-
-  lanelet::ConstLanelets closest_lanelets;
-  for (const auto & lanelet : surrounding_lanelets) {
-    // Check if the close lanelets meet the necessary condition for start lanelets and
-    // Check if similar lanelet is inside the closest lanelet
-    if (
-      !checkCloseLaneletCondition(
-        lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) ||
-      isDuplicated(lanelet, closest_lanelets)) {
-      continue;
-    }
-
-    closest_lanelets.push_back(lanelet.second);
-  }
-
-  return closest_lanelets;
-}
-
-bool hasValidVelocityDirectionToLanelet(
-  const TrackedObject & object, const lanelet::ConstLanelets & lanelets,
-  const double max_lateral_velocity)
-{
-  // get object velocity direction
-  const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
-  const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x;
-  const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y;
-  const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x);  // local frame
-  const double object_vel_yaw_global =
-    tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw);
-  const double object_vel = std::hypot(object_vel_x, object_vel_y);
-
-  for (const auto & lanelet : lanelets) {
-    // get lanelet angle
-    const double lane_yaw = lanelet::utils::getLaneletAngle(
-      lanelet, object.kinematics.pose_with_covariance.pose.position);
-    const double delta_yaw = object_vel_yaw_global - lane_yaw;
-    const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
-
-    // get lateral velocity
-    const double lane_vel = object_vel * std::sin(normalized_delta_yaw);
-    // std::cout << "lane_vel: " << lane_vel <<  "  , delta yaw:" <<  normalized_delta_yaw << "  ,
-    // object_vel " << object_vel <<std::endl;
-    if (std::fabs(lane_vel) < max_lateral_velocity) {
-      return true;
-    }
-  }
-  return false;
-}
-
-}  // namespace
-
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options)
@@ -291,7 +140,7 @@ void RadarObjectTrackerNode::onMap(const HADMapBin::ConstSharedPtr msg)
 void RadarObjectTrackerNode::onMeasurement(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
 {
-  const auto self_transform = getTransformAnonymous(
+  const auto self_transform = radar_object_tracker_utils::getTransformAnonymous(
     tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
   if (!self_transform) {
     return;
@@ -387,8 +236,8 @@ std::shared_ptr<Tracker> RadarObjectTrackerNode::createNewTracker(
 void RadarObjectTrackerNode::onTimer()
 {
   rclcpp::Time current_time = this->now();
-  const auto self_transform =
-    getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time);
+  const auto self_transform = radar_object_tracker_utils::getTransformAnonymous(
+    tf_buffer_, world_frame_id_, "base_link", current_time);
   if (!self_transform) {
     return;
   }
@@ -434,14 +283,15 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter(
   for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) {
     autoware_auto_perception_msgs::msg::TrackedObject object;
     (*itr)->getTrackedObject(time, object);
-    const auto closest_lanelets = getClosestValidLanelets(
+    const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets(
       object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_);
 
     // 1. If the object is not close to any lanelet, delete the tracker
     const bool no_closest_lanelet = closest_lanelets.empty();
     // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker
     const bool is_velocity_direction_close_to_lanelet =
-      hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_);
+      radar_object_tracker_utils::hasValidVelocityDirectionToLanelet(
+        object, closest_lanelets, max_lateral_velocity_);
     if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) {
       // std::cout << "object removed due to map based noise filter" << " no close lanelet: " <<
       // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet
diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp
diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp
