diff --git a/control/obstacle_collision_checker/CMakeLists.txt b/control/obstacle_collision_checker/CMakeLists.txt new file mode 100644 index 0000000000000..9a329dfaedf3f --- /dev/null +++ b/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 +) diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md new file mode 100644 index 0000000000000..5478db1f5a135 --- /dev/null +++ b/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. diff --git a/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml b/control/obstacle_collision_checker/config/obstacle_collision_checker.param.yaml new file mode 100644 index 0000000000000..883b4d8259f49 --- /dev/null +++ b/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] diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp new file mode 100644 index 0000000000000..ba609407386d4 --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +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 processing_time_map; + bool will_collide; + autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + std::vector vehicle_footprints; + std::vector 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 createVehicleFootprints( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const vehicle_info_util::VehicleInfo & vehicle_info); + + static std::vector createVehiclePassingAreas( + const std::vector & vehicle_footprints); + + static LinearRing2d createHullFromFootprints( + const LinearRing2d & area1, const LinearRing2d & area2); + + static bool willCollide( + const pcl::PointCloud & obstacle_pointcloud, + const std::vector & vehicle_footprints); + + static bool hasCollision( + const pcl::PointCloud & obstacle_pointcloud, + const LinearRing2d & vehicle_footprint); +}; +} // namespace obstacle_collision_checker + +#endif // OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp new file mode 100644 index 0000000000000..591e1f1a0ea69 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +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 self_pose_listener_; + std::shared_ptr transform_listener_; + rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; + rclcpp::Subscription::SharedPtr + sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_trajectory_; + rclcpp::Subscription::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 debug_publisher_; + std::shared_ptr 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 & parameters); + + // Core + Input input_; + Output output_; + std::unique_ptr 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_ diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp new file mode 100644 index 0000000000000..5d80924603974 --- /dev/null +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/util/create_vehicle_footprint.hpp @@ -0,0 +1,47 @@ +// 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__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ + +#include +#include + +inline autoware_utils::LinearRing2d createVehicleFootprint( + const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) +{ + using autoware_utils::LinearRing2d; + using autoware_utils::Point2d; + + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m + i.wheel_base_m + margin; + const double x_center = i.wheel_base_m / 2.0; + const double x_rear = -(i.rear_overhang_m + margin); + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; + const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_center, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_center, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} + +#endif // OBSTACLE_COLLISION_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml new file mode 100755 index 0000000000000..1391692ad6b8a --- /dev/null +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml new file mode 100644 index 0000000000000..71ff20b67d013 --- /dev/null +++ b/control/obstacle_collision_checker/package.xml @@ -0,0 +1,35 @@ + + + obstacle_collision_checker + 0.1.0 + The obstacle_collision_checker package + Satoshi Tanaka + Kenji Miyake + Apache License 2.0 + + ament_cmake + + autoware_auto_planning_msgs + autoware_utils + boost + diagnostic_updater + eigen + geometry_msgs + nav_msgs + pcl_ros + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp new file mode 100644 index 0000000000000..221bc5d6349cb --- /dev/null +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -0,0 +1,261 @@ +// 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. + +#include "obstacle_collision_checker/obstacle_collision_checker.hpp" + +#include "obstacle_collision_checker/util/create_vehicle_footprint.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +namespace +{ +pcl::PointCloud getTransformedPointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, + const geometry_msgs::msg::Transform & transform) +{ + const Eigen::Matrix4f transform_matrix = tf2::transformToEigen(transform).matrix().cast(); + + sensor_msgs::msg::PointCloud2 transformed_msg; + pcl_ros::transformPointCloud(transform_matrix, pointcloud_msg, transformed_msg); + + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(transformed_msg, transformed_pointcloud); + + return transformed_pointcloud; +} + +pcl::PointCloud filterPointCloudByTrajectory( + const pcl::PointCloud & pointcloud, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) +{ + pcl::PointCloud filtered_pointcloud; + for (const auto & trajectory_point : trajectory.points) { + for (const auto & point : pointcloud.points) { + const double dx = trajectory_point.pose.position.x - point.x; + const double dy = trajectory_point.pose.position.y - point.y; + if (std::hypot(dx, dy) < radius) { + filtered_pointcloud.points.push_back(point); + } + } + } + return filtered_pointcloud; +} + +double calcBrakingDistance( + const double abs_velocity, const double max_deceleration, const double delay_time) +{ + const double idling_distance = abs_velocity * delay_time; + const double braking_distance = (abs_velocity * abs_velocity) / (2.0 * max_deceleration); + return idling_distance + braking_distance; +} + +} // namespace + +namespace obstacle_collision_checker +{ +ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) +: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) +{ +} + +Output ObstacleCollisionChecker::update(const Input & input) +{ + Output output; + autoware_utils::StopWatch stop_watch; + + // resample trajectory by braking distance + constexpr double min_velocity = 0.01; + const auto & raw_abs_velocity = std::abs(input.current_twist->linear.x); + const auto abs_velocity = raw_abs_velocity < min_velocity ? 0.0 : raw_abs_velocity; + const auto braking_distance = + calcBrakingDistance(abs_velocity, param_.max_deceleration, param_.delay_time); + output.resampled_trajectory = cutTrajectory( + resampleTrajectory(*input.predicted_trajectory, param_.resample_interval), braking_distance); + output.processing_time_map["resampleTrajectory"] = stop_watch.toc(true); + + // resample pointcloud + const auto obstacle_pointcloud = + getTransformedPointCloud(*input.obstacle_pointcloud, input.obstacle_transform->transform); + const auto filtered_obstacle_pointcloud = filterPointCloudByTrajectory( + obstacle_pointcloud, output.resampled_trajectory, param_.search_radius); + + output.vehicle_footprints = + createVehicleFootprints(output.resampled_trajectory, param_, vehicle_info_); + output.processing_time_map["createVehicleFootprints"] = stop_watch.toc(true); + + output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); + output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); + + output.will_collide = willCollide(filtered_obstacle_pointcloud, output.vehicle_passing_areas); + output.processing_time_map["willCollide"] = stop_watch.toc(true); + + return output; +} + +autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval) +{ + autoware_auto_planning_msgs::msg::Trajectory resampled; + resampled.header = trajectory.header; + + resampled.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = autoware_utils::fromMsg(resampled.points.back().pose.position).to_2d(); + const auto p2 = autoware_utils::fromMsg(point.pose.position).to_2d(); + + if (boost::geometry::distance(p1, p2) > interval) { + resampled.points.push_back(point); + } + } + resampled.points.push_back(trajectory.points.back()); + + return resampled; +} + +autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length) +{ + autoware_auto_planning_msgs::msg::Trajectory cut; + cut.header = trajectory.header; + + double total_length = 0.0; + cut.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = autoware_utils::fromMsg(point.pose.position); + const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + const auto remain_distance = length - total_length; + + // Over length + if (remain_distance <= 0) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + autoware_auto_planning_msgs::msg::TrajectoryPoint p; + p.pose.position.x = p_interpolated.x(); + p.pose.position.y = p_interpolated.y(); + p.pose.position.z = p_interpolated.z(); + p.pose.orientation = point.pose.orientation; + + cut.points.push_back(p); + break; + } + + cut.points.push_back(point); + total_length += points_distance; + } + + return cut; +} + +std::vector ObstacleCollisionChecker::createVehicleFootprints( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + // Create vehicle footprint in base_link coordinate + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info, param.footprint_margin); + + // Create vehicle footprint on each TrajectoryPoint + std::vector vehicle_footprints; + for (const auto & p : trajectory.points) { + vehicle_footprints.push_back(autoware_utils::transformVector( + local_vehicle_footprint, autoware_utils::pose2transform(p.pose))); + } + + return vehicle_footprints; +} + +std::vector ObstacleCollisionChecker::createVehiclePassingAreas( + const std::vector & vehicle_footprints) +{ + // Create hull from two adjacent vehicle footprints + std::vector areas; + for (size_t i = 0; i < vehicle_footprints.size() - 1; ++i) { + const auto & footprint1 = vehicle_footprints.at(i); + const auto & footprint2 = vehicle_footprints.at(i + 1); + areas.push_back(createHullFromFootprints(footprint1, footprint2)); + } + + return areas; +} + +LinearRing2d ObstacleCollisionChecker::createHullFromFootprints( + const LinearRing2d & area1, const LinearRing2d & area2) +{ + autoware_utils::MultiPoint2d combined; + for (const auto & p : area1) { + combined.push_back(p); + } + for (const auto & p : area2) { + combined.push_back(p); + } + LinearRing2d hull; + boost::geometry::convex_hull(combined, hull); + return hull; +} + +bool ObstacleCollisionChecker::willCollide( + const pcl::PointCloud & obstacle_pointcloud, + const std::vector & vehicle_footprints) +{ + for (const auto & vehicle_footprint : vehicle_footprints) { + if (hasCollision(obstacle_pointcloud, vehicle_footprint)) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide"); + return true; + } + } + + return false; +} + +bool ObstacleCollisionChecker::hasCollision( + const pcl::PointCloud & obstacle_pointcloud, + const LinearRing2d & vehicle_footprint) +{ + for (const auto & point : obstacle_pointcloud) { + if (boost::geometry::within(autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { + RCLCPP_WARN( + rclcpp::get_logger("obstacle_collision_checker"), + "[ObstacleCollisionChecker] Collide to Point x: %f y: %f", point.x, point.y); + return true; + } + } + + return false; +} +} // namespace obstacle_collision_checker diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp new file mode 100644 index 0000000000000..9817721d9c37a --- /dev/null +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -0,0 +1,341 @@ +// 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. + +#include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" + +#include "obstacle_collision_checker/util/create_vehicle_footprint.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace +{ +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = it->template get_value(); + } +} +} // namespace + +namespace obstacle_collision_checker +{ +ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("obstacle_collision_checker_node", node_options), updater_(this) +{ + using std::placeholders::_1; + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + + // Core Parameter + param_.delay_time = declare_parameter("delay_time", 0.3); + param_.footprint_margin = declare_parameter("footprint_margin", 0.0); + param_.max_deceleration = declare_parameter("max_deceleration", 2.0); + param_.resample_interval = declare_parameter("resample_interval", 0.5); + param_.search_radius = declare_parameter("search_radius", 5.0); + + // Dynamic Reconfigure + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ObstacleCollisionCheckerNode::paramCallback, this, _1)); + + // Core + obstacle_collision_checker_ = std::make_unique(*this); + obstacle_collision_checker_->setParam(param_); + + // Subscriber + self_pose_listener_ = std::make_shared(this); + transform_listener_ = std::make_shared(this); + + sub_obstacle_pointcloud_ = create_subscription( + "input/obstacle_pointcloud", 1, + std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); + sub_reference_trajectory_ = create_subscription( + "input/reference_trajectory", 1, + std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = create_subscription( + "input/predicted_trajectory", 1, + std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); + sub_odom_ = create_subscription( + "input/odometry", 1, std::bind(&ObstacleCollisionCheckerNode::onOdom, this, _1)); + + // Publisher + debug_publisher_ = std::make_shared(this, "debug/marker"); + time_publisher_ = std::make_shared(this); + + // Diagnostic Updater + updater_.setHardwareID("obstacle_collision_checker"); + + updater_.add( + "obstacle_collision_checker", this, &ObstacleCollisionCheckerNode::checkLaneDeparture); + + // Wait for first self pose + self_pose_listener_->waitForFirstPose(); + + // Timer + initTimer(1.0 / node_param_.update_rate); +} + +void ObstacleCollisionCheckerNode::onObstaclePointcloud( + const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + obstacle_pointcloud_ = msg; +} + +void ObstacleCollisionCheckerNode::onReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void ObstacleCollisionCheckerNode::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +void ObstacleCollisionCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_twist_ = std::make_shared(msg->twist.twist); +} + +void ObstacleCollisionCheckerNode::initTimer(double period_s) +{ + auto timer_callback = std::bind(&ObstacleCollisionCheckerNode::onTimer, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +bool ObstacleCollisionCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_pose..."); + return false; + } + + if (!obstacle_pointcloud_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for obstacle_pointcloud msg..."); + return false; + } + + if (!obstacle_transform_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for obstacle_transform..."); + return false; + } + + if (!reference_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for reference_trajectory msg..."); + return false; + } + + if (!predicted_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for predicted_trajectory msg..."); + return false; + } + + if (!current_twist_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_twist msg..."); + return false; + } + + return true; +} + +bool ObstacleCollisionCheckerNode::isDataTimeout() +{ + const auto now = this->now(); + + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp).seconds() - now.seconds(); + if (pose_time_diff > th_pose_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000 /* ms */, "pose is timeout..."); + return true; + } + + return false; +} + +void ObstacleCollisionCheckerNode::onTimer() +{ + current_pose_ = self_pose_listener_->getCurrentPose(); + if (obstacle_pointcloud_) { + const auto & header = obstacle_pointcloud_->header; + obstacle_transform_ = transform_listener_->getTransform( + "map", header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); + } + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + input_.current_pose = current_pose_; + input_.obstacle_pointcloud = obstacle_pointcloud_; + input_.obstacle_transform = obstacle_transform_; + input_.reference_trajectory = reference_trajectory_; + input_.predicted_trajectory = predicted_trajectory_; + input_.current_twist = current_twist_; + + output_ = obstacle_collision_checker_->update(input_); + + updater_.force_update(); + + debug_publisher_->publish("marker_array", createMarkerArray()); + + time_publisher_->publish(output_.processing_time_map); +} + +rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + Param param; + try { + update_param(parameters, "delay_time", param.delay_time); + update_param(parameters, "footprint_margin", param.footprint_margin); + update_param(parameters, "max_deceleration", param.max_deceleration); + update_param(parameters, "resample_interval", param.resample_interval); + update_param(parameters, "search_radius", param.search_radius); + param_ = param; + if (obstacle_collision_checker_) { + obstacle_collision_checker_->setParam(param_); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + return result; +} + +void ObstacleCollisionCheckerNode::checkLaneDeparture( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (output_.will_collide) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "vehicle will collide with obstacles"; + } + + stat.summary(level, msg); +} + +visualization_msgs::msg::MarkerArray ObstacleCollisionCheckerNode::createMarkerArray() const +{ + using autoware_utils::createDefaultMarker; + using autoware_utils::createMarkerColor; + using autoware_utils::createMarkerScale; + + visualization_msgs::msg::MarkerArray marker_array; + + const auto base_link_z = current_pose_->pose.position.z; + + if (output_.resampled_trajectory.points.size() >= 2) { + // Line of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", this->now(), "resampled_trajectory_line", 0, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.05, 0, 0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + for (const auto & p : output_.resampled_trajectory.points) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + + // Points of resampled_trajectory + { + auto marker = createDefaultMarker( + "map", this->now(), "resampled_trajectory_points", 0, + visualization_msgs::msg::Marker::SPHERE_LIST, createMarkerScale(0.1, 0.1, 0.1), + createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & p : output_.resampled_trajectory.points) { + marker.points.push_back(p.pose.position); + marker.colors.push_back(marker.color); + } + + marker_array.markers.push_back(marker); + } + } + + // Vehicle Footprints + { + const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); + const auto color_will_collide = createMarkerColor(1.0, 0.0, 0.0, 0.5); + + auto color = color_ok; + if (output_.will_collide) { + color = color_will_collide; + } + + auto marker = createDefaultMarker( + "map", this->now(), "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0, 0), color); + + for (const auto & vehicle_footprint : output_.vehicle_footprints) { + for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { + const auto p1 = vehicle_footprint.at(i); + const auto p2 = vehicle_footprint.at(i + 1); + + marker.points.push_back(toMsg(p1.to_3d(base_link_z))); + marker.points.push_back(toMsg(p2.to_3d(base_link_z))); + } + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} +} // namespace obstacle_collision_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_collision_checker::ObstacleCollisionCheckerNode)