feat: add collision_checker (autowarefoundation#332)

* feat: add collision_checker

* chore: move package

* chore: move package

* feat: add condition using adapi

* fix: typo

* chore: remove unnecessary comments

planning/collision_checker/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(collision_checker)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED COMPONENTS common)
+
+include_directories(
+  SYSTEM
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "collision_checker::CollisionCheckerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

planning/collision_checker/README.md

@@ -0,0 +1,49 @@
+# Collision Checker
+
+## Purpose
+
+This module subscribes required data (ego-pose, obstacles, etc), and publishes diagnostics if an object is within a specific distance.
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+### Algorithms
+
+### Check data
+
+Check that `collision_checker` receives no ground pointcloud, dynamic objects.
+
+### Get distance to nearest object
+
+Calculate distance between ego vehicle and the nearest object.
+In this function, it calculates the minimum distance between the polygon of ego vehicle and all points in pointclouds and the polygons of dynamic objects.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                           | Type                                                   | Description                                                        |
+| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ |
+| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2`                        | Pointcloud of obstacles which the ego-vehicle should stop or avoid |
+| `/perception/object_recognition/objects`       | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects                                                    |
+| `/tf`                                          | `tf2_msgs::msg::TFMessage`                             | TF                                                                 |
+| `/tf_static`                                   | `tf2_msgs::msg::TFMessage`                             | TF static                                                          |
+
+### Output
+
+| Name           | Type                                    | Description |
+| -------------- | --------------------------------------- | ----------- |
+| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics |
+
+## Parameters
+
+| Name                 | Type     | Description                                                      | Default value |
+| :------------------- | :------- | :--------------------------------------------------------------- | :------------ |
+| `use_pointcloud`     | `bool`   | Use pointcloud as obstacle check                                 | `true`        |
+| `use_dynamic_object` | `bool`   | Use dynamic object as obstacle check                             | `true`        |
+| `collision_distance` | `double` | If objects exist in this distance, publish error diagnostics [m] | 0.15          |
+
+## Assumptions / Known limits
+
+- This module is based on `surround_obstacle_checker`

planning/collision_checker/config/collision_checker.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    use_pointcloud: true # use pointcloud as obstacle check
+    use_dynamic_object: false # use dynamic object as obstacle check
+    collision_distance: 0.1 # Distance at which an object is determined to have collided with ego [m]

planning/collision_checker/include/collision_checker/node.hpp

@@ -0,0 +1,101 @@
+// 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 COLLISION_CHECKER__NODE_HPP_
+#define COLLISION_CHECKER__NODE_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2/utils.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+#include <utility>
+
+namespace collision_checker
+{
+using autoware_adapi_v1_msgs::msg::OperationModeState;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::Shape;
+using vehicle_info_util::VehicleInfo;
+
+using Obstacle = std::pair<double /* distance */, geometry_msgs::msg::Point>;
+
+class CollisionCheckerNode : public rclcpp::Node
+{
+public:
+  explicit CollisionCheckerNode(const rclcpp::NodeOptions & node_options);
+
+  struct NodeParam
+  {
+    bool use_pointcloud;
+    bool use_dynamic_object;
+    double collision_distance;
+  };
+
+private:
+  void checkCollision(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+
+  void onDynamicObjects(const PredictedObjects::ConstSharedPtr msg);
+
+  void onOperationMode(const OperationModeState::ConstSharedPtr msg);
+
+  boost::optional<Obstacle> getNearestObstacle() const;
+
+  boost::optional<Obstacle> getNearestObstacleByPointCloud() const;
+
+  boost::optional<Obstacle> getNearestObstacleByDynamicObject() const;
+
+  boost::optional<geometry_msgs::msg::TransformStamped> getTransform(
+    const std::string & source, const std::string & target, const rclcpp::Time & stamp,
+    double duration_sec) const;
+
+  // ros
+  mutable tf2_ros::Buffer tf_buffer_{get_clock()};
+  mutable tf2_ros::TransformListener tf_listener_{tf_buffer_};
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // publisher and subscriber
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pointcloud_;
+  rclcpp::Subscription<PredictedObjects>::SharedPtr sub_dynamic_objects_;
+  rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
+
+  // parameter
+  NodeParam node_param_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
+
+  // data
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_;
+  PredictedObjects::ConstSharedPtr object_ptr_;
+  OperationModeState::ConstSharedPtr operation_mode_ptr_;
+
+  // Diagnostic Updater
+  diagnostic_updater::Updater updater_;
+};
+}  // namespace collision_checker
+
+#endif  // COLLISION_CHECKER__NODE_HPP_

planning/collision_checker/launch/collision_checker.launch.xml

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="param_path" default="$(find-pkg-share collision_checker)/config/collision_checker.param.yaml"/>
+
+  <arg name="input_objects" default="/perception/object_recognition/objects"/>
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+
+  <node pkg="collision_checker" exec="collision_checker_node" name="collision_checker" output="screen">
+    <param from="$(var param_path)"/>
+    <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>
+    <remap from="~/input/objects" to="$(var input_objects)"/>
+  </node>
+</launch>

planning/collision_checker/package.xml

@@ -0,0 +1,42 @@
+<?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>collision_checker</name>
+  <version>0.1.0</version>
+  <description>The collision_checker package</description>
+  <maintainer email="shinnosuke.hirakawa@tier4.jp">Shinnosuke Hirakawa</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="shinnosuke.hirakawa@tier4.jp">Shinnosuke Hirakawa</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>eigen</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>vehicle_info_util</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>