From ccb9e990673de5fc2bee430d8996ebd35470a764 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 25 Oct 2024 17:34:06 +0900 Subject: [PATCH] add new package autoware_collision_detector Signed-off-by: Go Sakayori --- .../CMakeLists.txt | 33 ++ control/autoware_collision_detector/README.md | 49 +++ .../config/collision_detector.param.yaml | 5 + .../autoware/collision_detector/node.hpp | 102 +++++ .../launch/collision_detector.launch.xml | 12 + .../autoware_collision_detector/package.xml | 44 +++ .../autoware_collision_detector/src/node.cpp | 357 ++++++++++++++++++ 7 files changed, 602 insertions(+) create mode 100644 control/autoware_collision_detector/CMakeLists.txt create mode 100644 control/autoware_collision_detector/README.md create mode 100644 control/autoware_collision_detector/config/collision_detector.param.yaml create mode 100644 control/autoware_collision_detector/include/autoware/collision_detector/node.hpp create mode 100644 control/autoware_collision_detector/launch/collision_detector.launch.xml create mode 100644 control/autoware_collision_detector/package.xml create mode 100644 control/autoware_collision_detector/src/node.cpp diff --git a/control/autoware_collision_detector/CMakeLists.txt b/control/autoware_collision_detector/CMakeLists.txt new file mode 100644 index 000000000000..6fe72d173aaa --- /dev/null +++ b/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 +) diff --git a/control/autoware_collision_detector/README.md b/control/autoware_collision_detector/README.md new file mode 100644 index 000000000000..8249dd2d5f8a --- /dev/null +++ b/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` diff --git a/control/autoware_collision_detector/config/collision_detector.param.yaml b/control/autoware_collision_detector/config/collision_detector.param.yaml new file mode 100644 index 000000000000..c578b6637fd5 --- /dev/null +++ b/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] diff --git a/control/autoware_collision_detector/include/autoware/collision_detector/node.hpp b/control/autoware_collision_detector/include/autoware/collision_detector/node.hpp new file mode 100644 index 000000000000..6ba968c1046f --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +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; + +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 getNearestObstacle() const; + + boost::optional getNearestObstacleByPointCloud() const; + + boost::optional getNearestObstacleByDynamicObject() const; + + boost::optional 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::SharedPtr sub_pointcloud_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::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_ diff --git a/control/autoware_collision_detector/launch/collision_detector.launch.xml b/control/autoware_collision_detector/launch/collision_detector.launch.xml new file mode 100644 index 000000000000..bb3c51cff900 --- /dev/null +++ b/control/autoware_collision_detector/launch/collision_detector.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/control/autoware_collision_detector/package.xml b/control/autoware_collision_detector/package.xml new file mode 100644 index 000000000000..1f9ea7954f75 --- /dev/null +++ b/control/autoware_collision_detector/package.xml @@ -0,0 +1,44 @@ + + + + autoware_collision_detector + 0.1.0 + The collision_detector package + Shinnosuke Hirakawa + Kyoichi Sugahara + Go Sakayori + + Apache License 2.0 + + Shinnosuke Hirakawa + + ament_cmake + eigen3_cmake_module + + autoware_cmake + + autoware_adapi_v1_msgs + autoware_perception_msgs + autoware_utils + autoware_vehicle_info_utils + diagnostic_msgs + diagnostic_updater + eigen + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autoware_collision_detector/src/node.cpp b/control/autoware_collision_detector/src/node.cpp new file mode 100644 index 000000000000..1fa387ec52e4 --- /dev/null +++ b/control/autoware_collision_detector/src/node.cpp @@ -0,0 +1,357 @@ +// Copyright 2024 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 "autoware/collision_detector/node.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace autoware::collision_detector +{ +namespace bg = boost::geometry; +using Point2d = bg::model::d2::point_xy; +using Polygon2d = bg::model::polygon; +using autoware::universe_utils::createPoint; +using autoware::universe_utils::pose2transform; + +namespace +{ + +geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +Polygon2d createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) +{ + geometry_msgs::msg::Polygon transformed_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(pose); + tf2::doTransform(footprint, transformed_polygon, geometry_tf); + + Polygon2d object_polygon; + for (const auto & p : transformed_polygon.points) { + object_polygon.outer().push_back(Point2d(p.x, p.y)); + } + + bg::correct(object_polygon); + + return object_polygon; +} + +Polygon2d createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +{ + const double & length_m = size.x / 2.0; + const double & width_m = size.y / 2.0; + + geometry_msgs::msg::Polygon polygon{}; + + polygon.points.push_back(createPoint32(length_m, -width_m, 0.0)); + polygon.points.push_back(createPoint32(length_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-length_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-length_m, -width_m, 0.0)); + + return createObjPolygon(pose, polygon); +} + +Polygon2d createObjPolygonForCylinder(const geometry_msgs::msg::Pose & pose, const double diameter) +{ + geometry_msgs::msg::Polygon polygon{}; + + const double radius = diameter * 0.5; + // add hexagon points + for (int i = 0; i < 6; ++i) { + const double angle = 2.0 * M_PI * static_cast(i) / 6.0; + const double x = radius * std::cos(angle); + const double y = radius * std::sin(angle); + polygon.points.push_back(createPoint32(x, y, 0.0)); + } + + return createObjPolygon(pose, polygon); +} + +Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m; + const double & width_left_m = vehicle_info.max_lateral_offset_m; + const double & width_right_m = vehicle_info.min_lateral_offset_m; + const double & rear_m = vehicle_info.min_longitudinal_offset_m; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace + +CollisionDetectorNode::CollisionDetectorNode(const rclcpp::NodeOptions & node_options) +: Node("collision_detector_node", node_options), + node_param_(), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), + updater_(this) +{ + // Parameters + { + auto & p = node_param_; + p.use_pointcloud = this->declare_parameter("use_pointcloud"); + p.use_dynamic_object = this->declare_parameter("use_dynamic_object"); + p.collision_distance = this->declare_parameter("collision_distance"); + } + + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + + // Subscribers + sub_pointcloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&CollisionDetectorNode::onPointCloud, this, std::placeholders::_1)); + sub_dynamic_objects_ = this->create_subscription( + "~/input/objects", 1, + std::bind(&CollisionDetectorNode::onDynamicObjects, this, std::placeholders::_1)); + + sub_operation_mode_ = this->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&CollisionDetectorNode::onOperationMode, this, std::placeholders::_1)); + + last_obstacle_found_stamp_ = this->now(); + + // Diagnostics Updater + updater_.setHardwareID("collision_detector"); + updater_.add("collision_check", this, &CollisionDetectorNode::checkCollision); + updater_.setPeriod(0.1); +} + +void CollisionDetectorNode::checkCollision(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (node_param_.use_pointcloud && !pointcloud_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); + return; + } + + if (node_param_.use_dynamic_object && !object_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); + return; + } + + if (!operation_mode_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for operation mode info..."); + return; + } + + const auto nearest_obstacle = getNearestObstacle(); + + const auto is_obstacle_found = + !nearest_obstacle ? false : nearest_obstacle.get().first < node_param_.collision_distance; + + if (is_obstacle_found) { + last_obstacle_found_stamp_ = this->now(); + } + + const auto is_obstacle_found_recently = + (this->now() - last_obstacle_found_stamp_).seconds() < 1.0; + + diagnostic_msgs::msg::DiagnosticStatus status; + if ( + operation_mode_ptr_->mode == OperationModeState::AUTONOMOUS && + (is_obstacle_found || is_obstacle_found_recently)) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "collision detected"; + if (is_obstacle_found) { + stat.addf("Distance to nearest neighbor object", "%lf", nearest_obstacle.get().first); + } + } else { + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } + + stat.summary(status.level, status.message); +} + +void CollisionDetectorNode::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + pointcloud_ptr_ = msg; +} + +void CollisionDetectorNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) +{ + object_ptr_ = msg; +} + +void CollisionDetectorNode::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_ptr_ = msg; +} + +boost::optional CollisionDetectorNode::getNearestObstacle() const +{ + boost::optional nearest_pointcloud{boost::none}; + boost::optional nearest_object{boost::none}; + + if (node_param_.use_pointcloud) { + nearest_pointcloud = getNearestObstacleByPointCloud(); + } + + if (node_param_.use_dynamic_object) { + nearest_object = getNearestObstacleByDynamicObject(); + } + + if (!nearest_pointcloud && !nearest_object) { + return {}; + } + + if (!nearest_pointcloud) { + return nearest_object; + } + + if (!nearest_object) { + return nearest_pointcloud; + } + + return nearest_pointcloud.get().first < nearest_object.get().first ? nearest_pointcloud + : nearest_object; +} + +boost::optional CollisionDetectorNode::getNearestObstacleByPointCloud() const +{ + const auto transform_stamped = + getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); + + geometry_msgs::msg::Point nearest_point; + auto minimum_distance = std::numeric_limits::max(); + + if (!transform_stamped) { + return {}; + } + + Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast(); + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); + pcl::transformPointCloud(transformed_pointcloud, transformed_pointcloud, isometry); + + const auto ego_polygon = createSelfPolygon(vehicle_info_); + + for (const auto & p : transformed_pointcloud) { + Point2d boost_point(p.x, p.y); + + const auto distance_to_object = bg::distance(ego_polygon, boost_point); + + if (distance_to_object < minimum_distance) { + nearest_point = createPoint(p.x, p.y, p.z); + minimum_distance = distance_to_object; + } + } + + return std::make_pair(minimum_distance, nearest_point); +} + +boost::optional CollisionDetectorNode::getNearestObstacleByDynamicObject() const +{ + const auto transform_stamped = + getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); + + geometry_msgs::msg::Point nearest_point; + auto minimum_distance = std::numeric_limits::max(); + + if (!transform_stamped) { + return {}; + } + + tf2::Transform tf_src2target; + tf2::fromMsg(transform_stamped.get().transform, tf_src2target); + + const auto ego_polygon = createSelfPolygon(vehicle_info_); + + for (const auto & object : object_ptr_->objects) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + + tf2::Transform tf_src2object; + tf2::fromMsg(object_pose, tf_src2object); + + geometry_msgs::msg::Pose transformed_object_pose; + tf2::toMsg(tf_src2target.inverse() * tf_src2object, transformed_object_pose); + + const auto object_polygon = [&]() { + switch (object.shape.type) { + case Shape::POLYGON: + return createObjPolygon(transformed_object_pose, object.shape.footprint); + case Shape::CYLINDER: + return createObjPolygonForCylinder(transformed_object_pose, object.shape.dimensions.x); + case Shape::BOUNDING_BOX: + return createObjPolygon(transformed_object_pose, object.shape.dimensions); + default: + // node return warning + RCLCPP_WARN(this->get_logger(), "Unsupported shape type: %d", object.shape.type); + return createObjPolygon(transformed_object_pose, object.shape.dimensions); + } + }(); + + const auto distance_to_object = bg::distance(ego_polygon, object_polygon); + + if (distance_to_object < minimum_distance) { + nearest_point = object_pose.position; + minimum_distance = distance_to_object; + } + } + + return std::make_pair(minimum_distance, nearest_point); +} + +boost::optional CollisionDetectorNode::getTransform( + const std::string & source, const std::string & target, const rclcpp::Time & stamp, + double duration_sec) const +{ + geometry_msgs::msg::TransformStamped transform_stamped; + + try { + transform_stamped = + tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec)); + } catch (tf2::TransformException & ex) { + return {}; + } + + return transform_stamped; +} + +} // namespace autoware::collision_detector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::collision_detector::CollisionDetectorNode)