fate: add object merger package (autowarefoundation#107)

* Ros2 v0.8.0 object merger (autowarefoundation#302)

* 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>

* Feature/porting mussp with vendor (autowarefoundation#1323)

* change to mussp (autowarefoundation#1180)

* change to mussp

* add license

* add license

* change to solver plugin

* fix bug

* cosmetic change

* modified to follow ROS2 coding style

* Use mussp vendor

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix dependencies in package.xml

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add mussp_vendor into build_depends.repos

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Use fork instead of vendor package

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Cleanup CMakeLists.txt

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove comment

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Sort package dependencies in alphabetical order

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.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>

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* 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 merger (autowarefoundation#524)

* support for autoware_auto_perception_msgs

* use existence_probability

* add README of object_merger (autowarefoundation#621)

* add README of object_merger

* add comment about score

* fix typo

* Update README.md

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: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Keisuke Shima <keisuke.shima@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: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

perception/object_merger/CMakeLists.txt

@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.5)
+project(object_merger)
+
+find_package(PCL REQUIRED COMPONENTS common filters)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  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()
+
+include_directories(
+  include
+)
+
+ament_auto_add_library(object_association_merger SHARED
+  src/object_association_merger/utils/utils.cpp
+  src/object_association_merger/data_association/data_association.cpp
+  src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp
+  src/object_association_merger/node.cpp
+)
+
+target_link_libraries(object_association_merger ${PCL_LIBRARIES})
+
+rclcpp_components_register_node(object_association_merger
+  PLUGIN "object_association::ObjectAssociationMergerNode"
+  EXECUTABLE object_association_merger_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_merger/README.md

@@ -0,0 +1,72 @@
+# object_merger
+
+## Purpose
+
+object_merger is a package for merging detected objects from two methods by data association.
+
+## Inner-workings / Algorithms
+
+The successive shortest path algorithm is used to solve the data association problem (the minimum-cost flow problem). The cost is calculated by the distance between two objects and gate functions are applied to reset cost, s.t. the maximum distance, the maximum area and the minimum area.
+
+## Inputs / Outputs
+
+### Input
+
+| Name            | Type                                                  | Description       |
+| --------------- | ----------------------------------------------------- | ----------------- |
+| `input/object0` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection objects |
+| `input/object1` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection objects |
+
+### Output
+
+| Name            | Type                                                  | Description      |
+| --------------- | ----------------------------------------------------- | ---------------- |
+| `output/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | modified Objects |
+
+## Parameters
+
+No Parameters.
+
+## 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
+
+Data association algorithm was the same as that of multi_object_tracker, but the algorithm of multi_object_tracker was already updated.

perception/object_merger/include/object_association_merger/data_association.hpp

@@ -0,0 +1,52 @@
+// Copyright 2020 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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_
+#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_
+
+#include <list>
+#include <unordered_map>
+#include <vector>
+
+#define EIGEN_MPL2_ONLY
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+class DataAssociation
+{
+private:
+  double getDistance(
+    const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1);
+  geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud);
+  Eigen::MatrixXi can_assign_matrix_;
+  Eigen::MatrixXd max_dist_matrix_;
+  Eigen::MatrixXd max_area_matrix_;
+  Eigen::MatrixXd min_area_matrix_;
+  const double score_threshold_;
+
+public:
+  DataAssociation();
+  bool assign(
+    const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
+    std::unordered_map<int, int> & reverse_assignment);
+  Eigen::MatrixXd calcScoreMatrix(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & object0,
+    const autoware_auto_perception_msgs::msg::DetectedObjects & object1);
+  virtual ~DataAssociation() {}
+};
+
+#endif  // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_

perception/object_merger/include/object_association_merger/node.hpp

@@ -0,0 +1,60 @@
+// Copyright 2020 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 OBJECT_ASSOCIATION_MERGER__NODE_HPP_
+#define OBJECT_ASSOCIATION_MERGER__NODE_HPP_
+
+#include <object_association_merger/data_association.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+
+namespace object_association
+{
+class ObjectAssociationMergerNode : public rclcpp::Node
+{
+public:
+  explicit ObjectAssociationMergerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void objectsCallback(
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object0_msg,
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object1_msg);
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
+    merged_object_pub_;
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::DetectedObjects> object0_sub_;
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::DetectedObjects> object1_sub_;
+  typedef message_filters::sync_policies::ApproximateTime<
+    autoware_auto_perception_msgs::msg::DetectedObjects,
+    autoware_auto_perception_msgs::msg::DetectedObjects>
+    SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> Sync;
+  Sync sync_;
+  DataAssociation data_association_;
+};
+}  // namespace object_association
+
+#endif  // OBJECT_ASSOCIATION_MERGER__NODE_HPP_

perception/object_merger/include/object_association_merger/successive_shortest_path.hpp

@@ -0,0 +1,29 @@
+// Copyright 2020 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 OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_
+#define OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_
+
+#include <unordered_map>
+#include <vector>
+
+namespace assignment_problem
+{
+// See IMPORTANT NOTE at the top of the file.
+void MaximizeLinearAssignment(
+  const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
+  std::unordered_map<int, int> * reverse_assignment);
+}  // namespace assignment_problem
+
+#endif  // OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_

perception/object_merger/include/object_association_merger/utils/utils.hpp

@@ -0,0 +1,32 @@
+// Copyright 2020 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 OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_
+#define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_
+
+#include <autoware_auto_perception_msgs/msg/shape.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+#include <cmath>
+
+namespace utils
+{
+double getPolygonArea(const geometry_msgs::msg::Polygon & footprint);
+double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions);
+double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions);
+double getArea(const autoware_auto_perception_msgs::msg::Shape & shape);
+}  // namespace utils
+
+#endif  // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_

perception/object_merger/launch/object_association_merger.launch.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="input/object0" default="object0"/>
+  <arg name="input/object1" default="object1"/>
+  <arg name="output/object" default="merged_object"/>
+
+  <node pkg="object_merger" exec="object_association_merger_node" name="object_association_merger" output="screen">
+    <remap from="input/object0" to="$(var input/object0)"/>
+    <remap from="input/object1" to="$(var input/object1)"/>
+    <remap from="output/object" to="$(var output/object)"/>
+  </node>
+
+</launch>

perception/object_merger/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>object_merger</name>
+  <version>0.1.0</version>
+  <description>The object_merger 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>libpcl-all-dev</depend>
+  <depend>message_filters</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_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>