feat: add object range splitter package

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
+)

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
+
+<!-- Write how this package works. Flowcharts and figures are great. Add sub-sections as you like.
+
+Example:
+  ### Flowcharts
+
+  ...(PlantUML or something)
+
+  ### State Transitions
+
+  ...(PlantUML or something)
+
+  ### How to filter target obstacles
+
+  ...
+
+  ### How to optimize trajectory
+
+  ...
+-->
+
+## 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
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (Optional) Future extensions / Unimplemented parts
+
+<!-- Write future extensions of this package.
+
+Example:
+  Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
+  Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes.
+-->

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+
+#include <memory>
+
+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<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
+    long_range_object_pub_;
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
+    short_range_object_pub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr sub_;
+
+  // ROS Parameters
+  float spilt_range_;
+};
+
+}  // namespace object_range_splitter
+
+#endif  // OBJECT_RANGE_SPLITTER__NODE_HPP_

perception/object_range_splitter/launch/object_range_splitter.launch.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="input/object" default="object"/>
+  <arg name="output/long_range_object" default="long_range_object"/>
+  <arg name="output/short_range_object" default="short_range_object"/>
+  <arg name="split_range" default="30.0"/>
+
+  <node pkg="object_range_splitter" exec="object_range_splitter_node" name="object_range_splitter" output="screen">
+    <remap from="input/object" to="$(var input/object)"/>
+    <remap from="output/long_range_object" to="$(var output/long_range_object)"/>
+    <remap from="output/short_range_object" to="$(var output/short_range_object)"/>
+    <param name="split_range" value="$(var split_range)" />
+  </node>
+
+</launch>

perception/object_range_splitter/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>object_range_splitter</name>
+  <version>0.1.0</version>
+  <description>The object_range_splitter package</description>
+
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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<autoware_auto_perception_msgs::msg::DetectedObjects>(
+    "input/object", rclcpp::QoS{1}, std::bind(&ObjectRangeSplitterNode::objectCallback, this, _1));
+  long_range_object_pub_ =
+    this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
+      "output/long_range_object", rclcpp::QoS{1});
+  short_range_object_pub_ =
+    this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
+      "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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(object_range_splitter::ObjectRangeSplitterNode)