Skip to content

Commit

Permalink
feat(radar_object_tracker): fix lanelet based filter and add test (au…
Browse files Browse the repository at this point in the history 
…towarefoundation#7047)

* feat: change lanelet filtering method to remove unintened objects output

Signed-off-by: yoshiri <[email protected]>

* refactor: move util-like function to util folder

Signed-off-by: yoshiri <[email protected]>

* feat: add test

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi @saka1-s
YoshiRi authored and saka1-s committed May 23, 2024
1 parent ae0997b commit 142804f
Show file tree
Hide file tree
Showing 6 changed files with 382 additions and 161 deletions.
26 changes: 22 additions & 4 deletions perception/radar_object_tracker/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,17 @@ endif()
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}
)

# Targets
ament_auto_add_library(mu_successive_shortest_path SHARED
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
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
Expand All @@ -36,6 +38,8 @@ target_link_libraries(radar_object_tracker_node
Eigen3::Eigen
yaml-cpp
nlohmann_json::nlohmann_json # for debug
glog::glog
radar_object_tracker_utils
)

rclcpp_components_register_node(radar_object_tracker_node
Expand All @@ -44,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(
Expand Down
77 changes: 77 additions & 0 deletions ...on/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp
View file
Original file line number Diff line number Diff line change
@@ -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_
1 change: 1 addition & 0 deletions perception/radar_object_tracker/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,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>
Expand Down
164 changes: 7 additions & 157 deletions perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -35,158 +36,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)
Expand Down Expand Up @@ -283,7 +132,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;
Expand Down Expand Up @@ -375,8 +224,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;
}
Expand Down Expand Up @@ -422,14 +271,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
Expand Down
Loading

0 comments on commit 142804f

Please sign in to comment.