From 259a39a608730ef91d4782bb36c0d717cdf5e286 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Wed, 1 Jun 2022 10:46:04 +0900 Subject: [PATCH 01/18] add radar_fusion_to_detected_object Signed-off-by: scepter914 --- .../CMakeLists.txt | 43 ++ .../radar_fusion_to_detected_object/README.md | 78 +++ ...bject_fusion_to_detected_object.param.yaml | 15 + .../docs/algorithm.md | 34 ++ ...dar_fusion_to_detected_object_1.drawio.svg | 96 ++++ ...dar_fusion_to_detected_object_2.drawio.svg | 75 +++ ...dar_fusion_to_detected_object_3.drawio.svg | 85 +++ ...dar_fusion_to_detected_object_4.drawio.svg | 129 +++++ ...dar_fusion_to_detected_object_5.drawio.svg | 179 ++++++ ...dar_fusion_to_detected_object_6.drawio.svg | 532 ++++++++++++++++++ .../radar_fusion_to_detected_object.hpp | 106 ++++ ..._object_fusion_to_detected_object_node.hpp | 90 +++ ...bject_fusion_to_detected_object.launch.xml | 17 + .../package.xml | 31 + .../src/radar_fusion_to_detected_object.cpp | 312 ++++++++++ ..._object_fusion_to_detected_object_node.cpp | 221 ++++++++ 16 files changed, 2043 insertions(+) create mode 100644 perception/radar_fusion_to_detected_object/CMakeLists.txt create mode 100644 perception/radar_fusion_to_detected_object/README.md create mode 100644 perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml create mode 100644 perception/radar_fusion_to_detected_object/docs/algorithm.md create mode 100644 perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg create mode 100644 perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg create mode 100644 perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg create mode 100644 perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg create mode 100644 perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg create mode 100644 perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg create mode 100644 perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp create mode 100644 perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp create mode 100644 perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml create mode 100644 perception/radar_fusion_to_detected_object/package.xml create mode 100644 perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp create mode 100644 perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp 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..f279380f46204 --- /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(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +## 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..237474ffde7d9 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/README.md @@ -0,0 +1,78 @@ +# radar_fusion_to_detected_object + +This package is radar-based sensor fusion module to 3d detected object. +Main feature of this package are as following. + +- Attach velocity to lidar detection result from radar data to improve for tracking result and planning like adaptive cruise. +- Improve detection result with radar sensor information. If both lidar 3d detected objects with low score and high confidence of radar pointcloud / objects, then improve score of objects. + +![process_low_confidence](docs/radar_fusion_to_detected_object_6.drawio.svg) + +## Core algorithm + +The document of core algorithm is [here](docs/algorithm.md) + +### Parameter + +#### Radar fusion param + +- bounding_box_margin (double): The margin distance from bird's-eye view to choose radar pointcloud/objects within 3D bounding box [m] + - Default parameter is 2.0 +- double split_threshold_velocity (double): The threshold velocity to judge splitting two objects from radar information (now unused) [m/s] + - Default parameter is 5.0 + +#### Weight param for velocity estimation + +- velocity_weight_average (double): The twist coefficient of average twist of radar data in velocity estimation. + - Default parameter is 0.0 (= no use) +- velocity_weight_median (double): The twist coefficient of median twist of radar data in velocity estimation. + - Default parameter is 0.0 (= no use) +- velocity_weight_min_distance (double): The twist coefficient of nearest radar data to the center of bounding box in velocity estimation. + - Default parameter is 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. + - Default parameter is 0.0 (= no use) +- 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. + - Default parameter is 0.0 (= no use) + +#### Parameters for fixed object information + +- convert_doppler_to_twist (bool): Convert doppler velocity to twist using the yaw information of a detected object. + - Default parameter is "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. + - Default parameter is 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 +roslaunch radar_fusion_to_detected_object radar_object_to_detected_object.launch +``` + +### Input / Output + +- Input + - `~/input/objects` (autoware_auto_perception_msgs/msg/DetectedObject.msg): 3D detected objects. + - `~/input/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObjects.msg): Radar objects +- Output + - `~/output/objects` (autoware_auto_perception_msgs/msg/DetectedObjects.msg): 3D detected object with twist. + +### Parameters + +- `update_rate_hz` (double): The update rate [hz]. + - Default parameter is 20.0 +- Core algorithm parameter + +## 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..90576ee43c44d --- /dev/null +++ b/perception/radar_fusion_to_detected_object/docs/algorithm.md @@ -0,0 +1,34 @@ + +## 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. +Attach object to twist information of estimated twist. + +![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..980105eaaa822 --- /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 +
+ Centor 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..650bc5b8e423f --- /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..8b12af043efa4 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp @@ -0,0 +1,106 @@ +// 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::vector radars = {}; + DetectedObjects objects{}; + }; + + struct Output + { + DetectedObjects objects{}; + }; + + void setParam(const Param & param); + Output update(const Input & input); + +private: + rclcpp::Logger logger_; + Param param_{}; + std::vector filterRadarWithinObject( + const DetectedObject & object, const std::vector & radars); + std::vector splitObject( + const DetectedObject & object, const std::vector & radars); + TwistWithCovariance estimateTwist( + const DetectedObject & object, std::vector & radars); + bool isQualified(const DetectedObject & object, const std::vector & 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 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..a480d0ae15ba3 --- /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_FUSION_TO_DETECTED_OBJECT_NODE_HPP__ +#define RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_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( + TrackedObject radar_object, std_msgs::msg::Header header); +}; + +} // namespace radar_fusion_to_detected_object + +#endif 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..0bf230f6de0bf --- /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..8feb0f5d739f9 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -0,0 +1,31 @@ + + + + radar_fusion_to_detected_object + 0.0.0 + radar_fusion_to_detected_object + scepter914 + Apache License 2.0 + + ament_cmake_auto + + rclcpp + rclcpp_components + + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + std_msgs + + autoware_auto_perception_msgs + tier4_autoware_utils + + ament_clang_format + ament_lint_auto + ament_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..944f67b645e65 --- /dev/null +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -0,0 +1,312 @@ + +// 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 + +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; + + for (const auto & object : input.objects.objects) { + // Link between 3d bounding box and radar data + std::vector radars_within_object = filterRadarWithinObject(object, input.radars); + + // Split the object going in a different direction + std::vector split_objects = splitObject(object, radars_within_object); + + for (auto & split_object : split_objects) { + std::vector 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 + 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; +} + +std::vector +RadarFusionToDetectedObject::filterRadarWithinObject( + const DetectedObject & object, const std::vector & 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 outputs; +} + +std::vector RadarFusionToDetectedObject::splitObject( + const DetectedObject & object, const std::vector & radars) +{ + // [TODO] Implementation + std::vector output{}; + output.emplace_back(object); + return output; +} + +TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( + const DetectedObject & object, std::vector & radars) +{ + TwistWithCovariance twist_with_covariance{}; + if (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); + + // Convert doppler velocity to twist + if (param_.convert_doppler_to_twist) { + twist_with_covariance = convertDopplerToTwist(object, twist_with_covariance); + } + return twist_with_covariance; +} + +bool RadarFusionToDetectedObject::isQualified( + const DetectedObject & object, const std::vector & radars) +{ + if (object.classification[0].probability > param_.threshold_probability) { + return true; + } else { + if (!radars.empty()) { + return true; + } else { + return false; + } + } +} + +TwistWithCovariance RadarFusionToDetectedObject::convertDopplerToTwist( + const DetectedObject & object, const TwistWithCovariance & twist_with_covariance) +{ + // [TODO] (Satoshi Tanaka) Implement for radar pointcloud fusion + std::cout << "debug" << object.classification.at(0).probability << std::endl; + 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..007bd15290c24 --- /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,221 @@ + +// 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" + +using namespace std::literals; +using namespace std::placeholders; +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +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 + { + // Copy to local variable + auto p = node_param_; + + // Update params + update_param(params, "node_params.update_rate_hz", p.update_rate_hz); + + // Copy back to member variable + node_param_ = p; + } + + // 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); + + // Copy back to member variable + core_param_ = p; + + // 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; + } + + // Set input data + RadarFusionToDetectedObject::Input input{}; + input.objects = *detected_objects_; + for (const auto & radar_object_ : radar_objects_->objects) { + input.radars.emplace_back(setRadarInput(radar_object_, radar_objects_->header)); + } + + // Update + output_ = radar_fusion_to_detected_object_->update(input); + pub_objects_->publish(output_.objects); +} + +RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::setRadarInput( + TrackedObject radar_object, std_msgs::msg::Header header) +{ + RadarFusionToDetectedObject::RadarInput output{}; + output.header = header; + 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; + 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) From c5a22eea10f7890ca39000b9529eebb8546bb153 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Wed, 1 Jun 2022 10:47:22 +0900 Subject: [PATCH 02/18] apply pre-commit Signed-off-by: scepter914 --- perception/radar_fusion_to_detected_object/README.md | 6 +++--- .../docs/algorithm.md | 4 ++-- .../include/radar_fusion_to_detected_object.hpp | 6 +++--- .../radar_object_fusion_to_detected_object_node.hpp | 6 +++--- ...radar_object_fusion_to_detected_object.launch.xml | 8 ++++---- .../radar_fusion_to_detected_object/package.xml | 12 ++++++------ 6 files changed, 21 insertions(+), 21 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index 237474ffde7d9..59cb88403305b 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -62,10 +62,10 @@ roslaunch radar_fusion_to_detected_object radar_object_to_detected_object.launch ### Input / Output - Input - - `~/input/objects` (autoware_auto_perception_msgs/msg/DetectedObject.msg): 3D detected objects. - - `~/input/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObjects.msg): Radar objects + - `~/input/objects` (autoware_auto_perception_msgs/msg/DetectedObject.msg): 3D detected objects. + - `~/input/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObjects.msg): Radar objects - Output - - `~/output/objects` (autoware_auto_perception_msgs/msg/DetectedObjects.msg): 3D detected object with twist. + - `~/output/objects` (autoware_auto_perception_msgs/msg/DetectedObjects.msg): 3D detected object with twist. ### Parameters diff --git a/perception/radar_fusion_to_detected_object/docs/algorithm.md b/perception/radar_fusion_to_detected_object/docs/algorithm.md index 90576ee43c44d..01f5c59c51a80 100644 --- a/perception/radar_fusion_to_detected_object/docs/algorithm.md +++ b/perception/radar_fusion_to_detected_object/docs/algorithm.md @@ -1,5 +1,5 @@ - ## 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. @@ -27,7 +27,7 @@ On the other hand, because radar objects have twist information, radar object fu ![process_high_confidence](radar_fusion_to_detected_object_3.drawio.svg) -### 5. Delete objects with low probability. +### 5. Delete objects with low probability - Delete low confidence objects that do not have some radar points/objects. 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 index 8b12af043efa4..b7aed0313d2cf 100644 --- 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 @@ -12,8 +12,8 @@ // 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__ +#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" @@ -103,4 +103,4 @@ class RadarFusionToDetectedObject }; } // namespace radar_fusion_to_detected_object -#endif +#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 index a480d0ae15ba3..6f06e6d821f04 100644 --- 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 @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP__ -#define RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP__ +#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" @@ -87,4 +87,4 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node } // namespace radar_fusion_to_detected_object -#endif +#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 index 0bf230f6de0bf..d056434cbf322 100644 --- 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 @@ -1,11 +1,11 @@ - - + + - + - + diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 8feb0f5d739f9..d6b8fd8828fa9 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -9,18 +9,18 @@ ament_cmake_auto + autoware_auto_perception_msgs + geometry_msgs rclcpp rclcpp_components - + std_msgs tf2 - tf2_ros tf2_geometry_msgs - geometry_msgs - std_msgs - - autoware_auto_perception_msgs + tf2_ros tier4_autoware_utils + + ament_clang_format ament_lint_auto ament_lint_common From 46c47df14f95eac23dd1452634969dd56903c4f0 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Wed, 1 Jun 2022 10:49:41 +0900 Subject: [PATCH 03/18] fix namespace for cpplint Signed-off-by: scepter914 --- perception/radar_fusion_to_detected_object/package.xml | 2 -- .../radar_object_fusion_to_detected_object_node.cpp | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index d6b8fd8828fa9..028f139053399 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -19,8 +19,6 @@ tf2_ros tier4_autoware_utils - - ament_clang_format ament_lint_auto ament_lint_common 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 index 007bd15290c24..27e2b0ffc1364 100644 --- 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 @@ -18,10 +18,10 @@ #include "rclcpp/rclcpp.hpp" using namespace std::literals; -using namespace std::placeholders; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; +using std::placeholders::_1; namespace { From b62e0d0015c0ac185a6c062035753faeba236600 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 2 Jun 2022 13:06:51 +0900 Subject: [PATCH 04/18] fix compile error on autoware cmake Signed-off-by: scepter914 --- perception/radar_fusion_to_detected_object/CMakeLists.txt | 4 ++-- perception/radar_fusion_to_detected_object/package.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/CMakeLists.txt b/perception/radar_fusion_to_detected_object/CMakeLists.txt index f279380f46204..ec089e654a904 100644 --- a/perception/radar_fusion_to_detected_object/CMakeLists.txt +++ b/perception/radar_fusion_to_detected_object/CMakeLists.txt @@ -13,8 +13,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() ## Dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() ## Targets ament_auto_add_library(radar_object_fusion_to_detected_object_node_component SHARED diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 028f139053399..06ca7b644f0a5 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -8,6 +8,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs geometry_msgs @@ -19,9 +20,8 @@ tf2_ros tier4_autoware_utils - ament_clang_format - ament_lint_auto ament_lint_common + autoware_lint_common ament_cmake From f502f2157c69eb961da7f1d51296ad806ebb28d4 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 2 Jun 2022 13:27:03 +0900 Subject: [PATCH 05/18] suppress warning Signed-off-by: scepter914 --- .../src/radar_fusion_to_detected_object.cpp | 40 ++++++++++--------- 1 file changed, 21 insertions(+), 19 deletions(-) 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 index 944f67b645e65..4c5987d6e399a 100644 --- 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 @@ -73,8 +73,11 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( // Link between 3d bounding box and radar data std::vector 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 = splitObject(object, radars_within_object); + std::vector split_objects; + split_objects.emplace_back(object); for (auto & split_object : split_objects) { std::vector radars_within_split_object; @@ -123,14 +126,13 @@ RadarFusionToDetectedObject::filterRadarWithinObject( return outputs; } -std::vector RadarFusionToDetectedObject::splitObject( - const DetectedObject & object, const std::vector & radars) -{ - // [TODO] Implementation - std::vector output{}; - output.emplace_back(object); - return output; -} +// [TODO] (Satoshi Tanaka) Implementation +// std::vector RadarFusionToDetectedObject::splitObject( +// const DetectedObject & object, const std::vector & radars) +// { +// std::vector output{}; +// return output; +// } TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( const DetectedObject & object, std::vector & radars) @@ -222,10 +224,11 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( 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); - } + // if (param_.convert_doppler_to_twist) { + // twist_with_covariance = convertDopplerToTwist(object, twist_with_covariance); + // } return twist_with_covariance; } @@ -243,13 +246,12 @@ bool RadarFusionToDetectedObject::isQualified( } } -TwistWithCovariance RadarFusionToDetectedObject::convertDopplerToTwist( - const DetectedObject & object, const TwistWithCovariance & twist_with_covariance) -{ - // [TODO] (Satoshi Tanaka) Implement for radar pointcloud fusion - std::cout << "debug" << object.classification.at(0).probability << std::endl; - return twist_with_covariance; -} +// [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) { From 2e2c743c4ccbc37cf6352cc67984d53c2f579e44 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 10 Jun 2022 10:39:51 +0900 Subject: [PATCH 06/18] fix add twist condition Signed-off-by: yutaka --- .../src/radar_fusion_to_detected_object.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 index 4c5987d6e399a..b41d0b94d95ef 100644 --- 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 @@ -90,9 +90,11 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( } // Estimate twist of object - split_object.kinematics.has_twist = true; - split_object.kinematics.twist_with_covariance = - estimateTwist(split_object, radars_within_split_object); + if(!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)) { From 587322440bc5d396acaffb2fe9db3a8b6b02ed18 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jun 2022 01:41:32 +0000 Subject: [PATCH 07/18] ci(pre-commit): autofix --- .../src/radar_fusion_to_detected_object.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index b41d0b94d95ef..47370bf61fabb 100644 --- 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 @@ -90,7 +90,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( } // Estimate twist of object - if(!radars_within_split_object.empty()) { + if (!radars_within_split_object.empty()) { split_object.kinematics.has_twist = true; split_object.kinematics.twist_with_covariance = estimateTwist(split_object, radars_within_split_object); From 3f84c2aef64c1b3c6886bcb5fbe62c45ede4e6ba Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 10 Jun 2022 10:43:04 +0900 Subject: [PATCH 08/18] fix format Signed-off-by: yutaka --- .../src/radar_fusion_to_detected_object.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index b41d0b94d95ef..47370bf61fabb 100644 --- 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 @@ -90,7 +90,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( } // Estimate twist of object - if(!radars_within_split_object.empty()) { + if (!radars_within_split_object.empty()) { split_object.kinematics.has_twist = true; split_object.kinematics.twist_with_covariance = estimateTwist(split_object, radars_within_split_object); From d746edd5d3d28b93a2774c77b1c051415aaa66c5 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 20 Jun 2022 13:34:52 +0900 Subject: [PATCH 09/18] add include path Signed-off-by: scepter914 --- .../radar_object_fusion_to_detected_object_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 index 27e2b0ffc1364..c5021645758a2 100644 --- 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 @@ -17,6 +17,10 @@ #include "rclcpp/rclcpp.hpp" +#include +#include +#include + using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; From 27391123d6323282d5ae55ee10c578252398788b Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 20 Jun 2022 14:27:55 +0900 Subject: [PATCH 10/18] fix for CTest Signed-off-by: scepter914 --- .../src/radar_fusion_to_detected_object.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 index 47370bf61fabb..cf416d00683a7 100644 --- 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 @@ -19,7 +19,10 @@ #include #include +#include #include +#include +#include namespace radar_fusion_to_detected_object { From 85e559d362451876c7d9bde5ea9e91e9b722d81c Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 20 Jun 2022 16:06:39 +0900 Subject: [PATCH 11/18] delete paramter copy Signed-off-by: scepter914 --- .../radar_object_fusion_to_detected_object_node.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) 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 index c5021645758a2..9683773bc2054 100644 --- 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 @@ -122,20 +122,16 @@ rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode:: try { // Node Parameter { - // Copy to local variable - auto p = node_param_; + auto & p = node_param_; // Update params update_param(params, "node_params.update_rate_hz", p.update_rate_hz); - - // Copy back to member variable - node_param_ = p; } // Core Parameter { // Copy to local variable - auto p = core_param_; + auto & p = core_param_; // Update params update_param(params, "core_params.bounding_box_margin", p.bounding_box_margin); @@ -148,9 +144,6 @@ rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode:: update_param( params, "core_params.velocity_weight_target_value_top", p.velocity_weight_target_value_top); - // Copy back to member variable - core_param_ = p; - // Set parameter to instance if (radar_fusion_to_detected_object_) { radar_fusion_to_detected_object_->setParam(core_param_); From 9a822b660790ac26cf87d416137a9c70c808448c Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 23 Jun 2022 11:40:47 +0900 Subject: [PATCH 12/18] update README Signed-off-by: scepter914 --- .../radar_fusion_to_detected_object/README.md | 80 +++++++++---------- 1 file changed, 39 insertions(+), 41 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index 59cb88403305b..c79476dcf18db 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -12,38 +12,31 @@ Main feature of this package are as following. The document of core algorithm is [here](docs/algorithm.md) -### Parameter - -#### Radar fusion param - -- bounding_box_margin (double): The margin distance from bird's-eye view to choose radar pointcloud/objects within 3D bounding box [m] - - Default parameter is 2.0 -- double split_threshold_velocity (double): The threshold velocity to judge splitting two objects from radar information (now unused) [m/s] - - Default parameter is 5.0 - -#### Weight param for velocity estimation - -- velocity_weight_average (double): The twist coefficient of average twist of radar data in velocity estimation. - - Default parameter is 0.0 (= no use) -- velocity_weight_median (double): The twist coefficient of median twist of radar data in velocity estimation. - - Default parameter is 0.0 (= no use) -- velocity_weight_min_distance (double): The twist coefficient of nearest radar data to the center of bounding box in velocity estimation. - - Default parameter is 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. - - Default parameter is 0.0 (= no use) -- 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. - - Default parameter is 0.0 (= no use) - -#### Parameters for fixed object information - -- convert_doppler_to_twist (bool): Convert doppler velocity to twist using the yaw information of a detected object. - - Default parameter is "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. - - Default parameter is 0.4 +### Parameters for sensor fusion + +| Name | Type | Description | Default value | +| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The margin distance from bird's-eye view to choose radar pointcloud/objects within 3D bounding box [m] | 2.0 | +| split_threshold_velocity | double | The threshold velocity to judge splitting two objects from radar information (now unused) [m/s] | 5.0 | + + +### Weight parameters for velocity estimation + +| 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 nearest radar data 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 @@ -59,19 +52,24 @@ Sensor fusion with radar objects and a detected object. roslaunch radar_fusion_to_detected_object radar_object_to_detected_object.launch ``` -### Input / Output +### 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. | + +### Output -- Input - - `~/input/objects` (autoware_auto_perception_msgs/msg/DetectedObject.msg): 3D detected objects. - - `~/input/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObjects.msg): Radar objects -- Output - - `~/output/objects` (autoware_auto_perception_msgs/msg/DetectedObjects.msg): 3D detected object with twist. +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | ------------------------------ | +| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. | ### Parameters -- `update_rate_hz` (double): The update rate [hz]. - - Default parameter is 20.0 -- Core algorithm parameter +| Name | Type | Description | Default value | +| :------------- | :----- | :-------------------- | :------------ | +| update_rate_hz | double | The update rate [hz]. | 20.0 | ## radar_scan_fusion_to_detected_object (TBD) From 5ba72b15104c594e1a772d671d4a34c455c07452 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 23 Jun 2022 11:41:19 +0900 Subject: [PATCH 13/18] update README Signed-off-by: scepter914 --- perception/radar_fusion_to_detected_object/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index c79476dcf18db..bfcafd7398e5d 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -19,7 +19,6 @@ The document of core algorithm is [here](docs/algorithm.md) | bounding_box_margin | double | The margin distance from bird's-eye view to choose radar pointcloud/objects within 3D bounding box [m] | 2.0 | | split_threshold_velocity | double | The threshold velocity to judge splitting two objects from radar information (now unused) [m/s] | 5.0 | - ### Weight parameters for velocity estimation | Name | Type | Description | Default value | From c8a936c536b64e7e80e8821db6cd9bd00c785a6d Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 30 Jun 2022 11:51:13 +0900 Subject: [PATCH 14/18] fix to smart ptr Signed-off-by: scepter914 --- .../radar_fusion_to_detected_object/README.md | 29 +++---- .../docs/algorithm.md | 13 +++- ...dar_fusion_to_detected_object_1.drawio.svg | 12 +-- ...dar_fusion_to_detected_object_2.drawio.svg | 44 +++++------ .../radar_fusion_to_detected_object.hpp | 15 ++-- ..._object_fusion_to_detected_object_node.hpp | 4 +- .../package.xml | 3 - .../src/radar_fusion_to_detected_object.cpp | 78 +++++++++++-------- ..._object_fusion_to_detected_object_node.cpp | 21 +++-- 9 files changed, 123 insertions(+), 96 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index bfcafd7398e5d..846dec9309bb5 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -1,10 +1,9 @@ # radar_fusion_to_detected_object -This package is radar-based sensor fusion module to 3d detected object. -Main feature of this package are as following. +This package contains a sensor fusion module for radar-detected objects and 3D detected objects. The fusion node can: -- Attach velocity to lidar detection result from radar data to improve for tracking result and planning like adaptive cruise. -- Improve detection result with radar sensor information. If both lidar 3d detected objects with low score and high confidence of radar pointcloud / objects, then improve score of objects. +- 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) @@ -14,18 +13,20 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The margin distance from bird's-eye view to choose radar pointcloud/objects within 3D bounding box [m] | 2.0 | -| split_threshold_velocity | double | The threshold velocity to judge splitting two objects from radar information (now unused) [m/s] | 5.0 | +| 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 nearest radar data to the center of bounding box in velocity estimation. | 1.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 | @@ -48,15 +49,15 @@ Sensor fusion with radar objects and a detected object. ### How to launch ```sh -roslaunch radar_fusion_to_detected_object radar_object_to_detected_object.launch +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. | +| 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 diff --git a/perception/radar_fusion_to_detected_object/docs/algorithm.md b/perception/radar_fusion_to_detected_object/docs/algorithm.md index 01f5c59c51a80..6ea4ada5a51db 100644 --- a/perception/radar_fusion_to_detected_object/docs/algorithm.md +++ b/perception/radar_fusion_to_detected_object/docs/algorithm.md @@ -14,8 +14,17 @@ Choose radar pointcloud/objects within 3D bounding box from lidar-base detection ### 3. Estimate twist of object -Estimate twist from chosen radar pointcloud/objects. -Attach object to twist information of estimated twist. +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) 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 index 980105eaaa822..5b1f3caddf87d 100644 --- 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 @@ -1,4 +1,4 @@ - + @@ -19,7 +19,7 @@ - + DetectedObject @@ -53,11 +53,11 @@ - + -
+
@@ -66,7 +66,7 @@
or
- Centor gravity of radar object + Center of gravity of radar object
@@ -74,7 +74,7 @@
- + Radar pointcloud... 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 index 650bc5b8e423f..b719eb0619d2a 100644 --- 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 @@ -1,13 +1,13 @@ - + - - - + + + -
+
@@ -23,29 +23,29 @@
- + Twist observed from radars - - - - - - - - - - - - - - + + + + + + + + + + + + + + -
+
@@ -58,7 +58,7 @@
- + Estimated object twist 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 index b7aed0313d2cf..b9692c3fbfa59 100644 --- 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 @@ -21,7 +21,7 @@ #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 "std_msgs/msg/header.hpp" #include #include @@ -71,8 +71,8 @@ class RadarFusionToDetectedObject struct Input { - std::vector radars = {}; - DetectedObjects objects{}; + std::vector> radars{}; + DetectedObjects::ConstSharedPtr objects{}; }; struct Output @@ -86,13 +86,14 @@ class RadarFusionToDetectedObject private: rclcpp::Logger logger_; Param param_{}; - std::vector filterRadarWithinObject( - const DetectedObject & object, const std::vector & radars); + std::vector> filterRadarWithinObject( + const DetectedObject & object, const std::vector> & radars); std::vector splitObject( const DetectedObject & object, const std::vector & radars); TwistWithCovariance estimateTwist( - const DetectedObject & object, std::vector & radars); - bool isQualified(const DetectedObject & object, const std::vector & radars); + const DetectedObject & object, std::vector> & radars); + bool isQualified( + const DetectedObject & object, std::vector> & radars); TwistWithCovariance convertDopplerToTwist( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance); Twist addTwist(const Twist & twist_1, const Twist & twist_2); 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 index 6f06e6d821f04..a007361711848 100644 --- 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 @@ -81,8 +81,8 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node std::unique_ptr radar_fusion_to_detected_object_{}; // Lapper - RadarFusionToDetectedObject::RadarInput setRadarInput( - TrackedObject radar_object, std_msgs::msg::Header header); + std::shared_ptr setRadarInput( + const TrackedObject & radar_object, std_msgs::msg::Header header_); }; } // namespace radar_fusion_to_detected_object diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 06ca7b644f0a5..3bfd47562015a 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -15,9 +15,6 @@ rclcpp rclcpp_components std_msgs - tf2 - tf2_geometry_msgs - tf2_ros tier4_autoware_utils ament_lint_common 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 index cf416d00683a7..0edc7f4e38c19 100644 --- 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 @@ -70,11 +70,17 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( const RadarFusionToDetectedObject::Input & input) { RadarFusionToDetectedObject::Output output{}; - output.objects.header = input.objects.header; - for (const auto & object : input.objects.objects) { + output.objects.header = input.objects->header; + + if (input.objects->objects.empty()) { + return output; + } + + for (auto & object : input.objects->objects) { // Link between 3d bounding box and radar data - std::vector radars_within_object = filterRadarWithinObject(object, input.radars); + std::vector> radars_within_object = + filterRadarWithinObject(object, input.radars); // [TODO] (Satoshi Tanaka) Implement // Split the object going in a different direction @@ -83,7 +89,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( split_objects.emplace_back(object); for (auto & split_object : split_objects) { - std::vector radars_within_split_object; + std::vector> 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; @@ -110,11 +116,12 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( return output; } -std::vector +std::vector> RadarFusionToDetectedObject::filterRadarWithinObject( - const DetectedObject & object, const std::vector & radars) + const DetectedObject & object, + const std::vector> & radars) { - std::vector outputs{}; + 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); @@ -123,7 +130,7 @@ RadarFusionToDetectedObject::filterRadarWithinObject( for (const auto & radar : radars) { Point2d radar_point{ - radar.pose_with_covariance.pose.position.x, radar.pose_with_covariance.pose.position.y}; + 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); } @@ -140,7 +147,7 @@ RadarFusionToDetectedObject::filterRadarWithinObject( // } TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( - const DetectedObject & object, std::vector & radars) + const DetectedObject & object, std::vector> & radars) { TwistWithCovariance twist_with_covariance{}; if (radars.empty()) { @@ -150,37 +157,39 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( // 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 comp_func = + [&](const std::shared_ptr & a, const std::shared_ptr & 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; + 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); - }; + auto ascending_func = + [&](const std::shared_ptr & a, const std::shared_ptr & 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; + 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), + radars.at(median_index - 1)->twist_with_covariance.twist, + radars.at(median_index)->twist_with_covariance.twist), 0.5); } } @@ -189,7 +198,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( 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 = addTwist(twist_average, radar->twist_with_covariance.twist); } twist_average = scaleTwist(twist_average, (1.0 / radars.size())); } @@ -197,11 +206,12 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( // 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 comp_func = + [](const std::shared_ptr & a, const std::shared_ptr & 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; + twist_top_target_value = (*iter)->twist_with_covariance.twist; } // calculate twist for radar data with target_value * average @@ -210,9 +220,9 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( 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; + 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); } @@ -238,7 +248,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( } bool RadarFusionToDetectedObject::isQualified( - const DetectedObject & object, const std::vector & radars) + const DetectedObject & object, std::vector> & radars) { if (object.classification[0].probability > param_.threshold_probability) { return true; 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 index 9683773bc2054..a8239a0b2eee0 100644 --- 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 @@ -188,11 +188,17 @@ void RadarObjectFusionToDetectedObjectNode::onTimer() return; } + if (radar_objects_->objects.empty()) { + pub_objects_->publish(*detected_objects_); + return; + } + // Set input data RadarFusionToDetectedObject::Input input{}; - input.objects = *detected_objects_; + input.objects = detected_objects_; for (const auto & radar_object_ : radar_objects_->objects) { - input.radars.emplace_back(setRadarInput(radar_object_, radar_objects_->header)); + auto radar_ptr = setRadarInput(radar_object_, radar_objects_->header); + input.radars.emplace_back(radar_ptr); } // Update @@ -200,15 +206,18 @@ void RadarObjectFusionToDetectedObjectNode::onTimer() pub_objects_->publish(output_.objects); } -RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::setRadarInput( - TrackedObject radar_object, std_msgs::msg::Header header) +std::shared_ptr +RadarObjectFusionToDetectedObjectNode::setRadarInput( + const TrackedObject & radar_object, std_msgs::msg::Header header_) { RadarFusionToDetectedObject::RadarInput output{}; - output.header = header; 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; - return output; + output.header = header_; + std::shared_ptr output_ = + std::make_shared(output); + return output_; } } // namespace radar_fusion_to_detected_object From a3ce98c127015c98aa5f8458add55416de0bce9d Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 30 Jun 2022 17:59:11 +0900 Subject: [PATCH 15/18] exchange between smart_ptr and std::vector Signed-off-by: scepter914 --- .../radar_fusion_to_detected_object.hpp | 12 +-- ..._object_fusion_to_detected_object_node.hpp | 4 +- .../src/radar_fusion_to_detected_object.cpp | 99 +++++++++---------- ..._object_fusion_to_detected_object_node.cpp | 17 ++-- 4 files changed, 64 insertions(+), 68 deletions(-) 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 index b9692c3fbfa59..4ef313a9e97ad 100644 --- 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 @@ -71,7 +71,7 @@ class RadarFusionToDetectedObject struct Input { - std::vector> radars{}; + std::shared_ptr> radars{}; DetectedObjects::ConstSharedPtr objects{}; }; @@ -86,14 +86,14 @@ class RadarFusionToDetectedObject private: rclcpp::Logger logger_; Param param_{}; - std::vector> filterRadarWithinObject( - const DetectedObject & object, const std::vector> & radars); + std::shared_ptr> filterRadarWithinObject( + const DetectedObject & object, const std::shared_ptr> & radars); std::vector splitObject( - const DetectedObject & object, const std::vector & radars); + const DetectedObject & object, const std::shared_ptr> & radars); TwistWithCovariance estimateTwist( - const DetectedObject & object, std::vector> & radars); + const DetectedObject & object, std::shared_ptr> & radars); bool isQualified( - const DetectedObject & object, std::vector> & radars); + 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); 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 index a007361711848..45c3c4e86cc93 100644 --- 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 @@ -81,8 +81,8 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node std::unique_ptr radar_fusion_to_detected_object_{}; // Lapper - std::shared_ptr setRadarInput( - const TrackedObject & radar_object, std_msgs::msg::Header header_); + RadarFusionToDetectedObject::RadarInput setRadarInput( + const TrackedObject & radar_object, const std_msgs::msg::Header & header_); }; } // namespace radar_fusion_to_detected_object 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 index 0edc7f4e38c19..fdfe8065a4601 100644 --- 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 @@ -73,13 +73,13 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( output.objects.header = input.objects->header; - if (input.objects->objects.empty()) { + if (!input.objects || input.objects->objects.empty()) { return output; } for (auto & object : input.objects->objects) { // Link between 3d bounding box and radar data - std::vector> radars_within_object = + std::shared_ptr> radars_within_object = filterRadarWithinObject(object, input.radars); // [TODO] (Satoshi Tanaka) Implement @@ -89,7 +89,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( split_objects.emplace_back(object); for (auto & split_object : split_objects) { - std::vector> radars_within_split_object; + 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; @@ -99,7 +99,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( } // Estimate twist of object - if (!radars_within_split_object.empty()) { + 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); @@ -116,26 +116,26 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( return output; } -std::vector> +std::shared_ptr> RadarFusionToDetectedObject::filterRadarWithinObject( const DetectedObject & object, - const std::vector> & radars) + const std::shared_ptr> & radars) { - std::vector> outputs{}; + 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) { + for (const auto & radar : (*radars)) { Point2d radar_point{ - radar->pose_with_covariance.pose.position.x, radar->pose_with_covariance.pose.position.y}; + 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 outputs; + return std::make_shared>(outputs); } // [TODO] (Satoshi Tanaka) Implementation @@ -147,49 +147,47 @@ RadarFusionToDetectedObject::filterRadarWithinObject( // } TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( - const DetectedObject & object, std::vector> & radars) + const DetectedObject & object, std::shared_ptr> & radars) { TwistWithCovariance twist_with_covariance{}; - if (radars.empty()) { + 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 std::shared_ptr & a, const std::shared_ptr & 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; + 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 std::shared_ptr & a, const std::shared_ptr & 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; + 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; + 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), + (*radars).at(median_index - 1).twist_with_covariance.twist, + (*radars).at(median_index).twist_with_covariance.twist), 0.5); } } @@ -197,32 +195,31 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( // 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); + for (const auto & radar : (*radars)) { + twist_average = addTwist(twist_average, radar.twist_with_covariance.twist); } - twist_average = scaleTwist(twist_average, (1.0 / radars.size())); + 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 std::shared_ptr & a, const std::shared_ptr & 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; + 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) { + 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; + 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); } @@ -248,12 +245,12 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( } bool RadarFusionToDetectedObject::isQualified( - const DetectedObject & object, std::vector> & radars) + const DetectedObject & object, std::shared_ptr> & radars) { if (object.classification[0].probability > param_.threshold_probability) { return true; } else { - if (!radars.empty()) { + if (!radars || !(*radars).empty()) { return true; } else { return false; 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 index a8239a0b2eee0..2ee4e24ff98cf 100644 --- 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 @@ -195,29 +195,28 @@ void RadarObjectFusionToDetectedObjectNode::onTimer() // Set input data RadarFusionToDetectedObject::Input input{}; - input.objects = detected_objects_; + std::vector radars_{}; for (const auto & radar_object_ : radar_objects_->objects) { - auto radar_ptr = setRadarInput(radar_object_, radar_objects_->header); - input.radars.emplace_back(radar_ptr); + 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); } -std::shared_ptr -RadarObjectFusionToDetectedObjectNode::setRadarInput( - const TrackedObject & radar_object, std_msgs::msg::Header header_) +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_; - std::shared_ptr output_ = - std::make_shared(output); - return output_; + return output; } } // namespace radar_fusion_to_detected_object From 51fc988b20cc5679c9f4fc2b345400de07539d62 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 30 Jun 2022 19:02:11 +0900 Subject: [PATCH 16/18] fix unused function Signed-off-by: scepter914 --- .../include/radar_fusion_to_detected_object.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 index 4ef313a9e97ad..c3ea24c8c3e6b 100644 --- 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 @@ -88,8 +88,9 @@ class RadarFusionToDetectedObject Param param_{}; std::shared_ptr> filterRadarWithinObject( const DetectedObject & object, const std::shared_ptr> & radars); - std::vector splitObject( - 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( From aaa332b7f269fbe7c8f126e408f262f039eb9e13 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 30 Jun 2022 19:18:22 +0900 Subject: [PATCH 17/18] add comment Signed-off-by: scepter914 --- .../src/radar_fusion_to_detected_object.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 index fdfe8065a4601..8b9c96f9d60ed 100644 --- 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 @@ -116,6 +116,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( 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, @@ -146,6 +147,8 @@ RadarFusionToDetectedObject::filterRadarWithinObject( // 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) { @@ -244,6 +247,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( 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) { From 4e8b4bde5e479317c8d396af771f5cff72ff2b08 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 30 Jun 2022 10:19:45 +0000 Subject: [PATCH 18/18] ci(pre-commit): autofix --- .../src/radar_fusion_to_detected_object.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 index 8b9c96f9d60ed..1b6da03c3b689 100644 --- 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 @@ -116,7 +116,8 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( return output; } -// Choose radar pointcloud/objects within 3D bounding box from lidar-base detection with margin space from bird's-eye view. +// 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, @@ -148,7 +149,8 @@ RadarFusionToDetectedObject::filterRadarWithinObject( // } // 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). +// (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) {