diff --git a/planning/collision_checker/CMakeLists.txt b/planning/collision_checker/CMakeLists.txt new file mode 100644 index 0000000000000..cc21a4ca90d01 --- /dev/null +++ b/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 +) diff --git a/planning/collision_checker/README.md b/planning/collision_checker/README.md new file mode 100644 index 0000000000000..fddd29477ffe4 --- /dev/null +++ b/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` diff --git a/planning/collision_checker/config/collision_checker.param.yaml b/planning/collision_checker/config/collision_checker.param.yaml new file mode 100644 index 0000000000000..250f76cf99259 --- /dev/null +++ b/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] diff --git a/planning/collision_checker/include/collision_checker/node.hpp b/planning/collision_checker/include/collision_checker/node.hpp new file mode 100644 index 0000000000000..4c3e8cf503782 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +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; + +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 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_; + 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_ diff --git a/planning/collision_checker/launch/collision_checker.launch.xml b/planning/collision_checker/launch/collision_checker.launch.xml new file mode 100644 index 0000000000000..aeb00c3f58d70 --- /dev/null +++ b/planning/collision_checker/launch/collision_checker.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/planning/collision_checker/package.xml b/planning/collision_checker/package.xml new file mode 100644 index 0000000000000..4bca5f541dc6f --- /dev/null +++ b/planning/collision_checker/package.xml @@ -0,0 +1,42 @@ + + + + collision_checker + 0.1.0 + The collision_checker package + Shinnosuke Hirakawa + Apache License 2.0 + + Shinnosuke Hirakawa + + ament_cmake + eigen3_cmake_module + + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_tf2 + 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 + tier4_autoware_utils + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/collision_checker/src/node.cpp b/planning/collision_checker/src/node.cpp new file mode 100644 index 0000000000000..fae2cdc473967 --- /dev/null +++ b/planning/collision_checker/src/node.cpp @@ -0,0 +1,322 @@ +// 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. + +#include "collision_checker/node.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace collision_checker +{ +namespace bg = boost::geometry; +using Point2d = bg::model::d2::point_xy; +using Polygon2d = bg::model::polygon; +using tier4_autoware_utils::createPoint; +using tier4_autoware_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 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 + +CollisionCheckerNode::CollisionCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("collision_checker_node", node_options), updater_(this) +{ + // Parameters + { + auto & p = node_param_; + p.use_pointcloud = this->declare_parameter("use_pointcloud", true); + p.use_dynamic_object = this->declare_parameter("use_dynamic_object", true); + p.collision_distance = this->declare_parameter("collision_distance", 0.15); + } + + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + ; + + // Subscribers + sub_pointcloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&CollisionCheckerNode::onPointCloud, this, std::placeholders::_1)); + sub_dynamic_objects_ = this->create_subscription( + "~/input/objects", 1, + std::bind(&CollisionCheckerNode::onDynamicObjects, this, std::placeholders::_1)); + + sub_operation_mode_ = this->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&CollisionCheckerNode::onOperationMode, this, std::placeholders::_1)); + + // Diagnostics Updater + updater_.setHardwareID("collision_checker"); + updater_.add("collision_check", this, &CollisionCheckerNode::checkCollision); + updater_.setPeriod(0.1); +} + +void CollisionCheckerNode::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; + + diagnostic_msgs::msg::DiagnosticStatus status; + if (operation_mode_ptr_->mode == OperationModeState::AUTONOMOUS && is_obstacle_found) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "collision detected"; + 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 CollisionCheckerNode::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + pointcloud_ptr_ = msg; +} + +void CollisionCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) +{ + object_ptr_ = msg; +} + +void CollisionCheckerNode::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_ptr_ = msg; +} + +boost::optional CollisionCheckerNode::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 CollisionCheckerNode::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 CollisionCheckerNode::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 = + object.shape.type == Shape::POLYGON + ? createObjPolygon(transformed_object_pose, object.shape.footprint) + : 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 CollisionCheckerNode::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 collision_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(collision_checker::CollisionCheckerNode)