diff --git a/perception/radar_fusion_to_detected_object/CMakeLists.txt b/perception/radar_fusion_to_detected_object/CMakeLists.txt new file mode 100644 index 0000000000000..ec089e654a904 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_fusion_to_detected_object) + +## Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +## Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +## Targets +ament_auto_add_library(radar_object_fusion_to_detected_object_node_component SHARED + src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp + src/radar_fusion_to_detected_object.cpp +) + +rclcpp_components_register_node(radar_object_fusion_to_detected_object_node_component + PLUGIN "radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode" + EXECUTABLE radar_object_fusion_to_detected_object_node +) + +## Tests +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +## Package +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md new file mode 100644 index 0000000000000..846dec9309bb5 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/README.md @@ -0,0 +1,76 @@ +# radar_fusion_to_detected_object + +This package contains a sensor fusion module for radar-detected objects and 3D detected objects. The fusion node can: + +- Attach velocity to 3D detections when successfully matching radar data. The tracking modules use the velocity information to enhance the tracking results while planning modules use it to execute actions like adaptive cruise control. +- Improve the low confidence 3D detections when corresponding radar detections are found. + +![process_low_confidence](docs/radar_fusion_to_detected_object_6.drawio.svg) + +## Core algorithm + +The document of core algorithm is [here](docs/algorithm.md) + +### Parameters for sensor fusion + +| Name | Type | Description | Default value | +| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | + +### Weight parameters for velocity estimation + +To tune these weight parameters, please see [docs/algorithm.md](document) in detail. + +| Name | Type | Description | Default value | +| :----------------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| velocity_weight_average | double | The twist coefficient of average twist of radar data in velocity estimation. | 0.0 | +| velocity_weight_median | double | The twist coefficient of median twist of radar data in velocity estimation. | 0.0 | +| velocity_weight_min_distance | double | The twist coefficient of radar data nearest to the center of bounding box in velocity estimation. | 1.0 | +| velocity_weight_target_value_average | double | The twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. | +| 0.0 | +| velocity_weight_target_value_top | double | The twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. | 0.0 | + +### Parameters for fixed object information + +| Name | Type | Description | Default value | +| :----------------------- | :---- | :------------------------------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| convert_doppler_to_twist | bool | Convert doppler velocity to twist using the yaw information of a detected object. | false | +| threshold_probability | float | If the probability of an output object is lower than this parameter, and the output object doesn not have radar points/objects, then delete the object. | 0.4 | + +## radar_object_fusion_to_detected_object + +Sensor fusion with radar objects and a detected object. + +- Calculation cost is O(nm). + - n: the number of radar objects. + - m: the number of objects from 3d detection. + +### How to launch + +```sh +ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.launch.xml +``` + +### Input + +| Name | Type | Description | +| ----------------------- | ---------------------------------------------------- | ---------------------------------------------------------------------- | +| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. | +| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/TrackedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` | + +### Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | ------------------------------ | +| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. | + +### Parameters + +| Name | Type | Description | Default value | +| :------------- | :----- | :-------------------- | :------------ | +| update_rate_hz | double | The update rate [hz]. | 20.0 | + +## radar_scan_fusion_to_detected_object (TBD) + +TBD diff --git a/perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml b/perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml new file mode 100644 index 0000000000000..90d90dbdfa294 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + node_params: + update_rate_hz: 10.0 + + core_params: + bounding_box_margin: 2.0 + split_threshold_velocity: 5.0 + velocity_weight_average: 0.0 + velocity_weight_median: 0.0 + velocity_weight_min_distance: 1.0 + velocity_weight_target_value_average: 0.0 + velocity_weight_target_value_top: 0.0 + convert_doppler_to_twist: false + threshold_probability: 0.4 diff --git a/perception/radar_fusion_to_detected_object/docs/algorithm.md b/perception/radar_fusion_to_detected_object/docs/algorithm.md new file mode 100644 index 0000000000000..6ea4ada5a51db --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/algorithm.md @@ -0,0 +1,43 @@ +## Common Algorithm + +### 1. Link between 3d bounding box and radar data + +Choose radar pointcloud/objects within 3D bounding box from lidar-base detection with margin space from bird's-eye view. + +![choose_radar](radar_fusion_to_detected_object_1.drawio.svg) + +### 2. [Feature support] Split the object going in a different direction + +- Split two object for the low confidence object that can be estimated to derive two object. + +![process_low_confidence](radar_fusion_to_detected_object_4.drawio.svg) + +### 3. Estimate twist of object + +Estimate twist from chosen radar pointcloud/objects using twist and target value (Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects). +First, the estimation function calculate + +- Average twist for radar pointcloud/objects. +- Median twist for radar pointcloud/objects. +- Twist for radar pointcloud/objects nearest of the center of bounding box in velocity. +- Weighted average twist with target value of radar pointcloud/objects. +- Twist with max target value of radar pointcloud/objects. + +Second, the estimation function calculate weighted average of these list. +Third, twist information of estimated twist is attached to an object. + +![estimate_doppler_velocity](radar_fusion_to_detected_object_2.drawio.svg) + +### 4. [Feature support] [Option] Convert doppler velocity to twist + +If the twist information of radars is doppler velocity, convert from doppler velocity to twist using yaw angle of DetectedObject. +Because radar pointcloud has only doppler velocity information, radar pointcloud fusion should use this feature. +On the other hand, because radar objects have twist information, radar object fusion should not use this feature. + +![process_high_confidence](radar_fusion_to_detected_object_3.drawio.svg) + +### 5. Delete objects with low probability + +- Delete low confidence objects that do not have some radar points/objects. + +![process_low_confidence](radar_fusion_to_detected_object_5.drawio.svg) diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg new file mode 100644 index 0000000000000..5b1f3caddf87d --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg @@ -0,0 +1,96 @@ + + + + + + + + +
+
+
+ + + DetectedObject + +
+ +
+
+
+ + + DetectedObject + + + + + + + + + + + + + + +
+
+
+ + + 3d bounding box with param.bounding_box_margin + +
+ +
+
+
+ + + 3d bounding box with param.bounding_box_margin + + + + + + + +
+
+
+ + + Radar pointcloud +
+ or +
+ Center of gravity of radar object + +
+ +
+
+
+ + + Radar pointcloud... + + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg new file mode 100644 index 0000000000000..b719eb0619d2a --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg @@ -0,0 +1,75 @@ + + + + + + + + + +
+
+
+ + + Twist o + + + bserved from radars + +
+ +
+
+
+ + + Twist observed from radars + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + Estimated object twist + +
+ +
+
+
+ + + Estimated object twist + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg new file mode 100644 index 0000000000000..7759077396c5e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg @@ -0,0 +1,85 @@ + + + + + + + + + + +
+
+
+ + + Estimated object doppler velocity + +
+ +
+
+
+ + + Estimated object doppler velocity + + + + + + + + + +
+
+
+ + + Estimated twist + +
+ +
+
+
+ + + Estimated twist + + + + + + + + +
+
+
+ + + Yaw information from lidar-based detection + +
+ +
+
+
+ + + Yaw information from lidar-based detectio... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg new file mode 100644 index 0000000000000..8d6eb526ada8e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg @@ -0,0 +1,129 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + Split object + + +
+ +
+
+
+ + + Split object + + + + + + + + + + +
+
+
+ + + + Twist observed from radars + + +
+ +
+
+
+ + + Twist observed from radars + + + + + + + + + +
+
+
+ + + + Object velocity + + +
+ +
+
+
+ + + Object velocity + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg new file mode 100644 index 0000000000000..2133d00e2bc8e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg @@ -0,0 +1,179 @@ + + + + + + + + + + + +
+
+
+ + + + Twist observed from radars + + +
+ +
+
+
+ + + Twist observed from radars + + + + + + + + + +
+
+
+ + + + Object twist + + +
+ +
+
+
+ + + Object twist + + + + + + + +
+
+
+ + + + probability > threshold_ + + + + probability + +
+ +
+
+
+ + + probability > threshold_probability + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + probability + + + + < threshold_ + + + + probability + +
+ +
+
+
+ + + probability < threshold_probability + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + Delete obejct + + +
+ +
+
+
+ + + Delete obejct + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg new file mode 100644 index 0000000000000..a7e9f0f839c85 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg @@ -0,0 +1,532 @@ + + + + + + + + + + + + + + + + + +
+
+
+ + + + Twist observed from radars + + +
+ +
+
+
+ + + Twist observed from radars + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + +
+
+
+ + + score +
+ 0.3 + +
+ +
+
+
+ + + score... + + + + + + + + + +
+
+
+ + + score +
+ 0.3 + +
+ +
+
+
+ + + score... + + + + + + + + +
+
+
+ + + + Bounding box (BEV) + + +
+ +
+
+
+ + + Bounding box (BEV) + + + + + + + + +
+
+
+ + + Use 3d detection + +
+ +
+
+
+ + + Use 3d detection + + + + + + + +
+
+
+ + + Use radar fusion + +
+ +
+
+
+ + + Use radar fusion + + + + + + + +
+
+
+ + + + score < threshold (ex. 0.4) + + +
+ +
+
+
+ + + score < threshold (ex. 0.4) + + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + + + + + + +
+
+
+ + + score +
+ 0.3 + +
+ +
+
+
+ + + score... + + + + + + + + + +
+
+
+ + + score +
+ 0.3 + +
+ +
+
+
+ + + score... + + + + + + + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + +
+
+
+ + + score +
+ 0.5 + +
+ +
+
+
+ + + score... + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + + + + + +
+
+
+ + + + Object twist + + +
+ +
+
+
+ + + Object twist + + + + + + + +
+
+
+ + + + Improve score using radar data + + +
+ +
+
+
+ + + Improve score using radar data + + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + +
+
+
+ + + score +
+ 0.7 + +
+ +
+
+
+ + + score... + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp new file mode 100644 index 0000000000000..c3ea24c8c3e6b --- /dev/null +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp @@ -0,0 +1,108 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ + +#include "rclcpp/logger.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" +#include "geometry_msgs/msg/pose_with_covariance.hpp" +#include "geometry_msgs/msg/twist_with_covariance.hpp" +// #include "std_msgs/msg/header.hpp" + +#include +#include +#include + +namespace radar_fusion_to_detected_object +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::PoseWithCovariance; +using geometry_msgs::msg::Twist; +using geometry_msgs::msg::TwistWithCovariance; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::Point2d; + +class RadarFusionToDetectedObject +{ +public: + explicit RadarFusionToDetectedObject(const rclcpp::Logger & logger) : logger_(logger) {} + + struct Param + { + // Radar fusion param + double bounding_box_margin{}; + double split_threshold_velocity{}; + + // Weight param for velocity estimation + double velocity_weight_average{}; + double velocity_weight_median{}; + double velocity_weight_min_distance{}; + double velocity_weight_target_value_average{}; + double velocity_weight_target_value_top{}; + + // Parameters for fixed object information + bool convert_doppler_to_twist{}; + float threshold_probability{}; + }; + + struct RadarInput + { + std_msgs::msg::Header header{}; + PoseWithCovariance pose_with_covariance{}; + TwistWithCovariance twist_with_covariance{}; + double target_value{}; + }; + + struct Input + { + std::shared_ptr> radars{}; + DetectedObjects::ConstSharedPtr objects{}; + }; + + struct Output + { + DetectedObjects objects{}; + }; + + void setParam(const Param & param); + Output update(const Input & input); + +private: + rclcpp::Logger logger_; + Param param_{}; + std::shared_ptr> filterRadarWithinObject( + const DetectedObject & object, const std::shared_ptr> & radars); + // [TODO] (Satoshi Tanaka) Implement + // std::vector splitObject( + // const DetectedObject & object, const std::shared_ptr> & radars); + TwistWithCovariance estimateTwist( + const DetectedObject & object, std::shared_ptr> & radars); + bool isQualified( + const DetectedObject & object, std::shared_ptr> & radars); + TwistWithCovariance convertDopplerToTwist( + const DetectedObject & object, const TwistWithCovariance & twist_with_covariance); + Twist addTwist(const Twist & twist_1, const Twist & twist_2); + Twist scaleTwist(const Twist & twist, const double scale); + double getTwistNorm(const Twist & twist); + Twist sumTwist(const std::vector & twists); + LinearRing2d createObject2dWithMargin(const Point2d object_size, const double margin); +}; +} // namespace radar_fusion_to_detected_object + +#endif // RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ diff --git a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp new file mode 100644 index 0000000000000..45c3c4e86cc93 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp @@ -0,0 +1,90 @@ + +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#define RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ + +#include "radar_fusion_to_detected_object.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" +#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp" + +#include +#include +#include +#include + +namespace radar_fusion_to_detected_object +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node +{ +public: + explicit RadarObjectFusionToDetectedObjectNode(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + double update_rate_hz{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_object_{}; + rclcpp::Subscription::SharedPtr sub_radar_{}; + + // Callback + void onDetectedObjects(const DetectedObjects::ConstSharedPtr msg); + void onRadarObjects(const TrackedObjects::ConstSharedPtr msg); + + // Data Buffer + DetectedObjects::ConstSharedPtr detected_objects_{}; + TrackedObjects::ConstSharedPtr radar_objects_{}; + + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_{}; + + // Timer + rclcpp::TimerBase::SharedPtr timer_{}; + + bool isDataReady(); + void onTimer(); + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Parameter + NodeParam node_param_{}; + + // Core + RadarFusionToDetectedObject::Input input_{}; + RadarFusionToDetectedObject::Output output_{}; + RadarFusionToDetectedObject::Param core_param_{}; + std::unique_ptr radar_fusion_to_detected_object_{}; + + // Lapper + RadarFusionToDetectedObject::RadarInput setRadarInput( + const TrackedObject & radar_object, const std_msgs::msg::Header & header_); +}; + +} // namespace radar_fusion_to_detected_object + +#endif // RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ diff --git a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml b/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml new file mode 100644 index 0000000000000..d056434cbf322 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml new file mode 100644 index 0000000000000..3bfd47562015a --- /dev/null +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -0,0 +1,26 @@ + + + + radar_fusion_to_detected_object + 0.0.0 + radar_fusion_to_detected_object + scepter914 + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + geometry_msgs + rclcpp + rclcpp_components + std_msgs + tier4_autoware_utils + + ament_lint_common + autoware_lint_common + + + ament_cmake + + diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp new file mode 100644 index 0000000000000..1b6da03c3b689 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -0,0 +1,332 @@ + +// Copyright 2022 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 "radar_fusion_to_detected_object.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace radar_fusion_to_detected_object +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::PoseWithCovariance; +using geometry_msgs::msg::Twist; +using geometry_msgs::msg::TwistWithCovariance; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::Point2d; + +void RadarFusionToDetectedObject::setParam(const Param & param) +{ + // Radar fusion param + param_.bounding_box_margin = param.bounding_box_margin; + param_.split_threshold_velocity = param.split_threshold_velocity; + + // normalize weight param + double sum_weight = param.velocity_weight_median + param.velocity_weight_min_distance + + param.velocity_weight_average + param.velocity_weight_target_value_average + + param.velocity_weight_target_value_top; + + if (sum_weight < 0.01) { + param_.velocity_weight_min_distance = 1.0; + param_.velocity_weight_median = 0.0; + param_.velocity_weight_average = 0.0; + param_.velocity_weight_target_value_average = 0.0; + param_.velocity_weight_target_value_top = 0.0; + } else { + param_.velocity_weight_min_distance = param.velocity_weight_min_distance / sum_weight; + param_.velocity_weight_median = param.velocity_weight_median / sum_weight; + param_.velocity_weight_average = param.velocity_weight_average / sum_weight; + param_.velocity_weight_target_value_average = + param.velocity_weight_target_value_average / sum_weight; + param_.velocity_weight_target_value_top = param.velocity_weight_target_value_top / sum_weight; + } + + // Parameters for fixing object information + param_.threshold_probability = param.threshold_probability; + param_.convert_doppler_to_twist = param.convert_doppler_to_twist; +} + +RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( + const RadarFusionToDetectedObject::Input & input) +{ + RadarFusionToDetectedObject::Output output{}; + + output.objects.header = input.objects->header; + + if (!input.objects || input.objects->objects.empty()) { + return output; + } + + for (auto & object : input.objects->objects) { + // Link between 3d bounding box and radar data + std::shared_ptr> radars_within_object = + filterRadarWithinObject(object, input.radars); + + // [TODO] (Satoshi Tanaka) Implement + // Split the object going in a different direction + // std::vector split_objects = splitObject(object, radars_within_object); + std::vector split_objects; + split_objects.emplace_back(object); + + for (auto & split_object : split_objects) { + std::shared_ptr> radars_within_split_object; + if (split_objects.size() == 1) { + // If object is not split, radar data within object is same + radars_within_split_object = radars_within_object; + } else { + // If object is split, then filter radar again + radars_within_split_object = filterRadarWithinObject(object, radars_within_object); + } + + // Estimate twist of object + if (!radars_within_split_object || !(*radars_within_split_object).empty()) { + split_object.kinematics.has_twist = true; + split_object.kinematics.twist_with_covariance = + estimateTwist(split_object, radars_within_split_object); + } + + // Delete objects with low probability + if (isQualified(split_object, radars_within_split_object)) { + split_object.classification.at(0).probability = + std::max(split_object.classification.at(0).probability, param_.threshold_probability); + output.objects.objects.emplace_back(split_object); + } + } + } + return output; +} + +// Choose radar pointcloud/objects within 3D bounding box from lidar-base detection with margin +// space from bird's-eye view. +std::shared_ptr> +RadarFusionToDetectedObject::filterRadarWithinObject( + const DetectedObject & object, + const std::shared_ptr> & radars) +{ + std::vector outputs{}; + + tier4_autoware_utils::Point2d object_size{object.shape.dimensions.x, object.shape.dimensions.y}; + LinearRing2d object_box = createObject2dWithMargin(object_size, param_.bounding_box_margin); + object_box = tier4_autoware_utils::transformVector( + object_box, tier4_autoware_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); + + for (const auto & radar : (*radars)) { + Point2d radar_point{ + radar.pose_with_covariance.pose.position.x, radar.pose_with_covariance.pose.position.y}; + if (boost::geometry::within(radar_point, object_box)) { + outputs.emplace_back(radar); + } + } + return std::make_shared>(outputs); +} + +// [TODO] (Satoshi Tanaka) Implementation +// std::vector RadarFusionToDetectedObject::splitObject( +// const DetectedObject & object, const std::vector & radars) +// { +// std::vector output{}; +// return output; +// } + +// Estimate twist from chosen radar pointcloud/objects using twist and target value +// (Target value is amplitude if using radar pointcloud. Target value is probability if using radar +// objects). +TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( + const DetectedObject & object, std::shared_ptr> & radars) +{ + TwistWithCovariance twist_with_covariance{}; + if (!radars || (*radars).empty()) { + return twist_with_covariance; + } + + // calculate twist for radar data with min distance + Twist twist_min_distance{}; + if (param_.velocity_weight_min_distance > 0.0) { + auto comp_func = [&](const RadarInput & a, const RadarInput & b) { + return tier4_autoware_utils::calcSquaredDistance2d( + a.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.position) < + tier4_autoware_utils::calcSquaredDistance2d( + b.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.position); + }; + auto iter = std::min_element(std::begin(*radars), std::end(*radars), comp_func); + twist_min_distance = iter->twist_with_covariance.twist; + } + + // calculate twist for radar data with median twist + Twist twist_median{}; + if (param_.velocity_weight_median > 0.0) { + auto ascending_func = [&](const RadarInput & a, const RadarInput & b) { + return getTwistNorm(a.twist_with_covariance.twist) < + getTwistNorm(b.twist_with_covariance.twist); + }; + std::sort((*radars).begin(), (*radars).end(), ascending_func); + + if ((*radars).size() % 2 == 1) { + int median_index = ((*radars).size() - 1) / 2; + twist_median = (*radars).at(median_index).twist_with_covariance.twist; + } else { + int median_index = (*radars).size() / 2; + + twist_median = scaleTwist( + addTwist( + (*radars).at(median_index - 1).twist_with_covariance.twist, + (*radars).at(median_index).twist_with_covariance.twist), + 0.5); + } + } + + // calculate twist for radar data with average twist + Twist twist_average{}; + if (param_.velocity_weight_average > 0.0) { + for (const auto & radar : (*radars)) { + twist_average = addTwist(twist_average, radar.twist_with_covariance.twist); + } + twist_average = scaleTwist(twist_average, (1.0 / (*radars).size())); + } + + // calculate twist for radar data with top target value + Twist twist_top_target_value{}; + if (param_.velocity_weight_target_value_top > 0.0) { + auto comp_func = [](const RadarInput & a, const RadarInput & b) { + return a.target_value < b.target_value; + }; + auto iter = std::max_element(std::begin((*radars)), std::end((*radars)), comp_func); + twist_top_target_value = iter->twist_with_covariance.twist; + } + + // calculate twist for radar data with target_value * average + Twist twist_target_value_average{}; + double sum_target_value = 0.0; + if (param_.velocity_weight_target_value_average > 0.0) { + for (const auto & radar : (*radars)) { + twist_target_value_average = scaleTwist( + addTwist(twist_target_value_average, radar.twist_with_covariance.twist), + radar.target_value); + sum_target_value += radar.target_value; + } + twist_target_value_average = scaleTwist(twist_target_value_average, 1.0 / sum_target_value); + } + + // estimate doppler velocity with cost weight + std::vector weight_twists{}; + weight_twists.emplace_back(scaleTwist(twist_min_distance, param_.velocity_weight_min_distance)); + weight_twists.emplace_back(scaleTwist(twist_median, param_.velocity_weight_median)); + weight_twists.emplace_back(scaleTwist(twist_average, param_.velocity_weight_average)); + weight_twists.emplace_back( + scaleTwist(twist_top_target_value, param_.velocity_weight_target_value_top)); + weight_twists.emplace_back( + scaleTwist(twist_target_value_average, param_.velocity_weight_target_value_average)); + + twist_with_covariance.twist = sumTwist(weight_twists); + + // [TODO] (Satoshi Tanaka) Implement + // Convert doppler velocity to twist + // if (param_.convert_doppler_to_twist) { + // twist_with_covariance = convertDopplerToTwist(object, twist_with_covariance); + // } + return twist_with_covariance; +} + +// Jugde wether low confidence objects that do not have some radar points/objects or not. +bool RadarFusionToDetectedObject::isQualified( + const DetectedObject & object, std::shared_ptr> & radars) +{ + if (object.classification[0].probability > param_.threshold_probability) { + return true; + } else { + if (!radars || !(*radars).empty()) { + return true; + } else { + return false; + } + } +} + +// [TODO] (Satoshi Tanaka) Implement for radar pointcloud fusion +// TwistWithCovariance RadarFusionToDetectedObject::convertDopplerToTwist( +// const DetectedObject & object, const TwistWithCovariance & twist_with_covariance) +// { +// return twist_with_covariance; +// } + +Twist RadarFusionToDetectedObject::addTwist(const Twist & twist_1, const Twist & twist_2) +{ + Twist output{}; + output.linear.x = twist_1.linear.x + twist_2.linear.x; + output.linear.y = twist_1.linear.y + twist_2.linear.y; + output.linear.z = twist_1.linear.z + twist_2.linear.z; + output.angular.x = twist_1.angular.x + twist_2.angular.x; + output.angular.y = twist_1.angular.y + twist_2.angular.y; + output.angular.z = twist_1.angular.z + twist_2.angular.z; + return output; +} + +Twist RadarFusionToDetectedObject::scaleTwist(const Twist & twist, const double scale) +{ + Twist output{}; + output.linear.x = twist.linear.x * scale; + output.linear.y = twist.linear.y * scale; + output.linear.z = twist.linear.z * scale; + output.angular.x = twist.angular.x * scale; + output.angular.y = twist.angular.y * scale; + output.angular.z = twist.angular.z * scale; + return output; +} + +double RadarFusionToDetectedObject::getTwistNorm(const Twist & twist) +{ + double output = std::sqrt( + twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y + + twist.linear.z * twist.linear.z); + return output; +} + +Twist RadarFusionToDetectedObject::sumTwist(const std::vector & twists) +{ + Twist output{}; + for (const auto & twist : twists) { + output = addTwist(output, twist); + } + return output; +} + +LinearRing2d RadarFusionToDetectedObject::createObject2dWithMargin( + const Point2d object_size, const double margin) +{ + const double x_front = object_size.x() / 2.0 + margin; + const double x_rear = -object_size.x() / 2.0 - margin; + const double y_left = object_size.y() / 2.0 + margin; + const double y_right = -object_size.y() / 2.0 - margin; + + LinearRing2d box{}; + box.push_back(Point2d{x_front, y_left}); + box.push_back(Point2d{x_front, y_right}); + box.push_back(Point2d{x_rear, y_right}); + box.push_back(Point2d{x_rear, y_left}); + box.push_back(Point2d{x_front, y_left}); + + return box; +} +} // namespace radar_fusion_to_detected_object diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp new file mode 100644 index 0000000000000..2ee4e24ff98cf --- /dev/null +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -0,0 +1,226 @@ + +// Copyright 2022 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 "radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include + +using namespace std::literals; +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; +using std::placeholders::_1; + +namespace +{ +template +bool update_param( + const std::vector & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; + } + + value = itr->template get_value(); + return true; +} +} // namespace + +namespace radar_fusion_to_detected_object +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( + const rclcpp::NodeOptions & node_options) +: Node("radar_object_fusion_to_detected_object", node_options) +{ + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, _1)); + + // Node Parameter + node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz", 10.0); + + // Core Parameter + core_param_.bounding_box_margin = + declare_parameter("core_params.bounding_box_margin", 0.5); + core_param_.split_threshold_velocity = + declare_parameter("core_params.split_threshold_velocity", 0.0); + core_param_.velocity_weight_average = + declare_parameter("core_params.velocity_weight_average", 0.0); + core_param_.velocity_weight_median = + declare_parameter("core_params.velocity_weight_median", 0.0); + core_param_.velocity_weight_target_value_average = + declare_parameter("core_params.velocity_weight_target_value_average", 0.0); + core_param_.velocity_weight_target_value_top = + declare_parameter("core_params.velocity_weight_target_value_top", 1.0); + core_param_.convert_doppler_to_twist = + declare_parameter("core_params.convert_doppler_to_twist", false); + core_param_.threshold_probability = + declare_parameter("core_params.threshold_probability", 0.0); + + // Core + radar_fusion_to_detected_object_ = std::make_unique(get_logger()); + radar_fusion_to_detected_object_->setParam(core_param_); + + // Subscriber + sub_object_ = create_subscription( + "~/input/objects", rclcpp::QoS{1}, + std::bind(&RadarObjectFusionToDetectedObjectNode::onDetectedObjects, this, _1)); + sub_radar_ = create_subscription( + "~/input/radars", rclcpp::QoS{1}, + std::bind(&RadarObjectFusionToDetectedObjectNode::onRadarObjects, this, _1)); + + // Publisher + pub_objects_ = create_publisher("~/output/objects", 1); + + // Timer + const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, + std::bind(&RadarObjectFusionToDetectedObjectNode::onTimer, this)); +} + +void RadarObjectFusionToDetectedObjectNode::onDetectedObjects( + const DetectedObjects::ConstSharedPtr msg) +{ + detected_objects_ = msg; +} +void RadarObjectFusionToDetectedObjectNode::onRadarObjects(const TrackedObjects::ConstSharedPtr msg) +{ + radar_objects_ = msg; +} + +rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + + try { + // Node Parameter + { + auto & p = node_param_; + + // Update params + update_param(params, "node_params.update_rate_hz", p.update_rate_hz); + } + + // Core Parameter + { + // Copy to local variable + auto & p = core_param_; + + // Update params + update_param(params, "core_params.bounding_box_margin", p.bounding_box_margin); + update_param(params, "core_params.split_threshold_velocity", p.split_threshold_velocity); + update_param(params, "core_params.velocity_weight_average", p.velocity_weight_average); + update_param(params, "core_params.velocity_weight_median", p.velocity_weight_median); + update_param( + params, "core_params.velocity_weight_target_value_average", + p.velocity_weight_target_value_average); + update_param( + params, "core_params.velocity_weight_target_value_top", p.velocity_weight_target_value_top); + + // Set parameter to instance + if (radar_fusion_to_detected_object_) { + radar_fusion_to_detected_object_->setParam(core_param_); + } + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + + result.successful = true; + result.reason = "success"; + return result; +} + +bool RadarObjectFusionToDetectedObjectNode::isDataReady() +{ + if (!detected_objects_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 1000, "waiting for detected objects data msg..."); + return false; + } + if (!radar_objects_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for radar objects data msg..."); + return false; + } + + if (detected_objects_->header.frame_id != radar_objects_->header.frame_id) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "The frame id between detected objects and radar objects is not same"); + return false; + } + + return true; +} + +void RadarObjectFusionToDetectedObjectNode::onTimer() +{ + if (!isDataReady()) { + return; + } + + if (radar_objects_->objects.empty()) { + pub_objects_->publish(*detected_objects_); + return; + } + + // Set input data + RadarFusionToDetectedObject::Input input{}; + std::vector radars_{}; + for (const auto & radar_object_ : radar_objects_->objects) { + auto radar_input = setRadarInput(radar_object_, radar_objects_->header); + radars_.emplace_back(radar_input); + } + input.objects = detected_objects_; + input.radars = std::make_shared>(radars_); + + // Update + output_ = radar_fusion_to_detected_object_->update(input); + pub_objects_->publish(output_.objects); +} + +RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::setRadarInput( + const TrackedObject & radar_object, const std_msgs::msg::Header & header_) +{ + RadarFusionToDetectedObject::RadarInput output{}; + output.pose_with_covariance = radar_object.kinematics.pose_with_covariance; + output.twist_with_covariance = radar_object.kinematics.twist_with_covariance; + output.target_value = radar_object.classification.at(0).probability; + output.header = header_; + return output; +} + +} // namespace radar_fusion_to_detected_object + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode)