feat(front_vehicle_velocity_estimator): add front_vehicle_velocity_es…

…timator (tier4#1536)

* add front vehicle estimator

Signed-off-by: scepter914 <scepter914@gmail.com>

* update README

Signed-off-by: scepter914 <scepter914@gmail.com>

* ci(pre-commit): autofix

* fix cast

Signed-off-by: scepter914 <scepter914@gmail.com>

* add message filter

Signed-off-by: scepter914 <scepter914@gmail.com>

* run pre-commit

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix copyright

Signed-off-by: scepter914 <scepter914@gmail.com>

Signed-off-by: scepter914 <scepter914@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/front_vehicle_velocity_estimator/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 3.5)
+project(front_vehicle_velocity_estimator)
+
+# Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  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()
+
+# Dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(front_vehicle_velocity_estimator_node_component SHARED
+  src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp
+  src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp
+)
+
+rclcpp_components_register_node(front_vehicle_velocity_estimator_node_component
+  PLUGIN "front_vehicle_velocity_estimator::FrontVehicleVelocityEstimatorNode"
+  EXECUTABLE front_vehicle_velocity_estimator_node
+)
+
+# Tests
+if(BUILD_TESTING)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Package
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

perception/front_vehicle_velocity_estimator/README.md

@@ -0,0 +1,45 @@
+# front_vehicle_velocity_estimator
+
+This package contains a front vehicle velocity estimation for offline perception module analysis.
+This package can:
+
+- Attach velocity to 3D detections from velocity estimation with LiDAR pointcloud.
+
+## Algorithm
+
+- Processing flow
+  1. Choose front vehicle from front area objects.
+  2. Choose nearest neighbor point within front vehicle.
+  3. Estimate velocity of front vehicle by using the differentiated value from time series of nearest neighbor point positions.
+  4. Compensate ego vehicle twist
+
+## Input
+
+| Name                 | Type                                                 | Description          |
+| -------------------- | ---------------------------------------------------- | -------------------- |
+| `~/input/objects`    | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. |
+| `~/input/pointcloud` | sensor_msgs/msg/PointCloud2.msg                      | LiDAR pointcloud.    |
+| `~/input/odometry`   | nav_msgs::msg::Odometry.msg                          | Odometry data.       |
+
+## Output
+
+| Name                                  | Type                                                  | Description                                   |
+| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------- |
+| `~/output/objects`                    | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist.                |
+| `~/debug/nearest_neighbor_pointcloud` | sensor_msgs/msg/PointCloud2.msg                       | The pointcloud msg of nearest neighbor point. |
+
+## Node parameter
+
+| Name             | Type   | Description           | Default value |
+| :--------------- | :----- | :-------------------- | :------------ |
+| `update_rate_hz` | double | The update rate [hz]. | 10.0          |
+
+## Core parameter
+
+| Name                          | Type   | Description                                                                                                                                                                                                                                                                 | Default value |
+| :---------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `moving_average_num`          | int    | The moving average number for velocity estimation.                                                                                                                                                                                                                          | 1             |
+| `threshold_pointcloud_z_high` | float  | The threshold for z position value of point when choosing nearest neighbor point within front vehicle [m]. If z > `threshold_pointcloud_z_high`, the point is considered to noise.                                                                                          | 1.0f          |
+| `threshold_pointcloud_z_low`  | float  | The threshold for z position value of point when choosing nearest neighbor point within front vehicle [m]. If z < `threshold_pointcloud_z_low`, the point is considered to noise like ground.                                                                               | 0.6f          |
+| `threshold_relative_velocity` | double | The threshold for min and max of estimated relative velocity ($v_{re}$) [m/s]. If $v_{re}$ < - `threshold_relative_velocity` , then $v_{re}$ = - `threshold_relative_velocity`. If $v_{re}$ > `threshold_relative_velocity`, then $v_{re}$ = `threshold_relative_velocity`. | 10.0          |
+| `threshold_absolute_velocity` | double | The threshold for max of estimated absolute velocity ($v_{ae}$) [m/s]. If $v_{ae}$ > `threshold_absolute_velocity`, then $v_{ae}$ = `threshold_absolute_velocity`.                                                                                                          | 20.0          |

perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp

@@ -0,0 +1,107 @@
+// 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.
+
+#ifndef FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_
+#define FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_
+
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+#include "pcl_conversions/pcl_conversions.h"
+#include "rclcpp/logger.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include <deque>
+#include <memory>
+#include <string>
+
+namespace front_vehicle_velocity_estimator
+{
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjectKinematics;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::PointCloud2;
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::Point2d;
+
+class FrontVehicleVelocityEstimator
+{
+public:
+  explicit FrontVehicleVelocityEstimator(const rclcpp::Logger & logger) : logger_(logger) {}
+
+  struct Input
+  {
+    PointCloud2::ConstSharedPtr pointcloud{};
+    DetectedObjects::ConstSharedPtr objects{};
+    Odometry::ConstSharedPtr odometry{};
+  };
+
+  struct Output
+  {
+    DetectedObjects objects{};
+    PointCloud2 nearest_neighbor_pointcloud{};
+  };
+
+  struct Param
+  {
+    int moving_average_num{};
+    float threshold_pointcloud_z_high{};
+    float threshold_pointcloud_z_low{};
+    double threshold_relative_velocity{};
+    double threshold_absolute_velocity{};
+  };
+
+  void setParam(const Param & param) { param_ = param; }
+  Output update(const Input & input);
+
+private:
+  struct ObjectsWithFrontVehicle
+  {
+    DetectedObjects::SharedPtr objects_without_front_vehicle{};
+    DetectedObject front_vehicle{};
+    bool is_front_vehicle = false;
+  };
+
+  rclcpp::Logger logger_;
+
+  // Buffer data
+  Param param_{};
+  std::deque<double> velocity_queue_{};
+  rclcpp::Time prev_time_{};
+  pcl::PointXYZ prev_point_{};
+
+  // Function
+  LinearRing2d createBoxArea(const Point2d size);
+  LinearRing2d createObjectArea(const DetectedObject & object);
+  ObjectsWithFrontVehicle filterFrontVehicle(
+    DetectedObjects::ConstSharedPtr objects, const LinearRing2d & front_area);
+  pcl::PointXYZ getNearestNeighborPoint(
+    const DetectedObject & object, PointCloud2::ConstSharedPtr pointcloud,
+    const Point2d & front_size);
+  double estimateRelativeVelocity(const pcl::PointXYZ & point, const rclcpp::Time & header_time);
+  double estimateAbsoluteVelocity(
+    const double relative_velocity, Odometry::ConstSharedPtr odometry);
+  bool isFrontVehicle(const DetectedObject & object, const LinearRing2d & front_area);
+  bool isWithinVehicle(
+    const DetectedObject & object, const pcl::PointXYZ & point, const Point2d & front_size);
+};
+
+}  // namespace front_vehicle_velocity_estimator
+
+#endif  // FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_

perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp

@@ -0,0 +1,98 @@
+// 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.
+
+#ifndef FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_
+#define FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_
+
+#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace front_vehicle_velocity_estimator
+{
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::PointCloud2;
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::Point2d;
+
+class FrontVehicleVelocityEstimatorNode : public rclcpp::Node
+{
+public:
+  explicit FrontVehicleVelocityEstimatorNode(const rclcpp::NodeOptions & node_options);
+
+  struct NodeParam
+  {
+    double update_rate_hz{};
+  };
+
+private:
+  // Subscriber
+  message_filters::Subscriber<PointCloud2> sub_pointcloud_{};
+  message_filters::Subscriber<DetectedObjects> sub_objects_{};
+  message_filters::Subscriber<Odometry> sub_odometry_{};
+
+  using SyncPolicy =
+    message_filters::sync_policies::ApproximateTime<PointCloud2, DetectedObjects, Odometry>;
+  using Sync = message_filters::Synchronizer<SyncPolicy>;
+  typename std::shared_ptr<Sync> sync_ptr_;
+
+  // Callback
+  void onData(
+    const PointCloud2::ConstSharedPtr pointcloud_msg,
+    const DetectedObjects::ConstSharedPtr object_msg, const Odometry::ConstSharedPtr odometry_msg);
+
+  // Data Buffer
+  PointCloud2::ConstSharedPtr pointcloud_data_{};
+  DetectedObjects::ConstSharedPtr objects_data_{};
+  Odometry::ConstSharedPtr odometry_data_{};
+
+  // Publisher
+  rclcpp::Publisher<DetectedObjects>::SharedPtr pub_objects_{};
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_nearest_neighbor_pointcloud_{};
+
+  // Timer
+  bool isDataReady();
+
+  // Parameter Server
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & params);
+
+  // Parameter
+  NodeParam node_param_{};
+
+  // Core
+  FrontVehicleVelocityEstimator::Input input_{};
+  FrontVehicleVelocityEstimator::Output output_{};
+  FrontVehicleVelocityEstimator::Param core_param_{};
+  std::unique_ptr<FrontVehicleVelocityEstimator> front_vehicle_velocity_estimator_{};
+};
+
+}  // namespace front_vehicle_velocity_estimator
+
+#endif  // FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_

perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml

@@ -0,0 +1,28 @@
+<launch>
+  <!-- Input -->
+  <arg name="input/objects" default="~/input/objects"/>
+  <arg name="input/pointcloud" default="~/input/pointcloud"/>
+  <arg name="input/odometry" default="/localization/kinematic_state"/>
+
+  <!-- Output -->
+  <arg name="output/objects" default="~/output/objects"/>
+  <arg name="debug/nearest_neighbor_pointcloud" default="~/debug/nearest_neighbor_pointcloud"/>
+
+  <!-- Parameter -->
+  <arg name="core_params.moving_average_num" default="1"/>
+
+  <!-- Node -->
+  <node pkg="front_vehicle_velocity_estimator" exec="front_vehicle_velocity_estimator_node" name="front_vehicle_velocity_estimator" output="screen">
+    <!-- Input -->
+    <remap from="~/input/objects" to="$(var input/objects)"/>
+    <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+    <remap from="~/input/odometry" to="$(var input/odometry)"/>
+
+    <!-- Output -->
+    <remap from="~/output/objects" to="$(var output/objects)"/>
+    <remap from="~/debug/nearest_neighbor_pointcloud" to="$(var debug/nearest_neighbor_pointcloud)"/>
+
+    <!-- Parameter -->
+    <param name="core_params.moving_average_num" value="$(var core_params.moving_average_num)"/>
+  </node>
+</launch>

perception/front_vehicle_velocity_estimator/package.xml

@@ -0,0 +1,30 @@
+<?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>front_vehicle_velocity_estimator</name>
+  <version>0.1.0</version>
+  <description>front_vehicle_velocity_estimator</description>
+  <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>