|
14 | 14 |
|
15 | 15 | #include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp"
|
16 | 16 |
|
| 17 | +#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" |
17 | 18 | #include "radar_object_tracker/utils/utils.hpp"
|
18 | 19 |
|
19 | 20 | #include <Eigen/Core>
|
|
36 | 37 | #include <vector>
|
37 | 38 | #define EIGEN_MPL2_ONLY
|
38 | 39 |
|
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 |
| - |
191 | 40 | using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
|
192 | 41 |
|
193 | 42 | RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options)
|
@@ -291,7 +140,7 @@ void RadarObjectTrackerNode::onMap(const HADMapBin::ConstSharedPtr msg)
|
291 | 140 | void RadarObjectTrackerNode::onMeasurement(
|
292 | 141 | const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
|
293 | 142 | {
|
294 |
| - const auto self_transform = getTransformAnonymous( |
| 143 | + const auto self_transform = radar_object_tracker_utils::getTransformAnonymous( |
295 | 144 | tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
|
296 | 145 | if (!self_transform) {
|
297 | 146 | return;
|
@@ -387,8 +236,8 @@ std::shared_ptr<Tracker> RadarObjectTrackerNode::createNewTracker(
|
387 | 236 | void RadarObjectTrackerNode::onTimer()
|
388 | 237 | {
|
389 | 238 | 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); |
392 | 241 | if (!self_transform) {
|
393 | 242 | return;
|
394 | 243 | }
|
@@ -434,14 +283,15 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter(
|
434 | 283 | for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) {
|
435 | 284 | autoware_auto_perception_msgs::msg::TrackedObject object;
|
436 | 285 | (*itr)->getTrackedObject(time, object);
|
437 |
| - const auto closest_lanelets = getClosestValidLanelets( |
| 286 | + const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets( |
438 | 287 | object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_);
|
439 | 288 |
|
440 | 289 | // 1. If the object is not close to any lanelet, delete the tracker
|
441 | 290 | const bool no_closest_lanelet = closest_lanelets.empty();
|
442 | 291 | // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker
|
443 | 292 | 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_); |
445 | 295 | if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) {
|
446 | 296 | // std::cout << "object removed due to map based noise filter" << " no close lanelet: " <<
|
447 | 297 | // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet
|
|
0 commit comments