diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..77b10c0afb87a --- /dev/null +++ b/perception/simple_object_merger/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) +project(simple_object_merger) + +# Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(simple_object_merger_node_component SHARED + src/simple_object_merger_node/simple_object_merger_node.cpp +) + +rclcpp_components_register_node(simple_object_merger_node_component + PLUGIN "simple_object_merger::SimpleObjectMergerNode" + EXECUTABLE simple_object_merger_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 + launch +) diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md new file mode 100644 index 0000000000000..4609638cc008c --- /dev/null +++ b/perception/simple_object_merger/README.md @@ -0,0 +1,60 @@ +# simple_object_merger + +This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without data association algorithm. + +## Algorithm + +### Background + +This package can merge multiple DetectedObjects without matching processing. +[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. +In addition, for now, [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. +To merge 6 DetectedObjects topics, 6 [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) nodes need to stand. + +So this package aim to merge DetectedObjects simply. +This package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. + +### Limitation + +- Sensor data drops and delay + +Merged objects will not be published until all topic data is received when initializing. +In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. +When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. +For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout. +The timeout parameter should be determined by sensor cycle time. + +- Post-processing + +Because this package does not have matching processing, so it can be used only when post-processing is used. +For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. + +### Use case + +Use case is as below. + +- Multiple radar detection + +This package can be used for multiple radar detection. +Since [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) will be included later process in radar faraway detection, this package can be used. + +## Input + +| Name | Type | Description | +| ---- | ------------------------------------------------------------------ | ------------------------------------------------------ | +| | std::vector | 3D detected objects. Topic names are set by parameters | + +## Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | -------------- | +| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Merged objects | + +## Parameters + +| Name | Type | Description | Default value | +| :------------------ | :----------- | :----------------------------------- | :------------ | +| `update_rate_hz` | double | Update rate. [hz] | 20.0 | +| `new_frame_id` | string | The header frame_id of output topic. | "base_link" | +| `timeout_threshold` | double | Threshold for timeout judgement [s]. | 1.0 | +| `input_topics` | List[string] | Input topics name. | "[]" | diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp new file mode 100644 index 0000000000000..6e2daf9266c9c --- /dev/null +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ +#define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include + +namespace simple_object_merger +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + +class SimpleObjectMergerNode : public rclcpp::Node +{ +public: + explicit SimpleObjectMergerNode(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + double update_rate_hz{}; + double timeout_threshold{}; + std::vector topic_names{}; + std::string new_frame_id{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_objects_{}; + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + // Callback + void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number); + + // Data Buffer + std::vector objects_data_{}; + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_; + + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_{}; + + // Timer + rclcpp::TimerBase::SharedPtr timer_{}; + void onTimer(); + bool isDataReady(); + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Parameter + NodeParam node_param_{}; + + // Core + size_t input_topic_size; +}; + +} // namespace simple_object_merger + +#endif // SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ diff --git a/perception/simple_object_merger/launch/simple_object_merger.launch.xml b/perception/simple_object_merger/launch/simple_object_merger.launch.xml new file mode 100644 index 0000000000000..14fca01fa6438 --- /dev/null +++ b/perception/simple_object_merger/launch/simple_object_merger.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/perception/simple_object_merger/package.xml b/perception/simple_object_merger/package.xml new file mode 100644 index 0000000000000..9803ff6310d4c --- /dev/null +++ b/perception/simple_object_merger/package.xml @@ -0,0 +1,28 @@ + + + simple_object_merger + 0.1.0 + simple_object_merger + Sathshi Tanaka + Shunsuke Miura + Yoshi Ri + + Apache License 2.0 + Sathshi Tanaka + + ament_cmake_auto + + autoware_auto_perception_msgs + geometry_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp new file mode 100644 index 0000000000000..506a40e672fb6 --- /dev/null +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -0,0 +1,197 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simple_object_merger/simple_object_merger_node.hpp" + +#include +#include +#include + +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; +} + +autoware_auto_perception_msgs::msg::DetectedObjects::SharedPtr getTransformedObjects( + autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr objects, + const std::string & target_frame_id, + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform) +{ + autoware_auto_perception_msgs::msg::DetectedObjects::SharedPtr output_objects = + std::const_pointer_cast(objects); + + if (objects->header.frame_id == target_frame_id) { + return output_objects; + } + + output_objects->header.frame_id = target_frame_id; + for (auto & object : output_objects->objects) { + // convert by tf + geometry_msgs::msg::PoseStamped pose_stamped{}; + pose_stamped.pose = object.kinematics.pose_with_covariance.pose; + geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; + tf2::doTransform(pose_stamped, transformed_pose_stamped, *transform); + object.kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; + } + return output_objects; +} + +} // namespace + +namespace simple_object_merger +{ +using namespace std::literals; +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_options) +: Node("simple_object_merger", node_options) +{ + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SimpleObjectMergerNode::onSetParam, this, std::placeholders::_1)); + + // Node Parameter + node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); + node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); + node_param_.timeout_threshold = declare_parameter("timeout_threshold", 1.0); + + declare_parameter("input_topics", std::vector()); + node_param_.topic_names = get_parameter("input_topics").as_string_array(); + if (node_param_.topic_names.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing"); + return; + } + + input_topic_size = node_param_.topic_names.size(); + if (input_topic_size == 1) { + RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); + return; + } + + // Subscriber + transform_listener_ = std::make_shared(this); + sub_objects_array.resize(input_topic_size); + objects_data_.resize(input_topic_size); + + for (size_t i = 0; i < input_topic_size; i++) { + std::function func = + std::bind(&SimpleObjectMergerNode::onData, this, std::placeholders::_1, i); + sub_objects_array.at(i) = + create_subscription(node_param_.topic_names.at(i), rclcpp::QoS{1}, func); + } + + // 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(&SimpleObjectMergerNode::onTimer, this)); +} + +void SimpleObjectMergerNode::onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number) +{ + objects_data_.at(array_number) = msg; +} + +rcl_interfaces::msg::SetParametersResult SimpleObjectMergerNode::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + try { + // Node Parameter + { + auto & p = node_param_; + // Update params + update_param(params, "update_rate_hz", p.update_rate_hz); + update_param(params, "timeout_threshold", p.timeout_threshold); + update_param(params, "new_frame_id", p.new_frame_id); + update_param(params, "topic_names", p.topic_names); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + result.successful = true; + result.reason = "success"; + return result; +} + +bool SimpleObjectMergerNode::isDataReady() +{ + for (size_t i = 0; i < input_topic_size; i++) { + if (!objects_data_.at(i)) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for object msg..."); + return false; + } + } + + return true; +} + +void SimpleObjectMergerNode::onTimer() +{ + if (!isDataReady()) { + return; + } + + DetectedObjects output_objects; + output_objects.header = objects_data_.at(0)->header; + output_objects.header.frame_id = node_param_.new_frame_id; + + for (size_t i = 0; i < input_topic_size; i++) { + double time_diff = (this->get_clock()->now()).seconds() - + rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); + if (std::abs(time_diff) < node_param_.timeout_threshold) { + transform_ = transform_listener_->getTransform( + node_param_.new_frame_id, objects_data_.at(i)->header.frame_id, + objects_data_.at(i)->header.stamp, rclcpp::Duration::from_seconds(0.01)); + + DetectedObjects::SharedPtr transformed_objects = + getTransformedObjects(objects_data_.at(i), node_param_.new_frame_id, transform_); + + output_objects.objects.insert( + output_objects.objects.end(), std::begin(transformed_objects->objects), + std::end(transformed_objects->objects)); + } else { + RCLCPP_INFO( + rclcpp::get_logger("simple_object_merger"), "Topic of %s is timeout by %f sec", + node_param_.topic_names.at(i).c_str(), time_diff); + } + } + + pub_objects_->publish(output_objects); +} + +} // namespace simple_object_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(simple_object_merger::SimpleObjectMergerNode)