From 3f0c70d50f08ceb1f44d17acbf25eebfd45eb702 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 5 Dec 2023 22:24:57 +0900 Subject: [PATCH 1/4] feat(out_of_lane): improve reuse of previous decision (#1017) * Do not directly reuse a prev stop point but project it on the new path Signed-off-by: Maxime CLEMENT * Improve reuse of the previously inserted stop point Signed-off-by: Maxime CLEMENT * Fix precision of inserted stop point Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT --- .../src/overlapping_range.hpp | 3 +- .../src/scene_out_of_lane.cpp | 42 ++++++++++--------- .../src/scene_out_of_lane.hpp | 2 - 3 files changed, 25 insertions(+), 22 deletions(-) diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index 24bd6b3f16eb9..2b0830acc28cc 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -49,7 +49,8 @@ OverlapRanges calculate_overlapping_ranges( /// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path /// @param [in] lanelets lanelets used to calculate the overlaps /// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets +/// @return the overlapping ranges found between the footprints and the lanelets, sorted by +/// increasing arc length along the path OverlapRanges calculate_overlapping_ranges( const std::vector & path_footprints, const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 1a73e74a723e2..076b080cb0067 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -62,18 +62,6 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto ego_data.path.points = path->points; ego_data.first_path_idx = motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); - for (const auto & p : prev_overlapping_path_points_) { - const auto nearest_idx = - motion_utils::findNearestIndex(ego_data.path.points, p.point.pose, 1.0); - const auto insert_idx = - motion_utils::findNearestSegmentIndex(ego_data.path.points, p.point.pose, 1.0); - if (nearest_idx && insert_idx && *insert_idx > ego_data.first_path_idx) { - if ( - tier4_autoware_utils::calcDistance2d( - ego_data.path.points[*nearest_idx].point.pose, p.point.pose) > 0.5) - ego_data.path.points.insert(std::next(ego_data.path.points.begin(), *insert_idx), p); - } - } motion_utils::removeOverlapPoints(ego_data.path.points); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; @@ -113,10 +101,6 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto stopwatch.tic("calculate_overlapping_ranges"); const auto ranges = calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); - prev_overlapping_path_points_.clear(); - for (const auto & range : ranges) - prev_overlapping_path_points_.push_back( - ego_data.path.points[ego_data.first_path_idx + range.entering_path_idx]); const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); // Calculate stop and slowdown points stopwatch.tic("calculate_decisions"); @@ -143,7 +127,28 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == point_to_insert->slowdown.lane_to_avoid.id())) prev_inserted_point_time_ = clock_->now(); - if (!point_to_insert && prev_inserted_point_) point_to_insert = prev_inserted_point_; + // reuse previous stop point if there is no new one or if its velocity is not higher than the new + // one and its arc length is lower + const auto should_use_prev_inserted_point = [&]() { + if ( + point_to_insert && prev_inserted_point_ && + prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { + const auto arc_length = motion_utils::calcSignedArcLength( + path->points, 0LU, point_to_insert->point.point.pose.position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + path->points, 0LU, prev_inserted_point_->point.point.pose.position); + return prev_arc_length < arc_length; + } + return !point_to_insert && prev_inserted_point_; + }(); + if (should_use_prev_inserted_point) { + // if the path changed the prev point is no longer on the path so we project it + const auto insert_arc_length = motion_utils::calcSignedArcLength( + path->points, 0LU, prev_inserted_point_->point.point.pose.position); + prev_inserted_point_->point.point.pose = + motion_utils::calcInterpolatedPose(path->points, insert_arc_length); + point_to_insert = prev_inserted_point_; + } if (point_to_insert) { prev_inserted_point_ = point_to_insert; RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); @@ -152,8 +157,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto path->points, point_to_insert->point.point.pose.position) + 1; planning_utils::insertVelocity( - *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, - params_.precision); + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx); if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = point_to_insert->point.point.pose; diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index ebadf81bee004..a8f48900f6289 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -50,8 +50,6 @@ class OutOfLaneModule : public SceneModuleInterface private: PlannerParam params_; - std::vector - prev_overlapping_path_points_{}; std::optional prev_inserted_point_{}; rclcpp::Time prev_inserted_point_time_{}; From 52f1ab1b083d72beabaf470d120b5d4712eb6bc2 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 6 Dec 2023 14:41:27 +0900 Subject: [PATCH 2/4] fix(system_monitor): output command line (#5430) (#1057) * fix(system_monitor): output command line * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari Co-authored-by: takeshi-iwanari Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../process_monitor/process_monitor.hpp | 2 +- .../src/process_monitor/process_monitor.cpp | 18 +++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index 42bd2a3ea9236..ec2a90c6b27e4 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -94,7 +94,7 @@ class ProcessMonitor : public rclcpp::Node * @param [out] command output command line * @return true if success to get command line name */ - bool getCommandLineFromPiD(const std::string & pid, std::string * command); + bool getCommandLineFromPiD(const std::string & pid, std::string & command); /** * @brief get top-rated processes diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 6f58339f2ff4b..1173431b0f7c2 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,14 +414,22 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } -bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string & command) { std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; - std::ifstream commandFile(commandLineFilePath); + std::ifstream commandFile(commandLineFilePath, std::ios::in | std::ios::binary); + if (commandFile.is_open()) { - std::getline(commandFile, *command); + std::vector buffer; + std::copy( + std::istream_iterator(commandFile), std::istream_iterator(), + std::back_inserter(buffer)); commandFile.close(); - return true; + std::replace( + buffer.begin(), buffer.end(), '\0', + ' '); // 0x00 is used as delimiter in /cmdline instead of 0x20 (space) + command = std::string(buffer.begin(), buffer.end()); + return (buffer.size() > 0) ? true : false; // cmdline is empty if it is kernel process } else { return false; } @@ -478,7 +486,7 @@ void ProcessMonitor::getTopratedProcesses( std::string program_name; std::getline(stream, program_name); - bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + bool flag_find_command_line = getCommandLineFromPiD(info.processId, info.commandName); if (!flag_find_command_line) { info.commandName = program_name; // if command line is not found, use program name instead From f3d1cbfa5dd3c133c8e6106b17c929e18ff330e3 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 7 Dec 2023 14:11:50 +0900 Subject: [PATCH 3/4] chore: pull tracking object merger from awf/main Signed-off-by: yoshiri --- .../tracking_object_merger/CMakeLists.txt | 44 ++ perception/tracking_object_merger/README.md | 114 +++++ .../config/data_association_matrix.param.yaml | 168 ++++++++ .../decorative_tracker_merger.param.yaml | 26 ++ ...ecorative_tracker_merger_policy.param.yaml | 16 + .../decorative_tracker_merger.drawio.svg | 345 +++++++++++++++ .../image/time_sync.drawio.svg | 223 ++++++++++ .../image/tracklet_management.drawio.svg | 216 ++++++++++ .../data_association/data_association.hpp | 71 +++ .../data_association/solver/gnn_solver.hpp | 22 + .../solver/gnn_solver_interface.hpp | 35 ++ .../solver/mu_successive_shortest_path.hpp | 37 ++ .../solver/successive_shortest_path.hpp | 37 ++ .../decorative_tracker_merger.hpp | 134 ++++++ .../utils/tracker_state.hpp | 148 +++++++ .../tracking_object_merger/utils/utils.hpp | 133 ++++++ .../decorative_tracker_merger.launch.xml | 21 + perception/tracking_object_merger/package.xml | 32 ++ .../src/data_association/data_association.cpp | 238 +++++++++++ .../mu_successive_shortest_path_wrapper.cpp | 41 ++ .../successive_shortest_path.cpp | 370 ++++++++++++++++ .../src/decorative_tracker_merger.cpp | 403 ++++++++++++++++++ .../src/utils/tracker_state.cpp | 321 ++++++++++++++ .../src/utils/utils.cpp | 403 ++++++++++++++++++ 24 files changed, 3598 insertions(+) create mode 100644 perception/tracking_object_merger/CMakeLists.txt create mode 100644 perception/tracking_object_merger/README.md create mode 100644 perception/tracking_object_merger/config/data_association_matrix.param.yaml create mode 100644 perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml create mode 100644 perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml create mode 100644 perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg create mode 100644 perception/tracking_object_merger/image/time_sync.drawio.svg create mode 100644 perception/tracking_object_merger/image/tracklet_management.drawio.svg create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp create mode 100644 perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml create mode 100644 perception/tracking_object_merger/package.xml create mode 100644 perception/tracking_object_merger/src/data_association/data_association.cpp create mode 100644 perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp create mode 100644 perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp create mode 100644 perception/tracking_object_merger/src/decorative_tracker_merger.cpp create mode 100644 perception/tracking_object_merger/src/utils/tracker_state.cpp create mode 100644 perception/tracking_object_merger/src/utils/utils.cpp diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..5e46b9403462d --- /dev/null +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(tracking_object_merger VERSION 1.0.0) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wconversion) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() + + +# find dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(decorative_tracker_merger_node SHARED + src/data_association/data_association.cpp + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/decorative_tracker_merger.cpp + src/utils/utils.cpp + src/utils/tracker_state.cpp +) + +target_link_libraries(decorative_tracker_merger_node + Eigen3::Eigen +) + +rclcpp_components_register_node(decorative_tracker_merger_node + PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode" + EXECUTABLE decorative_tracker_merger +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md new file mode 100644 index 0000000000000..9964534372037 --- /dev/null +++ b/perception/tracking_object_merger/README.md @@ -0,0 +1,114 @@ +# Tracking Object Merger + +## Purpose + +This package try to merge two tracking objects from different sensor. + +## Inner-workings / Algorithms + +Merging tracking objects from different sensor is a combination of data association and state fusion algorithms. + +Detailed process depends on the merger policy. + +### decorative_tracker_merger + +In decorative_tracker_merger, we assume there are dominant tracking objects and sub tracking objects. +The name `decorative` means that sub tracking objects are used to complement the main objects. + +Usually the dominant tracking objects are from LiDAR and sub tracking objects are from Radar or Camera. + +Here show the processing pipeline. + +![decorative_tracker_merger](./image/decorative_tracker_merger.drawio.svg) + +#### time sync + +Sub object(Radar or Camera) often has higher frequency than dominant object(LiDAR). So we need to sync the time of sub object to dominant object. + +![time sync](image/time_sync.drawio.svg) + +#### data association + +In the data association, we use the following rules to determine whether two tracking objects are the same object. + +- gating + - `distance gate`: distance between two tracking objects + - `angle gate`: angle between two tracking objects + - `mahalanobis_distance_gate`: Mahalanobis distance between two tracking objects + - `min_iou_gate`: minimum IoU between two tracking objects + - `max_velocity_gate`: maximum velocity difference between two tracking objects +- score + - score used in matching is equivalent to the distance between two tracking objects + +#### tracklet update + +Sub tracking objects are merged into dominant tracking objects. + +Depends on the tracklet input sensor state, we update the tracklet state with different rules. + +| state\priority | 1st | 2nd | 3rd | +| -------------------------- | ------ | ----- | ------ | +| Kinematics except velocity | LiDAR | Radar | Camera | +| Forward velocity | Radar | LiDAR | Camera | +| Object classification | Camera | LiDAR | Radar | + +#### tracklet management + +We use the `existence_probability` to manage tracklet. + +- When we create a new tracklet, we set the `existence_probability` to $p_{sensor}$ value. +- In each update with specific sensor, we set the `existence_probability` to $p_{sensor}$ value. +- When tracklet does not have update with specific sensor, we reduce the `existence_probability` by `decay_rate` +- Object can be published if `existence_probability` is larger than `publish_probability_threshold` +- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold` + +![tracklet_management](./image/tracklet_management.drawio.svg) + +These parameter can be set in `config/decorative_tracker_merger.param.yaml`. + +```yaml +tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.6 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.6 + decay_rate: 0.1 + max_dt: 1.0 +``` + +#### input/parameters + +| topic name | message type | description | +| ------------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------- | +| `~input/main_object` | `autoware_auto_perception_msgs::TrackedObjects` | Dominant tracking objects. Output will be published with this dominant object stamps. | +| `~input/sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Sub tracking objects. | +| `output/object` | `autoware_auto_perception_msgs::TrackedObjects` | Merged tracking objects. | +| `debug/interpolated_sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Interpolated sub tracking objects. | + +Default parameters are set in [config/decorative_tracker_merger.param.yaml](./config/decorative_tracker_merger.param.yaml). + +| parameter name | description | default value | +| ------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `base_link_frame_id` | base link frame id. This is used to transform the tracking object. | "base_link" | +| `time_sync_threshold` | time sync threshold. If the time difference between two tracking objects is smaller than this value, we consider these two tracking objects are the same object. | 0.05 | +| `sub_object_timeout_sec` | sub object timeout. If the sub object is not updated for this time, we consider this object is not exist. | 0.5 | +| `main_sensor_type` | main sensor type. This is used to determine the dominant tracking object. | "lidar" | +| `sub_sensor_type` | sub sensor type. This is used to determine the sub tracking object. | "radar" | +| `tracker_state_parameter` | tracker state parameter. This is used to manage the tracklet. | | + +- the detail of `tracker_state_parameter` is described in [tracklet management](#tracklet-management) + +#### tuning + +As explained in [tracklet management](#tracklet-management), this tracker merger tend to maintain the both input tracking objects. + +If there are many false positive tracking objects, + +- decrease `default__existence_probability` of that sensor +- increase `decay_rate` +- increase `publish_probability_threshold` to publish only reliable tracking objects + +### equivalent_tracker_merger + +This is future work. diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..702809b3cede1 --- /dev/null +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -0,0 +1,168 @@ +/**: + ros__parameters: + lidar-lidar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN + + lidar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 5.5, 6.0, 6.0, 6.0, 1.0, 1.0, 1.0, #CAR + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #BUS + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN + + radar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 7.0, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #CAR + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #BUS + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml new file mode 100644 index 0000000000000..4a108be657a27 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -0,0 +1,26 @@ +# Node parameters +/**: + ros__parameters: + base_link_frame_id: "base_link" + time_sync_threshold: 0.999 + sub_object_timeout_sec: 0.8 + publish_interpolated_sub_objects: true #for debug + + # choose the input sensor type for each object type + # "lidar", "radar", "camera" are available + main_sensor_type: "lidar" + sub_sensor_type: "radar" + + # tracker settings + tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.5 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.5 + decay_rate: 0.1 + max_dt: 1.0 + + # logging settings + enable_logging: false + log_file_path: "/tmp/decorative_tracker_merger.log" diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000000..0b98e1b202957 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml @@ -0,0 +1,16 @@ +# Merger policy for decorative tracker merger node +# decorative tracker merger works by merging the sub-object trackers into a main object tracker result +# There are 3 merger policy: +# 1. "skip": skip the sub-object tracker result +# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result +# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion +/**: + ros__parameters: + kinematics_to_be_merged: "velocity" # currently only support "velocity" + + # choose the merger policy for each object type + # : "skip", "overwrite", "fusion" + kinematics_merge_policy: "overwrite" + classification_merge_policy: "skip" + existence_prob_merge_policy: "skip" + shape_merge_policy: "skip" diff --git a/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg new file mode 100644 index 0000000000000..3e35fa4d1c338 --- /dev/null +++ b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg @@ -0,0 +1,345 @@ + + + + + + + + + + + + +
+
+
+ main_object +
+
+
+ + main_object + + + + + + +
+
+
+ decorative_tracker_merger_node +
+
+
+ + decorative_tracker_merger_node + + + + + + + + +
+
+
+ sub_object +
+
+
+ + sub_object + + + + + + + + +
+
+
+ time-sync +
+ (delay compensation) +
+
+
+ + time-sync... + + + + + + + + +
+
+
+ buffer +
+
+
+ + buffer + + + + + + +
+
+
+ main obj +
+ timestamp +
+
+
+ + main obj... + + + + + + + + + + +
+
+
+ inner +
+ tracklet +
+
+
+ + inner... + + + + + + + +
+
+
+ not +
+ found +
+
+
+ + not... + + + + + + + +
+
+
+ found +
+
+
+ + found + + + + + + +
+
+
+ association +
+
+
+ + association + + + + + + + + +
+
+
+ update +
+ tracklet +
+
+
+ + update... + + + + + + + + +
+
+
+ add new +
+ tracklet +
+
+
+ + add new... + + + + + + + +
+
+
+ converted +
+ tracked object +
+
+
+ + converted... + + + + + + + + +
+
+
+ tracklet management +
+ (publisher) +
+
+
+ + tracklet management... + + + + + + +
+
+
+ merged object +
+
+
+ + merged object + + + + + + +
+
+
msg: TrackedObjects
+
+
+ + msg: TrackedObjects + + + + + + +
+
+
+ remove when low existence +
+ probability objects +
+
+
+ + remove when low existence... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/time_sync.drawio.svg b/perception/tracking_object_merger/image/time_sync.drawio.svg new file mode 100644 index 0000000000000..dad2eb1a7fd27 --- /dev/null +++ b/perception/tracking_object_merger/image/time_sync.drawio.svg @@ -0,0 +1,223 @@ + + + + + + + + + + + +
+
+
time
+
+
+ + time + + + + + + + +
+
+
main object topic
+
+
+ + main object topic + + + + + + +
+
+
sub object topic
+
+
+ + sub object topic + + + + + + + + + + + + + + + + + + + + +
+
+
publish timing
+
+
+ + publish timing + + + + + + +
+
+
+ interpolated sub object +
+
+
+ + interpolated sub object + + + + + + + + +
+
+
raw main/sub object
+
+
+ + raw main/sub object + + + + + + + + + +
+
+
+ time sync threshold +
+
+
+ + time sync threshold + + + + + + +
+
+
+ time synchronize prediction pattern +
+
+
+ + time synchronize prediction pattern + + + + + + + + + +
+
+
+ 1. forward +
+
+
+ 2. backward +
+
+
+ 3. interpolation +
+
+
+
+ + 1. forward... + + + + + + + + + + + + + + + + + + +
+
+
legend
+
+
+ + legend + + + + + + +
+
+
+ disabled +
+ now +
+
+
+ + disabled... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/tracklet_management.drawio.svg b/perception/tracking_object_merger/image/tracklet_management.drawio.svg new file mode 100644 index 0000000000000..4f2a00dfd4dfd --- /dev/null +++ b/perception/tracking_object_merger/image/tracklet_management.drawio.svg @@ -0,0 +1,216 @@ + + + + + + + + +
+
+
0.0
+
+
+ + 0.0 + + + + + + +
+
+
1.0
+
+
+ + 1.0 + + + + + + +
+
+
Existence Probability
+
+
+ + Existence Probability + + + + + + + +
+
+
remove threshold
+
+
+ + remove threshold + + + + + + +
+
+
can publish threshold
+
+
+ + can publish threshold + + + + + + + + + +
+
+
+ decay when +
+ not observed +
+
+
+ + decay when... + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + +
+
+
+ init or update probability +
+ from sensor +
+
+
+ + init or update probabilit... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp new file mode 100644 index 0000000000000..a62a39d11c192 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp @@ -0,0 +1,71 @@ +// 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" + +#include +#include + +#include +#include + +#include + +class DataAssociation +{ +private: + Eigen::MatrixXi can_assign_matrix_; + Eigen::MatrixXd max_dist_matrix_; + Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; + Eigen::MatrixXd max_velocity_diff_matrix_; + const double score_threshold_; + std::unique_ptr gnn_solver_ptr_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector); + void assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers); + double calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const; + virtual ~DataAssociation() {} +}; + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..31240848f1977 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp new file mode 100644 index 0000000000000..e915b5d2a9e7b --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp @@ -0,0 +1,35 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ + +#include +#include + +namespace gnn_solver +{ +class GnnSolverInterface +{ +public: + GnnSolverInterface() = default; + virtual ~GnnSolverInterface() = default; + + virtual void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) = 0; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp new file mode 100644 index 0000000000000..5c0ebc04fdde3 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class MuSSP : public GnnSolverInterface +{ +public: + MuSSP() = default; + ~MuSSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp new file mode 100644 index 0000000000000..47a737cf58298 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class SSP : public GnnSolverInterface +{ +public: + SSP() = default; + ~SSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp new file mode 100644 index 0000000000000..0509fb2a07dc5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -0,0 +1,134 @@ +// 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 TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ + +#include "tracking_object_merger/data_association/data_association.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tracking_object_merger +{ + +class DecorativeTrackerMergerNode : public rclcpp::Node +{ +public: + explicit DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options); + enum class PriorityMode : int { Object0 = 0, Object1 = 1, Confidence = 2 }; + +private: + void set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map); + + void mainObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + void subObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + + bool decorativeMerger( + const MEASUREMENT_STATE input_index, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + autoware_auto_perception_msgs::msg::TrackedObjects predictFutureState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg, + const std_msgs::msg::Header & header); + std::optional interpolateObjectState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg1, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg2, + const std_msgs::msg::Header & header); + TrackedObjects getTrackedObjects(const std_msgs::msg::Header & header); + TrackerState createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object); + +private: + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::Publisher::SharedPtr + merged_object_pub_; + rclcpp::Subscription::SharedPtr + sub_main_objects_; + rclcpp::Subscription::SharedPtr + sub_sub_objects_; + // debug object publisher + rclcpp::Publisher::SharedPtr + debug_object_pub_; + bool publish_interpolated_sub_objects_; + + /* handle objects */ + std::unordered_map> + input_merger_map_; + MEASUREMENT_STATE main_sensor_type_; + MEASUREMENT_STATE sub_sensor_type_; + std::vector inner_tracker_objects_; + std::unordered_map> data_association_map_; + std::string target_frame_; + std::string base_link_frame_id_; + // buffer to save the sub objects + std::vector + sub_objects_buffer_; + double sub_object_timeout_sec_; + double time_sync_threshold_; + + // tracker default settings + TrackerStateParameter tracker_state_parameter_; + + // merge policy (currently not used) + struct + { + std::string kinematics_to_be_merged; + merger_utils::MergePolicy kinematics_merge_policy; + merger_utils::MergePolicy classification_merge_policy; + merger_utils::MergePolicy existence_prob_merge_policy; + merger_utils::MergePolicy shape_merge_policy; + } merger_policy_params_; + + std::map merger_policy_map_ = { + {"skip", merger_utils::MergePolicy::SKIP}, + {"overwrite", merger_utils::MergePolicy::OVERWRITE}, + {"fusion", merger_utils::MergePolicy::FUSION}}; + + // debug parameters + struct logging + { + bool enable = false; + std::string path; + } logging_; +}; + +} // namespace tracking_object_merger + +#endif // TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp new file mode 100644 index 0000000000000..6bedf7db8d727 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp @@ -0,0 +1,148 @@ +// 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 TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// Enum classes to distinguish input sources +enum class MEASUREMENT_STATE : int { + NONE = 0, // 0000 + LIDAR = 1, // 0001 + RADAR = 2, // 0010 + CAMERA = 4, // 0100 + LIDAR_RADAR = 3, // 0011 + LIDAR_CAMERA = 5, // 0101 + RADAR_CAMERA = 6, // 0110 + LIDAR_RADAR_CAMERA = 7, // 0111 +}; + +// Operator overloading for MEASUREMENT_STATE +// 1. operator| for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR | MEASUREMENT_STATE::RADAR == MEASUREMENT_STATE::LIDAR_RADAR +// 2. operator& for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::LIDAR == true +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::CAMERA == false +inline MEASUREMENT_STATE operator|(MEASUREMENT_STATE lhs, MEASUREMENT_STATE rhs) +{ + return static_cast(static_cast(lhs) | static_cast(rhs)); +} +inline bool operator&(MEASUREMENT_STATE combined, MEASUREMENT_STATE target) +{ + return (static_cast(combined) & static_cast(target)) != 0; +} + +// Struct to handle tracker state public parameter +struct TrackerStateParameter +{ + double publish_probability_threshold = 0.5; + double remove_probability_threshold = 0.3; + double default_lidar_existence_probability = 0.8; + double default_radar_existence_probability = 0.7; + double default_camera_existence_probability = 0.6; + double decay_rate = 0.1; + double max_dt = 2.0; +}; + +// Class to handle tracker state +class TrackerState +{ +private: + /* data */ + TrackedObject tracked_object_; + rclcpp::Time last_update_time_; + // Eigen::MatrixXf state_matrix_; + // Eigen::MatrixXf covariance_matrix_; + + // timer handle + std::unordered_map last_updated_time_map_; + double max_dt_ = 2.0; + +public: + TrackerState( + const MEASUREMENT_STATE input, const rclcpp::Time & last_update_time, + const TrackedObject & tracked_object); + TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid); + + ~TrackerState(); + +public: + void setParameter(const TrackerStateParameter & parameter); + bool predict(const rclcpp::Time & time); + bool predict(const double dt, std::function func); + MEASUREMENT_STATE getCurrentMeasurementState(const rclcpp::Time & current_time) const; + bool updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object); + void updateWithLIDAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithRADAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithCAMERA(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithoutSensor(const rclcpp::Time & current_time); + bool update(const MEASUREMENT_STATE input, const TrackedObject & tracked_object); + bool updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object, + std::function update_func); + // const functions + bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const; + bool isValid() const; + bool canPublish() const; + TrackedObject getObject() const; + +public: + // handle uuid + unique_identifier_msgs::msg::UUID const_uuid_; + // each sensor input to uuid map + std::unordered_map> + input_uuid_map_; + MEASUREMENT_STATE measurement_state_; + + // following lifecycle control parameter is overwritten by TrackerStateParameter + std::unordered_map default_existence_probability_map_ = { + {MEASUREMENT_STATE::LIDAR, 0.8}, + {MEASUREMENT_STATE::RADAR, 0.7}, + {MEASUREMENT_STATE::CAMERA, 0.6}, + }; + double existence_probability_ = 0.0; + double publish_probability_threshold_ = 0.5; + double remove_probability_threshold_ = 0.3; + double decay_rate_ = 0.1; +}; + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & time); + +#endif // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp new file mode 100644 index 0000000000000..bc6dfef9b80bf --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -0,0 +1,133 @@ +// 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 TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ + +// #include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ +enum MSG_COV_IDX { + X_X = 0, + X_Y = 1, + X_Z = 2, + X_ROLL = 3, + X_PITCH = 4, + X_YAW = 5, + Y_X = 6, + Y_Y = 7, + Y_Z = 8, + Y_ROLL = 9, + Y_PITCH = 10, + Y_YAW = 11, + Z_X = 12, + Z_Y = 13, + Z_Z = 14, + Z_ROLL = 15, + Z_PITCH = 16, + Z_YAW = 17, + ROLL_X = 18, + ROLL_Y = 19, + ROLL_Z = 20, + ROLL_ROLL = 21, + ROLL_PITCH = 22, + ROLL_YAW = 23, + PITCH_X = 24, + PITCH_Y = 25, + PITCH_Z = 26, + PITCH_ROLL = 27, + PITCH_PITCH = 28, + PITCH_YAW = 29, + YAW_X = 30, + YAW_Y = 31, + YAW_Z = 32, + YAW_ROLL = 33, + YAW_PITCH = 34, + YAW_YAW = 35 +}; + +// linear interpolation for tracked objects +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2); + +// predict tracked objects +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt); + +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header); + +// predict tracked objects +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header); + +} // namespace utils + +namespace merger_utils +{ +// merge policy +enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 }; + +// object kinematics velocity merger +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy); + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// update tracked object +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj); + +} // namespace merger_utils + +#endif // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml new file mode 100644 index 0000000000000..5affbe474e8ae --- /dev/null +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml new file mode 100644 index 0000000000000..d74a8449b20e6 --- /dev/null +++ b/perception/tracking_object_merger/package.xml @@ -0,0 +1,32 @@ + + + + tracking_object_merger + 0.0.0 + merge tracking object + Yukihiro Saito + Yoshi Ri + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + eigen + mussp + object_recognition_utils + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp new file mode 100644 index 0000000000000..e7ab6077250f8 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -0,0 +1,238 @@ +// 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. + +#include "tracking_object_merger/data_association/data_association.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include +namespace +{ +double getFormedYawAngle( + const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, + const bool distinguish_front_or_back = true) +{ + const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; + const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; + // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 + double fixed_yaw0 = yaw0; + while (angle_range <= yaw1 - fixed_yaw0) { + fixed_yaw0 = fixed_yaw0 + angle_step; + } + while (angle_range <= fixed_yaw0 - yaw1) { + fixed_yaw0 = fixed_yaw0 - angle_step; + } + return std::fabs(fixed_yaw0 - yaw1); +} +} // namespace + +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector) +: score_threshold_(0.01) +{ + { + const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), assign_label_num, assign_label_num); + can_assign_matrix_ = can_assign_matrix_tmp.transpose(); + } + { + const int max_dist_label_num = static_cast(std::sqrt(max_dist_vector.size())); + Eigen::Map max_dist_matrix_tmp( + max_dist_vector.data(), max_dist_label_num, max_dist_label_num); + max_dist_matrix_ = max_dist_matrix_tmp.transpose(); + } + { + const int max_rad_label_num = static_cast(std::sqrt(max_rad_vector.size())); + Eigen::Map max_rad_matrix_tmp( + max_rad_vector.data(), max_rad_label_num, max_rad_label_num); + max_rad_matrix_ = max_rad_matrix_tmp.transpose(); + } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } + { + const int max_velocity_diff_label_num = + static_cast(std::sqrt(max_velocity_diff_vector.size())); + Eigen::Map max_velocity_diff_matrix_tmp( + max_velocity_diff_vector.data(), max_velocity_diff_label_num, max_velocity_diff_label_num); + max_velocity_diff_matrix_ = max_velocity_diff_matrix_tmp.transpose(); + } + + gnn_solver_ptr_ = std::make_unique(); +} + +void DataAssociation::assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) +{ + std::vector> score(src.rows()); + for (int row = 0; row < src.rows(); ++row) { + score.at(row).resize(src.cols()); + for (int col = 0; col < src.cols(); ++col) { + score.at(row).at(col) = src(row, col); + } + } + // Solve + gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + + for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) { + if (src(itr->first, itr->second) < score_threshold_) { + itr = direct_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) { + if (src(itr->second, itr->first) < score_threshold_) { + itr = reverse_assignment.erase(itr); + continue; + } else { + ++itr; + } + } +} + +/** + * @brief calc score matrix between two tracked objects + * This is used to associate input measurements + * + * @param objects0 : measurements + * @param objects1 : base objects(tracker objects) + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1) +{ + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size()); + for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) { + const auto & object1 = objects1.objects.at(objects1_idx); + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(objects1_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +/** + * @brief calc score matrix between two tracked object and Tracker State + * + * @param objects0 : measurements + * @param objects1 : tracker inner objects + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers) +{ + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & object1 = trackers.at(trackers_idx).getObject(); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +double DataAssociation::calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const +{ + const std::uint8_t object1_label = + object_recognition_utils::getHighestProbLabel(object1.classification); + const std::uint8_t object0_label = + object_recognition_utils::getHighestProbLabel(object0.classification); + + std::vector tracker_pose = { + object1.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y}; + std::vector measurement_pose = { + object0.kinematics.pose_with_covariance.pose.position.x, + object0.kinematics.pose_with_covariance.pose.position.y}; + + double score = 0.0; + if (can_assign_matrix_(object1_label, object0_label)) { + const double max_dist = max_dist_matrix_(object1_label, object0_label); + const double dist = tier4_autoware_utils::calcDistance2d( + object0.kinematics.pose_with_covariance.pose.position, + object1.kinematics.pose_with_covariance.pose.position); + + bool passed_gate = true; + // dist gate + if (passed_gate) { + if (max_dist < dist) passed_gate = false; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(object1_label, object0_label); + const double angle = getFormedYawAngle( + object0.kinematics.pose_with_covariance.pose.orientation, + object1.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) passed_gate = false; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(object1_label, object0_label); + const double min_union_iou_area = 1e-2; + const double iou = object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + if (iou < min_iou) passed_gate = false; + } + // max velocity diff gate + if (passed_gate) { + const double max_velocity_diff = max_velocity_diff_matrix_(object1_label, object0_label); + // calc absolute velocity diff (only thinking about speed) + const auto speed0 = std::hypot( + object0.kinematics.twist_with_covariance.twist.linear.x, + object0.kinematics.twist_with_covariance.twist.linear.y); + const auto speed1 = std::hypot( + object1.kinematics.twist_with_covariance.twist.linear.x, + object1.kinematics.twist_with_covariance.twist.linear.y); + const double velocity_diff = std::fabs(speed0 - speed1); + if (max_velocity_diff < velocity_diff) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; + } + } + return score; +} diff --git a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp new file mode 100644 index 0000000000000..fcc79c0a3cbd7 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -0,0 +1,41 @@ +// 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. + +#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +void MuSSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // Terminate if the graph is empty + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Solve DA by muSSP + solve_muSSP(cost, direct_assignment, reverse_assignment); +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..133f0d377373f --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,370 @@ +// 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. + +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +struct ResidualEdge +{ + // Destination node + const int dst; + int capacity; + const double cost; + int flow; + // Access to the reverse edge by adjacency_list.at(dst).at(reverse) + const int reverse; + + // ResidualEdge() + // : dst(0), capacity(0), cost(0), flow(0), reverse(0) {} + + ResidualEdge(int dst, int capacity, double cost, int flow, int reverse) + : dst(dst), capacity(capacity), cost(cost), flow(flow), reverse(reverse) + { + } +}; + +void SSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // NOTE: Need to set as default arguments + bool sparse_cost = true; + // bool sparse_cost = false; + + // Hyperparameters + // double MAX_COST = 6; + const double MAX_COST = 10; + const double INF_DIST = 10000000; + const double EPS = 1e-5; + + // When there is no agents or no tasks, terminate + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Construct a bipartite graph from the cost matrix + int n_agents = cost.size(); + int n_tasks = cost.at(0).size(); + + int n_dummies; + if (sparse_cost) { + n_dummies = n_agents; + } else { + n_dummies = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummies + 2; + + // // Print cost matrix + // std::cout << std::endl; + // for (int agent = 0; agent < n_agents; agent++) + // { + // for (int task = 0; task < n_tasks; task++) + // { + // std::cout << cost.at(agent).at(task) << " "; + // } + // std::cout << std::endl; + // } + + // std::chrono::system_clock::time_point start_time, end_time; + // start_time = std::chrono::system_clock::now(); + + // Adjacency list of residual graph (index: nodes) + // - 0: source node + // - {1, ..., n_agents}: agent nodes + // - {n_agents+1, ..., n_agents+n_tasks}: task nodes + // - n_agents+n_tasks+1: sink node + // - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}: + // dummy node (when sparse_cost is true) + std::vector> adjacency_list(n_nodes); + + // Reserve memory + for (int v = 0; v < n_nodes; ++v) { + if (v == source) { + // Source + adjacency_list.at(v).reserve(n_agents); + } else if (v <= n_agents) { + // Agents + adjacency_list.at(v).reserve(n_tasks + 1 + 1); + } else if (v <= n_agents + n_tasks) { + // Tasks + adjacency_list.at(v).reserve(n_agents + 1); + } else if (v == sink) { + // Sink + adjacency_list.at(v).reserve(n_tasks + n_dummies); + } else { + // Dummies + adjacency_list.at(v).reserve(2); + } + } + + // Add edges form source + for (int agent = 0; agent < n_agents; ++agent) { + // From source to agent + adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size()); + // From agent to source + adjacency_list.at(agent + 1).emplace_back( + source, 0, 0, 0, adjacency_list.at(source).size() - 1); + } + + // Add edges from agents + for (int agent = 0; agent < n_agents; ++agent) { + for (int task = 0; task < n_tasks; ++task) { + if (!sparse_cost || cost.at(agent).at(task) > EPS) { + // From agent to task + adjacency_list.at(agent + 1).emplace_back( + task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0, + adjacency_list.at(task + n_agents + 1).size()); + + // From task to agent + adjacency_list.at(task + n_agents + 1) + .emplace_back( + agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0, + adjacency_list.at(agent + 1).size() - 1); + } + } + } + + // Add edges form tasks + for (int task = 0; task < n_tasks; ++task) { + // From task to sink + adjacency_list.at(task + n_agents + 1) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to task + adjacency_list.at(sink).emplace_back( + task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1); + } + + // Add edges from dummy + if (sparse_cost) { + for (int agent = 0; agent < n_agents; ++agent) { + // From agent to dummy + adjacency_list.at(agent + 1).emplace_back( + agent + n_agents + n_tasks + 2, 1, MAX_COST, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size()); + + // From dummy to agent + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1); + + // From dummy to sink + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to dummy + adjacency_list.at(sink).emplace_back( + agent + n_agents + n_tasks + 2, 0, 0, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1); + } + } + + // Maximum flow value + const int max_flow = std::min(n_agents, n_tasks); + + // Feasible potentials + std::vector potentials(n_nodes, 0); + + // Shortest path lengths + std::vector distances(n_nodes, INF_DIST); + + // Whether previously visited the node or not + std::vector is_visited(n_nodes, false); + + // Parent node () + std::vector> prev_values(n_nodes); + + for (int i = 0; i < max_flow; ++i) { + // Initialize priority queue () + std::priority_queue< + std::pair, std::vector>, + std::greater>> + p_queue; + + // Reset all trajectory states + if (i > 0) { + std::fill(distances.begin(), distances.end(), INF_DIST); + std::fill(is_visited.begin(), is_visited.end(), false); + } + + // Start trajectory from the source node + p_queue.push(std::make_pair(0, source)); + distances.at(source) = 0; + + while (!p_queue.empty()) { + // Get the next element + std::pair cur_elem = p_queue.top(); + // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; + p_queue.pop(); + + double cur_node_dist = cur_elem.first; + int cur_node = cur_elem.second; + + // If already visited node, skip and continue + if (is_visited.at(cur_node)) { + continue; + } + assert(cur_node_dist == distances.at(cur_node)); + + // Mark as visited + is_visited.at(cur_node) = true; + // Update potential + potentials.at(cur_node) += cur_node_dist; + + // When reached to the sink node, terminate. + if (cur_node == sink) { + break; + } + + // Loop over the incident nodes(/edges) + for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin(); + it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) { + // If the node is not visited and have capacity to increase flow, visit. + if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) { + // Calculate reduced cost + double reduced_cost = + it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + if (distances.at(it_incident_edge->dst) > reduced_cost) { + distances.at(it_incident_edge->dst) = reduced_cost; + prev_values.at(it_incident_edge->dst) = + std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); + // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; + p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + } + } + } + } + + // Shortest path length to sink is greater than MAX_COST, + // which means no non-dummy routes left, terminate + if (potentials.at(sink) >= MAX_COST) { + break; + } + + // Update potentials of unvisited nodes + for (int v = 0; v < n_nodes; ++v) { + if (!is_visited.at(v)) { + potentials.at(v) += distances.at(sink); + } + } + // //Print potentials + // for (int v = 0; v < n_nodes; ++v) + // { + // std::cout << potentials.at(v) << ", "; + // } + // std::cout << std::endl; + + // Increase/decrease flow and capacity along the shortest path from the source to the sink + int v = sink; + int prev_v; + while (v != source) { + ResidualEdge & e_forward = + adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second); + assert(e_forward.dst == v); + ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); + prev_v = e_backward.dst; + + if (e_backward.flow == 0) { + // Increase flow + // State A + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 0); + + e_forward.capacity -= 1; + e_forward.flow += 1; + e_backward.capacity += 1; + + // State B + assert(e_forward.capacity == 0); + assert(e_forward.flow == 1); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } else { + // Decrease flow + // State B + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 1); + + e_forward.capacity -= 1; + e_backward.capacity += 1; + e_backward.flow -= 1; + + // State A + assert(e_forward.capacity == 0); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } + + v = prev_v; + } + +#ifndef NDEBUG + // Check if the potentials are feasible potentials + for (int v = 0; v < n_nodes; ++v) { + for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + if (it_incident_edge->capacity > 0) { + double reduced_cost = + it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + } + } + } +#endif + } + + // Output + for (int agent = 0; agent < n_agents; ++agent) { + for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin(); + it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) { + int task = it_incident_edge->dst - (n_agents + 1); + + // If the flow value is 1 and task is not dummy, assign the task to the agent. + if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) { + (*direct_assignment)[agent] = task; + (*reverse_assignment)[task] = agent; + break; + } + } + } + +#ifndef NDEBUG + // Check if the result is valid assignment + for (int agent = 0; agent < n_agents; ++agent) { + if (direct_assignment->find(agent) != direct_assignment->cend()) { + int task = (*direct_assignment).at(agent); + assert(direct_assignment->at(agent) == task); + assert(reverse_assignment->at(task) == agent); + } + } +#endif +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp new file mode 100644 index 0000000000000..47a333691eabf --- /dev/null +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -0,0 +1,403 @@ +// 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. + +#include "tracking_object_merger/decorative_tracker_merger.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +namespace tracking_object_merger +{ + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// get unix time from header +double getUnixTime(const std_msgs::msg::Header & header) +{ + return header.stamp.sec + header.stamp.nanosec * 1e-9; +} + +// calc association score matrix +Eigen::MatrixXd calcScoreMatrixForAssociation( + const MEASUREMENT_STATE measurement_state, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers, + const std::unordered_map> & data_association_map + // const bool debug_log, const std::string & file_name // do not logging for now +) +{ + // get current time + const rclcpp::Time current_time = rclcpp::Time(objects0.header.stamp); + + // calc score matrix + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & tracker_obj = trackers.at(trackers_idx); + const auto & object1 = tracker_obj.getObject(); + const auto & tracker_state = tracker_obj.getCurrentMeasurementState(current_time); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + // switch calc score function by input and trackers measurement state + // we assume that lidar and radar are exclusive + double score; + const auto input_has_lidar = measurement_state & MEASUREMENT_STATE::LIDAR; + const auto tracker_has_lidar = tracker_state & MEASUREMENT_STATE::LIDAR; + if (input_has_lidar && tracker_has_lidar) { + score = data_association_map.at("lidar-lidar")->calcScoreBetweenObjects(object0, object1); + } else if (!input_has_lidar && !tracker_has_lidar) { + score = data_association_map.at("radar-radar")->calcScoreBetweenObjects(object0, object1); + } else { + score = data_association_map.at("lidar-radar")->calcScoreBetweenObjects(object0, object1); + } + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("decorative_object_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_) +{ + // Subscriber + sub_main_objects_ = create_subscription( + "input/main_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::mainObjectsCallback, this, std::placeholders::_1)); + sub_sub_objects_ = create_subscription( + "input/sub_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::subObjectsCallback, this, std::placeholders::_1)); + + // merged object publisher + merged_object_pub_ = create_publisher("output/object", rclcpp::QoS{1}); + // debug object publisher + debug_object_pub_ = + create_publisher("debug/interpolated_sub_object", rclcpp::QoS{1}); + + // logging + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // Parameters + publish_interpolated_sub_objects_ = declare_parameter("publish_interpolated_sub_objects"); + base_link_frame_id_ = declare_parameter("base_link_frame_id"); + time_sync_threshold_ = declare_parameter("time_sync_threshold"); + sub_object_timeout_sec_ = declare_parameter("sub_object_timeout_sec"); + // default setting parameter for tracker + tracker_state_parameter_.remove_probability_threshold = + declare_parameter("tracker_state_parameter.remove_probability_threshold"); + tracker_state_parameter_.publish_probability_threshold = + declare_parameter("tracker_state_parameter.publish_probability_threshold"); + tracker_state_parameter_.default_lidar_existence_probability = + declare_parameter("tracker_state_parameter.default_lidar_existence_probability"); + tracker_state_parameter_.default_radar_existence_probability = + declare_parameter("tracker_state_parameter.default_radar_existence_probability"); + tracker_state_parameter_.default_camera_existence_probability = + declare_parameter("tracker_state_parameter.default_camera_existence_probability"); + tracker_state_parameter_.decay_rate = + declare_parameter("tracker_state_parameter.decay_rate"); + tracker_state_parameter_.max_dt = declare_parameter("tracker_state_parameter.max_dt"); + + const std::string main_sensor_type = declare_parameter("main_sensor_type"); + const std::string sub_sensor_type = declare_parameter("sub_sensor_type"); + // str to MEASUREMENT_STATE + auto str2measurement_state = [](const std::string & str) { + if (str == "lidar") { + return MEASUREMENT_STATE::LIDAR; + } else if (str == "radar") { + return MEASUREMENT_STATE::RADAR; + } else { + throw std::runtime_error("invalid sensor type"); + } + }; + main_sensor_type_ = str2measurement_state(main_sensor_type); + sub_sensor_type_ = str2measurement_state(sub_sensor_type); + + /* init association **/ + // lidar-lidar association matrix + set3dDataAssociation("lidar-lidar", data_association_map_); + // lidar-radar association matrix + set3dDataAssociation("lidar-radar", data_association_map_); + // radar-radar association matrix + set3dDataAssociation("radar-radar", data_association_map_); +} + +void DecorativeTrackerMergerNode::set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map) +{ + const auto tmp = this->declare_parameter>(prefix + ".can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + const auto max_dist_matrix = + this->declare_parameter>(prefix + ".max_dist_matrix"); + const auto max_rad_matrix = + this->declare_parameter>(prefix + ".max_rad_matrix"); + const auto min_iou_matrix = + this->declare_parameter>(prefix + ".min_iou_matrix"); + const auto max_velocity_diff_matrix = + this->declare_parameter>(prefix + ".max_velocity_diff_matrix"); + + data_association_map[prefix] = std::make_unique( + can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix, max_velocity_diff_matrix); +} + +/** + * @brief callback function for main objects + * + * @param main_objects + * @note if there are no sub objects, publish main objects as it is + * else, merge main objects and sub objects + */ +void DecorativeTrackerMergerNode::mainObjectsCallback( + const TrackedObjects::ConstSharedPtr & main_objects) +{ + // try to merge sub object + if (!sub_objects_buffer_.empty()) { + // get interpolated sub objects + // get newest sub objects which timestamp is earlier to main objects + TrackedObjects::ConstSharedPtr closest_time_sub_objects; + TrackedObjects::ConstSharedPtr closest_time_sub_objects_later; + for (const auto & sub_object : sub_objects_buffer_) { + if (getUnixTime(sub_object->header) < getUnixTime(main_objects->header)) { + closest_time_sub_objects = sub_object; + } else { + closest_time_sub_objects_later = sub_object; + break; + } + } + // get delay compensated sub objects + const auto interpolated_sub_objects = interpolateObjectState( + closest_time_sub_objects, closest_time_sub_objects_later, main_objects->header); + if (interpolated_sub_objects.has_value()) { + // Merge sub objects + const auto interp_sub_objs = interpolated_sub_objects.value(); + debug_object_pub_->publish(interp_sub_objs); + this->decorativeMerger( + sub_sensor_type_, std::make_shared(interpolated_sub_objects.value())); + } else { + RCLCPP_DEBUG(this->get_logger(), "interpolated_sub_objects is null"); + } + } + + // try to merge main object + this->decorativeMerger(main_sensor_type_, main_objects); + + merged_object_pub_->publish(getTrackedObjects(main_objects->header)); +} + +/** + * @brief callback function for sub objects + * + * @param msg + * @note push back sub objects to buffer and remove old sub objects + */ +void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::ConstSharedPtr & msg) +{ + sub_objects_buffer_.push_back(msg); + // remove old sub objects + // const auto now = get_clock()->now(); + const auto now = rclcpp::Time(msg->header.stamp); + const auto remove_itr = std::remove_if( + sub_objects_buffer_.begin(), sub_objects_buffer_.end(), [now, this](const auto & sub_object) { + return (now - rclcpp::Time(sub_object->header.stamp)).seconds() > sub_object_timeout_sec_; + }); + sub_objects_buffer_.erase(remove_itr, sub_objects_buffer_.end()); +} + +/** + * @brief merge objects into inner_tracker_objects_ + * + * @param main_objects + * @return TrackedObjects + * + * @note 1. interpolate sub objects to sync main objects + * 2. do association + * 3. merge objects + */ +bool DecorativeTrackerMergerNode::decorativeMerger( + const MEASUREMENT_STATE input_sensor, const TrackedObjects::ConstSharedPtr & input_objects_msg) +{ + // get current time + const auto current_time = rclcpp::Time(input_objects_msg->header.stamp); + if (input_objects_msg->objects.empty()) { + return false; + } + if (inner_tracker_objects_.empty()) { + for (const auto & object : input_objects_msg->objects) { + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object)); + } + } + + // do prediction for inner objects + for (auto & object : inner_tracker_objects_) { + object.predict(current_time); + } + + // TODO(yoshiri): pre-association + + // associate inner objects and input objects + /* global nearest neighbor */ + std::unordered_map direct_assignment, reverse_assignment; + const auto & objects1 = input_objects_msg->objects; + Eigen::MatrixXd score_matrix = calcScoreMatrixForAssociation( + input_sensor, *input_objects_msg, inner_tracker_objects_, data_association_map_); + data_association_map_.at("lidar-lidar") + ->assign(score_matrix, direct_assignment, reverse_assignment); + + // look for tracker + for (int tracker_idx = 0; tracker_idx < static_cast(inner_tracker_objects_.size()); + ++tracker_idx) { + auto & object0_state = inner_tracker_objects_.at(tracker_idx); + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found and merge + const auto & object1 = objects1.at(direct_assignment.at(tracker_idx)); + // merge object1 into object0_state + object0_state.updateState(input_sensor, current_time, object1); + } else { // not found + // decrease existence probability + object0_state.updateWithoutSensor(current_time); + } + } + // look for new object + for (int object1_idx = 0; object1_idx < static_cast(objects1.size()); ++object1_idx) { + const auto & object1 = objects1.at(object1_idx); + if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found + } else { // not found + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object1)); + } + } + return true; +} + +/** + * @brief interpolate sub objects to sync main objects + * + * @param former_msg + * @param latter_msg + * @param output_header + * @return std::optional + * + * @note 1. if both msg is nullptr, return null optional + * 2. if former_msg is nullptr, return latter_msg + * 3. if latter_msg is nullptr, return former_msg + * 4. if both msg is not nullptr, do the interpolation + */ +std::optional DecorativeTrackerMergerNode::interpolateObjectState( + const TrackedObjects::ConstSharedPtr & former_msg, + const TrackedObjects::ConstSharedPtr & latter_msg, const std_msgs::msg::Header & output_header) +{ + // Assumption: output_header must be newer than former_msg and older than latter_msg + // There three possible patterns + // 1. both msg is nullptr + // 2. former_msg is nullptr + // 3. latter_msg is nullptr + // 4. both msg is not nullptr + + // 1. both msg is nullptr + if (former_msg == nullptr && latter_msg == nullptr) { + // return null optional + RCLCPP_DEBUG(this->get_logger(), "interpolation err: both msg is nullptr"); + return std::nullopt; + } + + // 2. former_msg is nullptr + if (former_msg == nullptr) { + // std::cout << "interpolation: 2. former_msg is nullptr" << std::endl; + // depends on header stamp difference + if ( + (rclcpp::Time(latter_msg->header.stamp) - rclcpp::Time(output_header.stamp)).seconds() > + time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG( + this->get_logger(), "interpolation err: latter msg is too different from output msg"); + return std::nullopt; + } else { // else, return latter_msg + return *latter_msg; + } + + // 3. latter_msg is nullptr + } else if (latter_msg == nullptr) { + // std::cout << "interpolation: 3. latter_msg is nullptr" << std::endl; + // depends on header stamp difference + const auto dt = + (rclcpp::Time(output_header.stamp) - rclcpp::Time(former_msg->header.stamp)).seconds(); + if (dt > time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG(this->get_logger(), "interpolation err: former msg is too old"); + return std::nullopt; + } else { + // else, return former_msg + return utils::predictPastOrFutureTrackedObjects(*former_msg, output_header); + } + + // 4. both msg is not nullptr + } else { + // do the interpolation + // std::cout << "interpolation: 4. both msg is not nullptr" << std::endl; + TrackedObjects interpolated_msg = + utils::interpolateTrackedObjects(*former_msg, *latter_msg, output_header); + return interpolated_msg; + } +} + +// get merged objects +TrackedObjects DecorativeTrackerMergerNode::getTrackedObjects(const std_msgs::msg::Header & header) +{ + // get main objects + rclcpp::Time current_time = rclcpp::Time(header.stamp); + return getTrackedObjectsFromTrackerStates(inner_tracker_objects_, current_time); +} + +// create new tracker +TrackerState DecorativeTrackerMergerNode::createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object) +{ + // check if object id is not included in innner_tracker_objects_ + for (const auto & object : inner_tracker_objects_) { + if (object.const_uuid_ == input_object.object_id) { + // create new uuid + unique_identifier_msgs::msg::UUID uuid; + // Generate random number + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid.uuid.begin(), uuid.uuid.end(), bit_eng); + auto new_tracker = TrackerState(input_index, current_time, input_object, uuid); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; + } + } + // if not found, just create new tracker + auto new_tracker = TrackerState(input_index, current_time, input_object); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; +} + +} // namespace tracking_object_merger + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(tracking_object_merger::DecorativeTrackerMergerNode) diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp new file mode 100644 index 0000000000000..f0dc657145a24 --- /dev/null +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -0,0 +1,321 @@ +// 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. + +#include "tracking_object_merger/utils/tracker_state.hpp" + +#include "tracking_object_merger/utils/utils.hpp" + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +/** + * @brief Construct a new Tracker State:: Tracker State object + * + * @param input_source : input source distinguished + * @param tracked_object : input source tracked object + * @param last_update_time : last update time + */ +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(tracked_object.object_id), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(uuid), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +void TrackerState::setParameter(const TrackerStateParameter & parameter) +{ + max_dt_ = parameter.max_dt; + publish_probability_threshold_ = parameter.publish_probability_threshold; + remove_probability_threshold_ = parameter.remove_probability_threshold; + default_existence_probability_map_.at(MEASUREMENT_STATE::LIDAR) = + parameter.default_lidar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::RADAR) = + parameter.default_radar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::CAMERA) = + parameter.default_camera_existence_probability; + decay_rate_ = parameter.decay_rate; +} + +/** + * @brief Predict state to current time + * + * @param current_time + * @return true + * @return false + */ +bool TrackerState::predict(const rclcpp::Time & current_time) +{ + // get dt and give warning if dt is too large + double dt = (current_time - last_update_time_).seconds(); + if (std::abs(dt) > max_dt_) { + std::cerr << "[tracking_object_merger] dt is too large: " << dt << std::endl; + return false; + } + + // do prediction + last_update_time_ = current_time; + return this->predict(dt, utils::predictPastOrFutureTrackedObject); +} + +/** + * @brief Predict state to current time with given update function + * + * @param dt : time to predict + * @param func : update function (e.g. PredictPastOrFutureTrackedObject) + * @return true: prediction succeeded + * @return false: prediction failed + */ +bool TrackerState::predict( + const double dt, std::function func) +{ + const auto predicted_object = func(tracked_object_, dt); + tracked_object_ = predicted_object; + return true; +} + +// get current measurement state +MEASUREMENT_STATE TrackerState::getCurrentMeasurementState(const rclcpp::Time & current_time) const +{ + MEASUREMENT_STATE measurement_state = MEASUREMENT_STATE::NONE; + // check LIDAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::LIDAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::LIDAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::LIDAR; + } + } + // check RADAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::RADAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::RADAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::RADAR; + } + } + // check CAMERA + if (last_updated_time_map_.find(MEASUREMENT_STATE::CAMERA) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::CAMERA)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::CAMERA; + } + } + return measurement_state; +} + +bool TrackerState::updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0.0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return false; + } + + // predict + if (dt > 0.0) { + this->predict(dt, utils::predictPastOrFutureTrackedObject); + } + + // get current measurement state + measurement_state_ = getCurrentMeasurementState(current_time); + + // update with input + if (input & MEASUREMENT_STATE::LIDAR) { + updateWithLIDAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::RADAR) { + updateWithRADAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::CAMERA) { + updateWithCAMERA(current_time, tracked_object); + } + return true; +} + +void TrackerState::updateWithCAMERA( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::CAMERA, current_time, tracked_object, + merger_utils::updateOnlyClassification); +} + +void TrackerState::updateWithLIDAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if radar is available, do not update velocity + if (measurement_state_ & MEASUREMENT_STATE::RADAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, merger_utils::updateExceptVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +void TrackerState::updateWithRADAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if lidar is available, update only velocity + if (measurement_state_ & MEASUREMENT_STATE::LIDAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateOnlyObjectVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +bool TrackerState::updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & input_tracked_object, + std::function update_func) +{ + // put input uuid and last update time + if (current_time > last_update_time_) { + const auto predict_successful = predict(current_time); + if (!predict_successful) { + return false; + } + } + + // update with measurement state + last_updated_time_map_[input] = current_time; + input_uuid_map_[input] = input_tracked_object.object_id; + measurement_state_ = measurement_state_ | input; + existence_probability_ = + std::max(existence_probability_, default_existence_probability_map_[input]); + + // update tracked object + update_func(tracked_object_, input_tracked_object); + tracked_object_.object_id = const_uuid_; // overwrite uuid to stay same + return true; +} + +void TrackerState::updateWithoutSensor(const rclcpp::Time & current_time) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return; + } + + // predict + if (dt > 0.0) { + const auto predict_successful = this->predict(dt, utils::predictPastOrFutureTrackedObject); + if (!predict_successful) { + existence_probability_ = 0.0; // remove object + } + } + + // reduce probability + existence_probability_ -= decay_rate_; + if (existence_probability_ < 0.0) { + existence_probability_ = 0.0; + } +} + +TrackedObject TrackerState::getObject() const +{ + TrackedObject tracked_object = tracked_object_; + tracked_object.object_id = const_uuid_; + tracked_object.existence_probability = + static_cast(existence_probability_); // double -> float + return tracked_object; +} + +bool TrackerState::hasUUID( + const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const +{ + if (input_uuid_map_.find(input) == input_uuid_map_.end()) { + return false; + } + return input_uuid_map_.at(input) == uuid; +} + +bool TrackerState::isValid() const +{ + // check if tracker state is valid + if (existence_probability_ < remove_probability_threshold_) { + return false; + } + return true; +} + +bool TrackerState::canPublish() const +{ + // check if tracker state is valid + if (existence_probability_ < publish_probability_threshold_) { + return false; + } + return true; +} + +TrackerState::~TrackerState() +{ + // destructor +} + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & current_time) +{ + TrackedObjects tracked_objects; + + // sanitize and get tracked objects + for (auto it = tracker_states.begin(); it != tracker_states.end();) { + // check if tracker state is valid + if (it->isValid()) { + if (it->canPublish()) { + // if valid, get tracked object + tracked_objects.objects.push_back(it->getObject()); + } + ++it; + } else { + // if not valid, delete tracker state + it = tracker_states.erase(it); + } + } + + // update header + tracked_objects.header.stamp = current_time; + tracked_objects.header.frame_id = "map"; // TODO(yoshiri): get frame_id from input + return tracked_objects; +} diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp new file mode 100644 index 0000000000000..5987bdc2d1bef --- /dev/null +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -0,0 +1,403 @@ +// 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. + +#include "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ + +/** + * @brief linear interpolation for tracked object + * + * @param obj1 : obj1 and obj2 must have same shape type + * @param obj2 : obj1 and obj2 must have same shape type + * @param weight : 0 <= weight <= 1, weight = 0: obj1, weight = 1: obj2 + * @return TrackedObject : interpolated object + */ +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2, const double weight) +{ + // interpolate obj1 and obj2 with weight: obj1 * (1 - weight) + obj2 * weight + // weight = 0: obj1, weight = 1: obj2 + + // weight check (0 <= weight <= 1) + if (weight < 0 || weight > 1) { + std::cerr << "weight must be 0 <= weight <= 1 in linearInterpolationForTrackedObject function." + << std::endl; + return obj1; + } + + // output object + TrackedObject output; + // copy input object at first + output = obj1; + + // get current twist and pose + const auto twist1 = obj1.kinematics.twist_with_covariance.twist; + const auto pose1 = obj1.kinematics.pose_with_covariance.pose; + const auto twist2 = obj2.kinematics.twist_with_covariance.twist; + const auto pose2 = obj2.kinematics.pose_with_covariance.pose; + + // interpolate twist + auto & output_twist = output.kinematics.twist_with_covariance.twist; + output_twist.linear.x = twist1.linear.x * (1 - weight) + twist2.linear.x * weight; + output_twist.linear.y = twist1.linear.y * (1 - weight) + twist2.linear.y * weight; + output_twist.linear.z = twist1.linear.z * (1 - weight) + twist2.linear.z * weight; + output_twist.angular.x = twist1.angular.x * (1 - weight) + twist2.angular.x * weight; + output_twist.angular.y = twist1.angular.y * (1 - weight) + twist2.angular.y * weight; + output_twist.angular.z = twist1.angular.z * (1 - weight) + twist2.angular.z * weight; + + // interpolate pose + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x = pose1.position.x * (1 - weight) + pose2.position.x * weight; + output_pose.position.y = pose1.position.y * (1 - weight) + pose2.position.y * weight; + output_pose.position.z = pose1.position.z * (1 - weight) + pose2.position.z * weight; + // interpolate orientation with slerp + // https://en.wikipedia.org/wiki/Slerp + const auto q1 = tf2::Quaternion( + pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w); + const auto q2 = tf2::Quaternion( + pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w); + const auto q = q1.slerp(q2, weight); + output_pose.orientation.x = q.x(); + output_pose.orientation.y = q.y(); + output_pose.orientation.z = q.z(); + output_pose.orientation.w = q.w(); + + // interpolate shape(mostly for bounding box) + const auto shape1 = obj1.shape; + const auto shape2 = obj2.shape; + if (shape1.type != shape2.type) { + // if shape type is different, return obj1 + } else { + if (shape1.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + auto & output_shape = output.shape; + output_shape.dimensions.x = shape1.dimensions.x * (1 - weight) + shape2.dimensions.x * weight; + output_shape.dimensions.y = shape1.dimensions.y * (1 - weight) + shape2.dimensions.y * weight; + output_shape.dimensions.z = shape1.dimensions.z * (1 - weight) + shape2.dimensions.z * weight; + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // (TODO) implement + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // (TODO) implement + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in linearInterpolationForTrackedObject function." + << std::endl; + return output; + } + } + + return output; +} + +/** + * @brief predict past or future tracked object + * + * @param obj + * @param dt + * @return TrackedObject + */ +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt) +{ + // output object + TrackedObject output; + // copy input object at first + output = obj; + if (dt == 0) { + return output; + } + + // get current twist and pose + const auto twist = obj.kinematics.twist_with_covariance.twist; + const auto pose = obj.kinematics.pose_with_covariance.pose; + + // get yaw + const auto yaw = tf2::getYaw(pose.orientation); + const auto vx = twist.linear.x; + const auto vy = twist.linear.y; + + // linear prediction equation: + // x = x0 + vx * cos(yaw) * dt - vy * sin(yaw) * dt + // y = y0 + vx * sin(yaw) * dt + vy * cos(yaw) * dt + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x += vx * std::cos(yaw) * dt - vy * std::sin(yaw) * dt; + output_pose.position.y += vx * std::sin(yaw) * dt + vy * std::cos(yaw) * dt; + + // (TODO) covariance prediction + + return output; +} + +/** + * @brief predict past or future tracked objects + * + * @param obj + * @param header + * @return TrackedObjects + */ +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header) +{ + // for each object, predict past or future + TrackedObjects output_objects; + output_objects.header = obj.header; + output_objects.header.stamp = header.stamp; + + const auto dt = (rclcpp::Time(header.stamp) - rclcpp::Time(obj.header.stamp)).seconds(); + for (const auto & obj : obj.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj, dt)); + } + return output_objects; +} + +/** + * @brief interpolate tracked objects + * + * @param objects1 + * @param objects2 + * @param header : header of output object + * @return TrackedObjects + */ +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header) +{ + // Assumption: time of output header is between objects1 and objects2 + // time of objects1 < header < objects2 + + // define output objects + TrackedObjects output_objects; + output_objects.header = header; + + // calc weight + const auto dt = + (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(objects2.header.stamp)).seconds(); + const auto dt1 = (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(header.stamp)).seconds(); + const auto weight = dt1 / dt; + + // check if object number is not zero + if (objects1.objects.size() == 0 && objects2.objects.size() == 0) { + // if objects1 and objects2 are empty, return empty objects + return output_objects; + } else if (objects1.objects.size() == 0) { + // if objects1 is empty return objects2 + for (const auto & obj2 : objects2.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj2, dt1 - dt)); + } + return output_objects; + } else if (objects2.objects.size() == 0) { + // if objects2 is empty return objects1 + for (const auto & obj1 : objects1.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj1, dt1)); + } + } else { + // if both objects1 and objects2 are not empty do nothing + } + + // map to handle selected objects + std::unordered_map selected_objects1; + // iterate with int + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + selected_objects1[i] = false; + } + // search for objects with int iterator + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + for (std::size_t j = 0; j < objects2.objects.size(); j++) { + if (objects1.objects[i].object_id == objects2.objects[j].object_id) { + // if id is same, interpolate + const auto interp_objects = + linearInterpolationForTrackedObject(objects1.objects[i], objects2.objects[j], weight); + output_objects.objects.push_back(interp_objects); + selected_objects1[i] = true; + break; + } + } + if (selected_objects1[i] == false) { + // if obj1 is not selected, predict future + const auto pred_objects = predictPastOrFutureTrackedObject(objects1.objects[i], dt1); + output_objects.objects.push_back(pred_objects); + } + } + + return output_objects; +} + +} // namespace utils + +namespace merger_utils +{ + +double mean(const double a, const double b) +{ + return (a + b) / 2.0; +} + +// object kinematics merger +// currently only support velocity fusion +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; + // copy main object at first + output_kinematics = main_obj.kinematics; + if (policy == MergePolicy::SKIP) { + return output_kinematics; + } else if (policy == MergePolicy::OVERWRITE) { + output_kinematics.twist_with_covariance.twist.linear.x = + sub_obj.kinematics.twist_with_covariance.twist.linear.x; + return output_kinematics; + } else if (policy == MergePolicy::FUSION) { + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + // inverse weight + const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + double main_vx_weight, sub_vx_weight; + if (main_vx_cov == 0.0) { + main_vx_weight = 1.0 * 1e6; + } else { + main_vx_weight = 1.0 / main_vx_cov; + } + if (sub_vx_cov == 0.0) { + sub_vx_weight = 1.0 * 1e6; + } else { + sub_vx_weight = 1.0 / sub_vx_cov; + } + // merge with covariance + output_kinematics.twist_with_covariance.twist.linear.x = + (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight); + output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight); + return output_kinematics; + } else { + std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl; + return output_kinematics; + } + return output_kinematics; +} + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_obj; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj; + } else if (policy == MergePolicy::FUSION) { + // merge two std::vector into one + TrackedObject dummy_obj; + dummy_obj.classification = main_obj.classification; + for (const auto & sub_class : sub_obj.classification) { + dummy_obj.classification.push_back(sub_class); + } + return dummy_obj; + } else { + std::cerr << "unknown merge policy in objectClassificationMerger function." << std::endl; + return main_obj; + } +} + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_prob; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_prob; + } else if (policy == MergePolicy::FUSION) { + return static_cast(mean(main_prob, sub_prob)); + } else { + std::cerr << "unknown merge policy in probabilityMerger function." << std::endl; + return main_prob; + } +} + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const autoware_auto_perception_msgs::msg::Shape & main_obj_shape, + const autoware_auto_perception_msgs::msg::Shape & sub_obj_shape, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::Shape output_shape; + // copy main object at first + output_shape = main_obj_shape; + + if (main_obj_shape.type != sub_obj_shape.type) { + // if shape type is different, return main object + return output_shape; + } + + if (policy == MergePolicy::SKIP) { + return output_shape; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj_shape; + } else if (policy == MergePolicy::FUSION) { + // write down fusion method for each shape type + if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // if shape type is bounding box, merge bounding box + output_shape.dimensions.x = mean(main_obj_shape.dimensions.x, sub_obj_shape.dimensions.x); + output_shape.dimensions.y = mean(main_obj_shape.dimensions.y, sub_obj_shape.dimensions.y); + output_shape.dimensions.z = mean(main_obj_shape.dimensions.z, sub_obj_shape.dimensions.z); + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // if shape type is cylinder, merge cylinder + // (TODO) implement + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // if shape type is polygon, merge polygon + // (TODO) + return output_shape; + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in shapeMerger function." << std::endl; + return output_shape; + } + } else { + std::cerr << "unknown merge policy in shapeMerger function." << std::endl; + return output_shape; + } +} + +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; +} + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj.kinematics = objectKinematicsVXMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = objectClassificationMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = sub_obj; +} + +} // namespace merger_utils From ae04fa1900114287b83d46e6db4d06fbc7e0ae37 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 7 Dec 2023 16:48:24 +0900 Subject: [PATCH 4/4] feat(intersection): check path margin for overshoot vehicles on red light (#5394) (#1056) Co-authored-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 + .../src/debug.cpp | 6 +++ .../src/manager.cpp | 3 ++ .../src/scene_intersection.cpp | 51 ++++++++++++++++++- .../src/scene_intersection.hpp | 4 ++ .../src/util.cpp | 11 +++- .../src/util_type.hpp | 1 + 7 files changed, 74 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 8b993db11b4d9..8942c6f304b4a 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -55,6 +55,8 @@ object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index c23c03c419038..3ff0b574e90e5 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -229,6 +229,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, + 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 774ae760e6166..c24e0565f10c9 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -139,6 +139,9 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 334f19db9d380..ff0f65725085e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -1466,18 +1467,64 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stop_line > braking_distance; }; - + const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stop_line || + !target_object.stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stop_line > braking_distance) { + return false; + } + const auto stopline_front = target_object.stop_line.value().front(); + const auto stopline_back = target_object.stop_line.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; // check collision between predicted_path and ego_area bool collision_detected = false; for (const auto & target_object : target_objects->all) { const auto & object = target_object.object; // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expectedToStopBeforeStopLine(target_object)) { + expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index dde94a4e9c3be..96aad965d4be9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -118,6 +118,10 @@ class IntersectionModule : public SceneModuleInterface { double object_expected_deceleration; } ignore_on_amber_traffic_light; + struct IgnoreOnRedTrafficLight + { + double object_margin_to_path; + } ignore_on_red_traffic_light; } collision_detection; struct Occlusion { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 10db86f1e35c4..3f339f4620a6f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -840,9 +840,16 @@ IntersectionLanelets getObjectiveLanelets( result.attention_stop_lines_.push_back(stop_line); } result.attention_non_preceding_ = std::move(detection_lanelets); - // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + std::optional stop_line = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + } + result.attention_non_preceding_stop_lines_.push_back(stop_line); } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 3f09b54f88be4..7cff989a73cfc 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -50,6 +50,7 @@ struct DebugData std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons;