diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/object_range_splitter/CMakeLists.txt new file mode 100644 index 0000000000000..8f7f5fd4f33c5 --- /dev/null +++ b/perception/object_range_splitter/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(object_range_splitter) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +########### +## Build ## +########### + +include_directories( + include +) + +ament_auto_add_library(object_range_splitter SHARED + src/node.cpp +) + +rclcpp_components_register_node(object_range_splitter + PLUGIN "object_range_splitter::ObjectRangeSplitterNode" + EXECUTABLE object_range_splitter_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/object_range_splitter/README.md b/perception/object_range_splitter/README.md new file mode 100644 index 0000000000000..89d266a666219 --- /dev/null +++ b/perception/object_range_splitter/README.md @@ -0,0 +1,97 @@ +# object_range_splitter + +## Purpose + +object_range_splitter is a package to divide detected objects into two messages by the distance from the origin. + +## Inner-workings / Algorithms + + + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------- | ----------------------------------------------------- | ---------------- | +| `input/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | + +### Output + +| Name | Type | Description | +| --------------------------- | ----------------------------------------------------- | ---------------------------- | +| `output/long_range_object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | long range detected objects | +| `output/short_range_object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | short range detected objects | + +## Parameters + +| Name | Type | Description | +| ------------- | ----- | ---------------------------------------------------- | +| `split_range` | float | the distance boundary to divide detected objects [m] | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/object_range_splitter/include/object_range_splitter/node.hpp b/perception/object_range_splitter/include/object_range_splitter/node.hpp new file mode 100644 index 0000000000000..91e3e9ee96130 --- /dev/null +++ b/perception/object_range_splitter/include/object_range_splitter/node.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 TierIV +// +// 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 OBJECT_RANGE_SPLITTER__NODE_HPP_ +#define OBJECT_RANGE_SPLITTER__NODE_HPP_ + +#include + +#include + +#include + +namespace object_range_splitter +{ +class ObjectRangeSplitterNode : public rclcpp::Node +{ +public: + explicit ObjectRangeSplitterNode(const rclcpp::NodeOptions & node_options); + +private: + void objectCallback( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg); + + rclcpp::Publisher::SharedPtr + long_range_object_pub_; + rclcpp::Publisher::SharedPtr + short_range_object_pub_; + rclcpp::Subscription::SharedPtr sub_; + + // ROS Parameters + float spilt_range_; +}; + +} // namespace object_range_splitter + +#endif // OBJECT_RANGE_SPLITTER__NODE_HPP_ diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/perception/object_range_splitter/launch/object_range_splitter.launch.xml new file mode 100644 index 0000000000000..ffbf3a06b1bc3 --- /dev/null +++ b/perception/object_range_splitter/launch/object_range_splitter.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/perception/object_range_splitter/package.xml b/perception/object_range_splitter/package.xml new file mode 100644 index 0000000000000..990d3be331667 --- /dev/null +++ b/perception/object_range_splitter/package.xml @@ -0,0 +1,23 @@ + + + object_range_splitter + 0.1.0 + The object_range_splitter package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + rclcpp + rclcpp_components + sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/object_range_splitter/src/node.cpp b/perception/object_range_splitter/src/node.cpp new file mode 100644 index 0000000000000..52383ef922621 --- /dev/null +++ b/perception/object_range_splitter/src/node.cpp @@ -0,0 +1,67 @@ +// Copyright 2020 TierIV +// +// 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 "object_range_splitter/node.hpp" + +namespace object_range_splitter +{ +ObjectRangeSplitterNode::ObjectRangeSplitterNode(const rclcpp::NodeOptions & node_options) +: Node("object_range_splitter_node", node_options) +{ + using std::placeholders::_1; + spilt_range_ = declare_parameter("split_range", 30.0); + sub_ = this->create_subscription( + "input/object", rclcpp::QoS{1}, std::bind(&ObjectRangeSplitterNode::objectCallback, this, _1)); + long_range_object_pub_ = + this->create_publisher( + "output/long_range_object", rclcpp::QoS{1}); + short_range_object_pub_ = + this->create_publisher( + "output/short_range_object", rclcpp::QoS{1}); +} + +void ObjectRangeSplitterNode::objectCallback( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg) +{ + // Guard + if ( + long_range_object_pub_->get_subscription_count() < 1 && + short_range_object_pub_->get_subscription_count() < 1) { + return; + } + // build output msg + autoware_auto_perception_msgs::msg::DetectedObjects output_long_range_object_msg, + output_short_range_object_msg; + output_long_range_object_msg.header = input_msg->header; + output_short_range_object_msg.header = input_msg->header; + + // split + for (const auto & object : input_msg->objects) { + const auto & position = object.kinematics.pose_with_covariance.pose.position; + const auto object_sq_dist = position.x * position.x + position.y * position.y; + if (object_sq_dist < spilt_range_ * spilt_range_) { // short range + output_short_range_object_msg.objects.push_back(object); + } else { // long range + output_long_range_object_msg.objects.push_back(object); + } + } + + // publish output msg + long_range_object_pub_->publish(output_long_range_object_msg); + short_range_object_pub_->publish(output_short_range_object_msg); +} +} // namespace object_range_splitter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(object_range_splitter::ObjectRangeSplitterNode)