Skip to content

Commit 2e2cf77

Browse files
authored
feat(radar_object_tracker): fix lanelet based filter and add test (#7047)
* feat: change lanelet filtering method to remove unintened objects output Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * refactor: move util-like function to util folder Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add test Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent a88496e commit 2e2cf77

File tree

6 files changed

+382
-159
lines changed

6 files changed

+382
-159
lines changed

perception/radar_object_tracker/CMakeLists.txt

+22-2
Original file line numberDiff line numberDiff line change
@@ -13,18 +13,23 @@ find_package(eigen3_cmake_module REQUIRED)
1313
find_package(Eigen3 REQUIRED)
1414
find_package(nlohmann_json REQUIRED) # for debug
1515
find_package(glog REQUIRED)
16+
find_package(ament_cmake_gtest REQUIRED)
1617

1718
include_directories(
1819
SYSTEM
1920
${EIGEN3_INCLUDE_DIR}
2021
)
2122

23+
ament_auto_add_library(radar_object_tracker_utils SHARED
24+
src/utils/utils.cpp
25+
src/utils/radar_object_tracker_utils.cpp
26+
)
27+
2228
ament_auto_add_library(radar_object_tracker_node SHARED
2329
src/radar_object_tracker_node/radar_object_tracker_node.cpp
2430
src/tracker/model/tracker_base.cpp
2531
src/tracker/model/linear_motion_tracker.cpp
2632
src/tracker/model/constant_turn_rate_motion_tracker.cpp
27-
src/utils/utils.cpp
2833
src/data_association/data_association.cpp
2934
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
3035
)
@@ -34,6 +39,7 @@ target_link_libraries(radar_object_tracker_node
3439
yaml-cpp
3540
nlohmann_json::nlohmann_json # for debug
3641
glog::glog
42+
radar_object_tracker_utils
3743
)
3844

3945
rclcpp_components_register_node(radar_object_tracker_node
@@ -42,7 +48,21 @@ rclcpp_components_register_node(radar_object_tracker_node
4248
)
4349

4450
# testing
45-
# todo
51+
if(BUILD_TESTING)
52+
find_package(ament_lint_auto REQUIRED)
53+
ament_lint_auto_find_test_dependencies()
54+
55+
ament_add_gtest(test_radar_object_tracker_utils
56+
test/test_radar_object_tracker_utils.cpp
57+
src/utils/radar_object_tracker_utils.cpp
58+
)
59+
target_include_directories(test_radar_object_tracker_utils PRIVATE
60+
${CMAKE_CURRENT_SOURCE_DIR}/include
61+
)
62+
target_link_libraries(test_radar_object_tracker_utils
63+
radar_object_tracker_utils
64+
)
65+
endif()
4666

4767
# Package
4868
ament_auto_package(
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
16+
#define RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_
17+
18+
#include <lanelet2_extension/utility/query.hpp>
19+
#include <lanelet2_extension/utility/utilities.hpp>
20+
21+
#include <geometry_msgs/msg/transform.hpp>
22+
23+
#include <boost/optional.hpp>
24+
25+
#include <lanelet2_core/LaneletMap.h>
26+
#include <lanelet2_core/geometry/BoundingBox.h>
27+
#include <lanelet2_core/geometry/Lanelet.h>
28+
#include <lanelet2_core/geometry/Point.h>
29+
#include <lanelet2_core/primitives/Lanelet.h>
30+
31+
#ifdef ROS_DISTRO_GALACTIC
32+
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
33+
#else
34+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
35+
#endif
36+
37+
#include <rclcpp/rclcpp.hpp>
38+
#include <tier4_autoware_utils/geometry/geometry.hpp>
39+
#include <tier4_autoware_utils/math/unit_conversion.hpp>
40+
41+
#include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
42+
43+
#include <tf2/LinearMath/Transform.h>
44+
#include <tf2/convert.h>
45+
#include <tf2/transform_datatypes.h>
46+
#include <tf2_ros/buffer.h>
47+
48+
#include <string>
49+
#include <utility>
50+
namespace radar_object_tracker_utils
51+
{
52+
53+
boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
54+
const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
55+
const std::string & target_frame_id, const rclcpp::Time & time);
56+
57+
bool isDuplicated(
58+
const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
59+
const lanelet::ConstLanelets & lanelets);
60+
61+
bool checkCloseLaneletCondition(
62+
const std::pair<double, lanelet::Lanelet> & lanelet,
63+
const autoware_auto_perception_msgs::msg::TrackedObject & object,
64+
const double max_distance_from_lane, const double max_angle_diff_from_lane);
65+
66+
lanelet::ConstLanelets getClosestValidLanelets(
67+
const autoware_auto_perception_msgs::msg::TrackedObject & object,
68+
const lanelet::LaneletMapPtr & lanelet_map_ptr, const double max_distance_from_lane,
69+
const double max_angle_diff_from_lane);
70+
71+
bool hasValidVelocityDirectionToLanelet(
72+
const autoware_auto_perception_msgs::msg::TrackedObject & object,
73+
const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity);
74+
75+
} // namespace radar_object_tracker_utils
76+
77+
#endif // RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_

perception/radar_object_tracker/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333

3434
<test_depend>ament_lint_auto</test_depend>
3535
<test_depend>autoware_lint_common</test_depend>
36+
<test_depend>gtest</test_depend>
3637

3738
<export>
3839
<build_type>ament_cmake</build_type>

perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp

+7-157
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
#include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp"
1616

17+
#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp"
1718
#include "radar_object_tracker/utils/utils.hpp"
1819

1920
#include <Eigen/Core>
@@ -36,158 +37,6 @@
3637
#include <vector>
3738
#define EIGEN_MPL2_ONLY
3839

39-
namespace
40-
{
41-
boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
42-
const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
43-
const std::string & target_frame_id, const rclcpp::Time & time)
44-
{
45-
try {
46-
// check if the frames are ready
47-
std::string errstr; // This argument prevents error msg from being displayed in the terminal.
48-
if (!tf_buffer.canTransform(
49-
target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) {
50-
return boost::none;
51-
}
52-
53-
geometry_msgs::msg::TransformStamped self_transform_stamped;
54-
self_transform_stamped = tf_buffer.lookupTransform(
55-
/*target*/ target_frame_id, /*src*/ source_frame_id, time,
56-
rclcpp::Duration::from_seconds(0.5));
57-
return self_transform_stamped.transform;
58-
} catch (tf2::TransformException & ex) {
59-
RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what());
60-
return boost::none;
61-
}
62-
}
63-
64-
// check if lanelet is close enough to the target lanelet
65-
bool isDuplicated(
66-
const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
67-
const lanelet::ConstLanelets & lanelets)
68-
{
69-
const double CLOSE_LANELET_THRESHOLD = 0.1;
70-
for (const auto & lanelet : lanelets) {
71-
const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back();
72-
const auto lanelet_end_p = lanelet.centerline2d().back();
73-
const double dist = std::hypot(
74-
target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y());
75-
if (dist < CLOSE_LANELET_THRESHOLD) {
76-
return true;
77-
}
78-
}
79-
80-
return false;
81-
}
82-
83-
// check if the lanelet is valid for object tracking
84-
bool checkCloseLaneletCondition(
85-
const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object,
86-
const double max_distance_from_lane, const double max_angle_diff_from_lane)
87-
{
88-
// Step1. If we only have one point in the centerline, we will ignore the lanelet
89-
if (lanelet.second.centerline().size() <= 1) {
90-
return false;
91-
}
92-
93-
// Step2. Check if the obstacle is inside or close enough to the lanelet
94-
lanelet::BasicPoint2d search_point(
95-
object.kinematics.pose_with_covariance.pose.position.x,
96-
object.kinematics.pose_with_covariance.pose.position.y);
97-
if (!lanelet::geometry::inside(lanelet.second, search_point)) {
98-
const auto distance = lanelet.first;
99-
if (distance > max_distance_from_lane) {
100-
return false;
101-
}
102-
}
103-
104-
// Step3. Calculate the angle difference between the lane angle and obstacle angle
105-
const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
106-
const double lane_yaw = lanelet::utils::getLaneletAngle(
107-
lanelet.second, object.kinematics.pose_with_covariance.pose.position);
108-
const double delta_yaw = object_yaw - lane_yaw;
109-
const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
110-
const double abs_norm_delta = std::fabs(normalized_delta_yaw);
111-
112-
// Step4. Check if the closest lanelet is valid, and add all
113-
// of the lanelets that are below max_dist and max_delta_yaw
114-
const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x;
115-
const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0;
116-
if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) {
117-
return true;
118-
}
119-
120-
return false;
121-
}
122-
123-
lanelet::ConstLanelets getClosestValidLanelets(
124-
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
125-
const double max_distance_from_lane, const double max_angle_diff_from_lane)
126-
{
127-
// obstacle point
128-
lanelet::BasicPoint2d search_point(
129-
object.kinematics.pose_with_covariance.pose.position.x,
130-
object.kinematics.pose_with_covariance.pose.position.y);
131-
132-
// nearest lanelet
133-
std::vector<std::pair<double, lanelet::Lanelet>> surrounding_lanelets =
134-
lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10);
135-
136-
// No Closest Lanelets
137-
if (surrounding_lanelets.empty()) {
138-
return {};
139-
}
140-
141-
lanelet::ConstLanelets closest_lanelets;
142-
for (const auto & lanelet : surrounding_lanelets) {
143-
// Check if the close lanelets meet the necessary condition for start lanelets and
144-
// Check if similar lanelet is inside the closest lanelet
145-
if (
146-
!checkCloseLaneletCondition(
147-
lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) ||
148-
isDuplicated(lanelet, closest_lanelets)) {
149-
continue;
150-
}
151-
152-
closest_lanelets.push_back(lanelet.second);
153-
}
154-
155-
return closest_lanelets;
156-
}
157-
158-
bool hasValidVelocityDirectionToLanelet(
159-
const TrackedObject & object, const lanelet::ConstLanelets & lanelets,
160-
const double max_lateral_velocity)
161-
{
162-
// get object velocity direction
163-
const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
164-
const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x;
165-
const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y;
166-
const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); // local frame
167-
const double object_vel_yaw_global =
168-
tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw);
169-
const double object_vel = std::hypot(object_vel_x, object_vel_y);
170-
171-
for (const auto & lanelet : lanelets) {
172-
// get lanelet angle
173-
const double lane_yaw = lanelet::utils::getLaneletAngle(
174-
lanelet, object.kinematics.pose_with_covariance.pose.position);
175-
const double delta_yaw = object_vel_yaw_global - lane_yaw;
176-
const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
177-
178-
// get lateral velocity
179-
const double lane_vel = object_vel * std::sin(normalized_delta_yaw);
180-
// std::cout << "lane_vel: " << lane_vel << " , delta yaw:" << normalized_delta_yaw << " ,
181-
// object_vel " << object_vel <<std::endl;
182-
if (std::fabs(lane_vel) < max_lateral_velocity) {
183-
return true;
184-
}
185-
}
186-
return false;
187-
}
188-
189-
} // namespace
190-
19140
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
19241

19342
RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options)
@@ -291,7 +140,7 @@ void RadarObjectTrackerNode::onMap(const HADMapBin::ConstSharedPtr msg)
291140
void RadarObjectTrackerNode::onMeasurement(
292141
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
293142
{
294-
const auto self_transform = getTransformAnonymous(
143+
const auto self_transform = radar_object_tracker_utils::getTransformAnonymous(
295144
tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
296145
if (!self_transform) {
297146
return;
@@ -387,8 +236,8 @@ std::shared_ptr<Tracker> RadarObjectTrackerNode::createNewTracker(
387236
void RadarObjectTrackerNode::onTimer()
388237
{
389238
rclcpp::Time current_time = this->now();
390-
const auto self_transform =
391-
getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time);
239+
const auto self_transform = radar_object_tracker_utils::getTransformAnonymous(
240+
tf_buffer_, world_frame_id_, "base_link", current_time);
392241
if (!self_transform) {
393242
return;
394243
}
@@ -434,14 +283,15 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter(
434283
for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) {
435284
autoware_auto_perception_msgs::msg::TrackedObject object;
436285
(*itr)->getTrackedObject(time, object);
437-
const auto closest_lanelets = getClosestValidLanelets(
286+
const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets(
438287
object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_);
439288

440289
// 1. If the object is not close to any lanelet, delete the tracker
441290
const bool no_closest_lanelet = closest_lanelets.empty();
442291
// 2. If the object velocity direction is not close to the lanelet direction, delete the tracker
443292
const bool is_velocity_direction_close_to_lanelet =
444-
hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_);
293+
radar_object_tracker_utils::hasValidVelocityDirectionToLanelet(
294+
object, closest_lanelets, max_lateral_velocity_);
445295
if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) {
446296
// std::cout << "object removed due to map based noise filter" << " no close lanelet: " <<
447297
// no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet

0 commit comments

Comments
 (0)