add new package autoware_collision_detector

Signed-off-by: Go Sakayori <[email protected]>

control/autoware_collision_detector/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_collision_detector)
+
+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 "autoware::collision_detector::CollisionDetectorNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

control/autoware_collision_detector/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_detector` 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_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                                 | `false`       |
+| `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.1           |
+
+## Assumptions / Known limits
+
+- This module is based on `surround_obstacle_checker`

control/autoware_collision_detector/config/collision_detector.param.yaml

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

control/autoware_collision_detector/include/autoware/collision_detector/node.hpp

@@ -0,0 +1,102 @@
+// 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 AUTOWARE__COLLISION_DETECTOR__NODE_HPP_
+#define AUTOWARE__COLLISION_DETECTOR__NODE_HPP_
+
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <boost/optional.hpp>
+
+#include <tf2/utils.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+#include <utility>
+
+namespace autoware::collision_detector
+{
+using autoware::vehicle_info_utils::VehicleInfo;
+using autoware_adapi_v1_msgs::msg::OperationModeState;
+using autoware_perception_msgs::msg::PredictedObjects;
+using autoware_perception_msgs::msg::Shape;
+
+using Obstacle = std::pair<double /* distance */, geometry_msgs::msg::Point>;
+
+class CollisionDetectorNode : public rclcpp::Node
+{
+public:
+  explicit CollisionDetectorNode(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_;
+  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+
+  // data
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_;
+  PredictedObjects::ConstSharedPtr object_ptr_;
+  OperationModeState::ConstSharedPtr operation_mode_ptr_;
+  rclcpp::Time last_obstacle_found_stamp_;
+
+  // Diagnostic Updater
+  diagnostic_updater::Updater updater_;
+};
+}  // namespace autoware::collision_detector
+
+#endif  // AUTOWARE__COLLISION_DETECTOR__NODE_HPP_

control/autoware_collision_detector/launch/collision_detector.launch.xml

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="param_path" default="$(find-pkg-share autoware_collision_detector)/config/collision_detector.param.yaml"/>
+
+  <arg name="input_objects" default="/perception/object_recognition/objects"/>
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+
+  <node pkg="autoware_collision_detector" exec="autoware_collision_detector_node" name="collision_detector" 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>

control/autoware_collision_detector/package.xml

@@ -0,0 +1,44 @@
+<?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>autoware_collision_detector</name>
+  <version>0.1.0</version>
+  <description>The collision_detector package</description>
+  <maintainer email="[email protected]">Shinnosuke Hirakawa</maintainer>
+  <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
+  <maintainer email="[email protected]">Go Sakayori</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">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_perception_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>autoware_vehicle_info_utils</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>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>