feat: add obstacle collision checker package

.gitignore

@@ -13,4 +13,3 @@ log/
 
 # Python
 *.pyc
-

control/obstacle_collision_checker/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.5)
+project(obstacle_collision_checker)
+
+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 -Werror)
+endif()
+
+
+include_directories(
+  include
+  ${PCL_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Target
+## obstacle_collision_checker_node
+ament_auto_add_library(obstacle_collision_checker SHARED
+  src/obstacle_collision_checker_node/obstacle_collision_checker.cpp
+  src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp
+)
+
+rclcpp_components_register_node(obstacle_collision_checker
+  PLUGIN "obstacle_collision_checker::ObstacleCollisionCheckerNode"
+  EXECUTABLE obstacle_collision_checker_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

control/obstacle_collision_checker/README.md

@@ -0,0 +1,86 @@
+# obstacle_collision_checker
+
+## Purpose
+
+`obstacle_collision_checker` is a module to check obstacle collision for predicted trajectory and publish diagnostic errors if collision is found.
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+```plantuml
+@startuml
+skinparam monochrome true
+
+title obstacle collision checker : update
+start
+
+:calculate braking distance;
+
+:resampling trajectory;
+note right
+to reduce calculation cost
+end note
+:filter point cloud by trajectory;
+
+:create vehicle foot prints;
+
+:create vehicle passing area;
+
+partition will_collide {
+
+while (has next ego vehicle foot print) is (yes)
+  :found collision with obstacle foot print;
+  if (has collision with obstacle) then (yes)
+      :set diag to ERROR;
+      stop
+  endif
+end while (no)
+:set diag to OK;
+stop
+}
+
+@enduml
+```
+
+### Algorithms
+
+### Check data
+
+Check that `obstacle_collision_checker` receives no ground pointcloud, predicted_trajectory, reference trajectory, and current velocity data.
+
+### Diagnostic update
+
+If any collision is found on predicted path, this module sets `ERROR` level as diagnostic status else sets `OK`.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                           | Type                                           | Description                                                        |
+| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ |
+| `~/input/trajectory`                           | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory                                               |
+| `~/input/trajectory`                           | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory                                               |
+| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2`                | Pointcloud of obstacles which the ego-vehicle should stop or avoid |
+| `/tf`                                          | `tf2_msgs::msg::TFMessage`                     | TF                                                                 |
+| `/tf_static`                                   | `tf2_msgs::msg::TFMessage`                     | TF static                                                          |
+
+### Output
+
+| Name             | Type                                   | Description              |
+| ---------------- | -------------------------------------- | ------------------------ |
+| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization |
+
+## Parameters
+
+| Name                | Type     | Description                                        | Default value |
+| :------------------ | :------- | :------------------------------------------------- | :------------ |
+| `delay_time`        | `double` | Delay time of vehicle [s]                          | 0.3           |
+| `footprint_margin`  | `double` | Foot print margin [m]                              | 0.0           |
+| `max_deceleration`  | `double` | Max deceleration for ego vehicle to stop [m/s^2]   | 2.0           |
+| `resample_interval` | `double` | Interval for resampling trajectory [m]             | 0.3           |
+| `search_radius`     | `double` | Search distance from trajectory to point cloud [m] | 5.0           |
+
+## Assumptions / Known limits
+
+To perform proper collision check, it is necessary to get probably predicted trajectory and obstacle pointclouds without noise.

control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    # Node
+    update_rate: 10.0
+
+    # Core
+    delay_time: 0.3 # delay time of vehicle [s]
+    footprint_margin: 0.0 # margin for footprint [m]
+    max_deceleration: 2.0 # max deceleration [m/ss]
+    resample_interval: 0.3 # interval distance to resample point cloud [m]
+    search_radius: 5.0 # search distance from trajectory to point cloud [m]

control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp

@@ -0,0 +1,107 @@
+// Copyright 2020 Tier IV, Inc. All rights reserved.
+//
+// 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 OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
+#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_
+
+#include <autoware_utils/geometry/boost_geometry.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <boost/optional.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace obstacle_collision_checker
+{
+using autoware_utils::LinearRing2d;
+
+struct Param
+{
+  double delay_time;
+  double footprint_margin;
+  double max_deceleration;
+  double resample_interval;
+  double search_radius;
+};
+
+struct Input
+{
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose;
+  geometry_msgs::msg::Twist::ConstSharedPtr current_twist;
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud;
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory;
+};
+
+struct Output
+{
+  std::map<std::string, double> processing_time_map;
+  bool will_collide;
+  autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory;
+  std::vector<LinearRing2d> vehicle_footprints;
+  std::vector<LinearRing2d> vehicle_passing_areas;
+};
+
+class ObstacleCollisionChecker
+{
+public:
+  explicit ObstacleCollisionChecker(rclcpp::Node & node);
+  Output update(const Input & input);
+
+  void setParam(const Param & param) { param_ = param; }
+
+private:
+  Param param_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
+
+  //! This function assumes the input trajectory is sampled dense enough
+  static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
+    const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval);
+
+  static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory(
+    const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length);
+
+  static std::vector<LinearRing2d> createVehicleFootprints(
+    const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param,
+    const vehicle_info_util::VehicleInfo & vehicle_info);
+
+  static std::vector<LinearRing2d> createVehiclePassingAreas(
+    const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static LinearRing2d createHullFromFootprints(
+    const LinearRing2d & area1, const LinearRing2d & area2);
+
+  static bool willCollide(
+    const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
+    const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static bool hasCollision(
+    const pcl::PointCloud<pcl::PointXYZ> & obstacle_pointcloud,
+    const LinearRing2d & vehicle_footprint);
+};
+}  // namespace obstacle_collision_checker
+
+#endif  // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_

control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp

@@ -0,0 +1,110 @@
+// Copyright 2020 Tier IV, Inc. All rights reserved.
+//
+// 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 OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_
+#define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_
+
+#include "obstacle_collision_checker/obstacle_collision_checker.hpp"
+
+#include <autoware_utils/geometry/geometry.hpp>
+#include <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/processing_time_publisher.hpp>
+#include <autoware_utils/ros/self_pose_listener.hpp>
+#include <autoware_utils/ros/transform_listener.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace obstacle_collision_checker
+{
+struct NodeParam
+{
+  double update_rate;
+};
+
+class ObstacleCollisionCheckerNode : public rclcpp::Node
+{
+public:
+  explicit ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Subscriber
+  std::shared_ptr<autoware_utils::SelfPoseListener> self_pose_listener_;
+  std::shared_ptr<autoware_utils::TransformListener> transform_listener_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_obstacle_pointcloud_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_reference_trajectory_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_predicted_trajectory_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+
+  // Data Buffer
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+  geometry_msgs::msg::Twist::ConstSharedPtr current_twist_;
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_;
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_;
+
+  // Callback
+  void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+  void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg);
+  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+  // Publisher
+  std::shared_ptr<autoware_utils::DebugPublisher> debug_publisher_;
+  std::shared_ptr<autoware_utils::ProcessingTimePublisher> time_publisher_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+  void initTimer(double period_s);
+
+  bool isDataReady();
+  bool isDataTimeout();
+  void onTimer();
+
+  // Parameter
+  NodeParam node_param_;
+  Param param_;
+
+  // Dynamic Reconfigure
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult paramCallback(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  // Core
+  Input input_;
+  Output output_;
+  std::unique_ptr<ObstacleCollisionChecker> obstacle_collision_checker_;
+
+  // Diagnostic Updater
+  diagnostic_updater::Updater updater_;
+
+  void checkLaneDeparture(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  // Visualization
+  visualization_msgs::msg::MarkerArray createMarkerArray() const;
+};
+}  // namespace obstacle_collision_checker
+
+#endif  // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_NODE_HPP_