feat: add object range splitter package (autowarefoundation#113)

* Ros2 v0.8.0 object range splitter (autowarefoundation#306) * Fix typo in perception module (autowarefoundation#440) * add use_sim-time option (autowarefoundation#454) * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Perception components (autowarefoundation#1368) * [bev_optical_flow]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [object_merger]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [object_range_splitter]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [shape_estimation]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [map_based_prediction]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [naive_path_prediction]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [roi_image_saver]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [lidar_apollo_instance_segmentation]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [object_flow_fusion]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [traffic_light_map_based_detector]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [dynamic_object_visualization]: component node Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix typo Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Detection by tracker (autowarefoundation#1910) * initial commit * backup * apply format * cosmetic change * implement divided under segmenterd clusters * cosmetic change * bug fix * bug fix * bug fix * modify launch * add debug and bug fix * bug fix * bug fix * add no found tracked object * modify parameters and cmake * bug fix * remove debug info * add readme * modify clustering launch * run pre-commit * cosmetic change * cosmetic change * cosmetic change * apply markdownlint * modify launch * modify for cpplint * modify qos * change int to size_T * bug fix * change perception qos * Update perception/object_recognition/detection/detection_by_tracker/package.xml Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * cosmetic change * cosmetic change * fix launch * Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * modify header include order * change include order * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * change to std::optional * cosmetic change * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * bug fix * modify readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * port object range splitter (autowarefoundation#494) * change autoware_perception_msgs to autoware_auto_perception_msgs * delete COLCON_IGNORE * use changed autoware_auto_perception_msgs (autowarefoundation#546) * add README of object_range_splitter (autowarefoundation#592) Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
satoshi-ota · Dec 6, 2021 · b451801 · b451801
1 parent 3d38627
commit b451801
Show file tree

Hide file tree

Showing 6 changed files with 293 additions and 0 deletions.
diff --git a/perception/object_range_splitter/CMakeLists.txt 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
@@ -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.
+-->
diff --git a/perception/object_range_splitter/include/object_range_splitter/node.hpp 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 <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_
diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/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>
diff --git a/perception/object_range_splitter/package.xml b/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>
diff --git a/perception/object_range_splitter/src/node.cpp 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<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)