feat(simple_object_tracker): add_simple_object_tracker

perception/simple_object_tracker/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.8)
+project(simple_object_tracker)
+
+### 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 -Werror)
+endif()
+
+# Ignore -Wnonportable-include-path in Clang for mussp
+if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-nonportable-include-path)
+endif()
+
+### Find Packages
+find_package(ament_cmake_auto REQUIRED)
+
+### Find dependencies listed in the package.xml
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(simple_object_tracker_node SHARED
+  src/simple_object_tracker_core.cpp
+)
+
+rclcpp_components_register_node(simple_object_tracker_node
+  PLUGIN "SimpleObjectTracker"
+  EXECUTABLE simple_object_tracker
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

perception/simple_object_tracker/README.md

@@ -0,0 +1,5 @@
+# multi_object_tracker
+
+## Purpose
+
+Simple Object Tracker just transforms a detected object tracker into a tracked object without delay. This packages will be useful when someone want to test their package with non-delayed object in simulation.

perception/simple_object_tracker/include/simple_object_tracker/simple_object_tracker_core.hpp

@@ -0,0 +1,59 @@
+// Copyright 2022 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.
+//
+//
+// Author: v1.0 Yutaka Shimizu
+///
+
+#ifndef SIMPLE_OBJECT_TRACKER__SIMPLE_OBJECT_TRACKER_CORE_HPP_
+#define SIMPLE_OBJECT_TRACKER__SIMPLE_OBJECT_TRACKER_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+class SimpleObjectTracker : public rclcpp::Node
+{
+public:
+  explicit SimpleObjectTracker(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
+    tracked_objects_pub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
+    detected_object_sub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  void onMeasurement(
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
+
+  std::string world_frame_id_;  // tracking frame
+};
+
+#endif  // SIMPLE_OBJECT_TRACKER__SIMPLE_OBJECT_TRACKER_CORE_HPP_

perception/simple_object_tracker/launch/simple_object_tracker.launch.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="input" default="/perception/object_recognition/detection/objects"/>
+  <arg name="output" default="objects"/>
+  <arg name="world_frame_id" default="map"/>
+
+  <node pkg="simple_object_tracker" exec="simple_object_tracker" name="simple_object_tracker" output="screen">
+    <remap from="input" to="$(var input)"/>
+    <remap from="output" to="$(var output)"/>
+    <param name="world_frame_id" value="$(var world_frame_id)" />
+  </node>
+</launch>

perception/simple_object_tracker/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>simple_object_tracker</name>
+  <version>1.0.0</version>
+  <description>The ROS2 simple_object_tracker package</description>
+  <maintainer email="[email protected]">yutaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_perception_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/simple_object_tracker/src/simple_object_tracker_core.cpp

@@ -0,0 +1,138 @@
+// Copyright 2022 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_tracker/simple_object_tracker_core.hpp"
+
+#include <rclcpp_components/register_node_macro.hpp>
+
+#include <boost/optional.hpp>
+
+#include <tf2_ros/create_timer_interface.h>
+#include <tf2_ros/create_timer_ros.h>
+
+namespace
+{
+boost::optional<geometry_msgs::msg::Transform> getTransform(
+  const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
+  const std::string & target_frame_id, const rclcpp::Time & time)
+{
+  try {
+    geometry_msgs::msg::TransformStamped self_transform_stamped;
+    self_transform_stamped = tf_buffer.lookupTransform(
+      /*target*/ target_frame_id, /*src*/ source_frame_id, time,
+      rclcpp::Duration::from_seconds(0.5));
+    return self_transform_stamped.transform;
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_WARN_STREAM(rclcpp::get_logger("simple_object_tracker"), ex.what());
+    return boost::none;
+  }
+}
+
+bool transformDetectedObjects(
+  const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg,
+  const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+  autoware_auto_perception_msgs::msg::DetectedObjects & output_msg)
+{
+  output_msg = input_msg;
+
+  /* transform to world coordinate */
+  if (input_msg.header.frame_id != target_frame_id) {
+    output_msg.header.frame_id = target_frame_id;
+    tf2::Transform tf_target2objects_world;
+    tf2::Transform tf_target2objects;
+    tf2::Transform tf_objects_world2objects;
+    {
+      const auto ros_target2objects_world =
+        getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+      if (!ros_target2objects_world) {
+        return false;
+      }
+      tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
+    }
+    for (size_t i = 0; i < output_msg.objects.size(); ++i) {
+      tf2::fromMsg(
+        output_msg.objects.at(i).kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+      tf2::toMsg(tf_target2objects, output_msg.objects.at(i).kinematics.pose_with_covariance.pose);
+      // TODO(yukkysaito) transform covariance
+    }
+  }
+  return true;
+}
+}  // namespace
+
+SimpleObjectTracker::SimpleObjectTracker(const rclcpp::NodeOptions & node_options)
+: rclcpp::Node("simple_object_tracker", node_options),
+  tf_buffer_(this->get_clock()),
+  tf_listener_(tf_buffer_)
+{
+  // Create publishers and subscribers
+  detected_object_sub_ = create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
+    "input", rclcpp::QoS{1},
+    std::bind(&SimpleObjectTracker::onMeasurement, this, std::placeholders::_1));
+  tracked_objects_pub_ =
+    create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>("output", rclcpp::QoS{1});
+
+  // Parameters
+  world_frame_id_ = declare_parameter<std::string>("world_frame_id", "world");
+
+  auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
+    this->get_node_base_interface(), this->get_node_timers_interface());
+  tf_buffer_.setCreateTimerInterface(cti);
+}
+
+void SimpleObjectTracker::onMeasurement(
+  const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
+{
+  const auto self_transform =
+    getTransform(tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
+  if (!self_transform) {
+    return;
+  }
+
+  /* transform to world coordinate */
+  autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
+  if (!transformDetectedObjects(
+        *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) {
+    return;
+  }
+
+  const auto subscriber_count = tracked_objects_pub_->get_subscription_count() +
+                                tracked_objects_pub_->get_intra_process_subscription_count();
+  if (subscriber_count < 1) {
+    return;
+  }
+
+  // Create output msg
+  autoware_auto_perception_msgs::msg::TrackedObjects output_msg;
+  output_msg.header.frame_id = world_frame_id_;
+  output_msg.header.stamp = input_objects_msg->header.stamp;
+  for (const auto & input_object : input_objects_msg->objects) {
+    autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
+    tracked_object.classification = input_object.classification;
+    tracked_object.existence_probability = input_object.existence_probability;
+    tracked_object.shape = input_object.shape;
+    tracked_object.kinematics.pose_with_covariance = input_object.kinematics.pose_with_covariance;
+    tracked_object.kinematics.twist_with_covariance = input_object.kinematics.twist_with_covariance;
+    tracked_object.kinematics.orientation_availability =
+      input_object.kinematics.orientation_availability;
+    output_msg.objects.push_back(tracked_object);
+  }
+
+  tracked_objects_pub_->publish(output_msg);
+}
+
+RCLCPP_COMPONENTS_REGISTER_NODE(SimpleObjectTracker)