From a8b42b1f97e8c1459fbfc583e7149240647b1049 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Mon, 17 Apr 2023 06:20:23 +0300 Subject: [PATCH] feat(predicted_path_checker): check predicted trajectory to avoid collisions planning can not handle (#2528) Signed-off-by: Berkay Karaman --- control/predicted_path_checker/CMakeLists.txt | 37 ++ control/predicted_path_checker/README.md | 23 + .../config/predicted_path_checker.param.yaml | 21 + .../collision_checker.hpp | 128 +++++ .../predicted_path_checker/debug_marker.hpp | 92 +++ .../predicted_path_checker_node.hpp | 175 ++++++ .../include/predicted_path_checker/utils.hpp | 94 ++++ .../launch/predicted_path_checker.launch.xml | 21 + control/predicted_path_checker/package.xml | 45 ++ .../collision_checker.cpp | 314 +++++++++++ .../debug_marker.cpp | 329 +++++++++++ .../predicted_path_checker_node.cpp | 525 ++++++++++++++++++ .../src/predicted_path_checker_node/utils.cpp | 345 ++++++++++++ 13 files changed, 2149 insertions(+) create mode 100644 control/predicted_path_checker/CMakeLists.txt create mode 100644 control/predicted_path_checker/README.md create mode 100644 control/predicted_path_checker/config/predicted_path_checker.param.yaml create mode 100644 control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp create mode 100644 control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp create mode 100644 control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp create mode 100644 control/predicted_path_checker/include/predicted_path_checker/utils.hpp create mode 100755 control/predicted_path_checker/launch/predicted_path_checker.launch.xml create mode 100644 control/predicted_path_checker/package.xml create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp diff --git a/control/predicted_path_checker/CMakeLists.txt b/control/predicted_path_checker/CMakeLists.txt new file mode 100644 index 0000000000000..d5e9de0addc03 --- /dev/null +++ b/control/predicted_path_checker/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(predicted_path_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + + + +ament_auto_add_library(predicted_path_checker SHARED + src/predicted_path_checker_node/predicted_path_checker_node.cpp + src/predicted_path_checker_node/collision_checker.cpp + src/predicted_path_checker_node/utils.cpp + src/predicted_path_checker_node/debug_marker.cpp + +) + +target_include_directories(predicted_path_checker + SYSTEM PUBLIC + ${EIGEN3_INCLUDE_DIR} +) + +rclcpp_components_register_node(predicted_path_checker + PLUGIN "predicted_path_checker::PredictedPathCheckerNode" + EXECUTABLE predicted_path_checker_node +) + +if(BUILD_TESTING) + +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md new file mode 100644 index 0000000000000..ee904472958ab --- /dev/null +++ b/control/predicted_path_checker/README.md @@ -0,0 +1,23 @@ +# predicted_path_checker + +## Purpose + +## Inner-workings / Algorithms + +### Flow chart + +### Algorithms + +### Check data + +### Diagnostic update + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits diff --git a/control/predicted_path_checker/config/predicted_path_checker.param.yaml b/control/predicted_path_checker/config/predicted_path_checker.param.yaml new file mode 100644 index 0000000000000..cf2621b333491 --- /dev/null +++ b/control/predicted_path_checker/config/predicted_path_checker.param.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + max_deceleration: 1.5 + delay_time: 0.17 + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + stop_margin: 0.5 # [m] + min_trajectory_check_length: 1.5 + trajectory_check_time: 3.0 + resample_interval: 0.5 + distinct_point_distance_threshold: 0.3 + distinct_point_yaw_threshold: 5.0 # [deg] + + collision_checker_params: + width_margin: 0.1 + longitudinal_margin: 0.15 + enable_z_axis_obstacle_filtering: false + z_axis_filtering_buffer: 0.3 + chattering_threshold: 0.2 diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp new file mode 100644 index 0000000000000..9cc66af3e070f --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -0,0 +1,128 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using PointArray = std::vector; + +namespace bg = boost::geometry; + +struct CollisionCheckerParam +{ + double width_margin; + double z_axis_filtering_buffer; + bool enable_z_axis_obstacle_filtering; + double chattering_threshold; +}; + +struct PredictedObjectWithDetectionTime +{ + explicit PredictedObjectWithDetectionTime( + const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) + : detection_time(t), point(p), object(std::move(obj)) + { + } + + rclcpp::Time detection_time; + geometry_msgs::msg::Point point; + PredictedObject object; +}; + +class CollisionChecker +{ +public: + explicit CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr); + + boost::optional> + checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, + PredictedObjects::ConstSharedPtr dynamic_objects); + + void setParam(const CollisionCheckerParam & param); + +private: + // Functions + + boost::optional> checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max); + + boost::optional> checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max); + + void updatePredictedObjectHistory(const rclcpp::Time & now) + { + for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold; + + if (expired) { + itr = predicted_object_history_.erase(itr); + continue; + } + + itr++; + } + } + + // Parameter + CollisionCheckerParam param_; + + // Variables + std::shared_ptr debug_ptr_; + rclcpp::Node * node_; + vehicle_info_util::VehicleInfo vehicle_info_; + std::vector predicted_object_history_{}; +}; +} // namespace predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp new file mode 100644 index 0000000000000..51feee6b3a70c --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -0,0 +1,92 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY + +#include +#include +#include + +namespace predicted_path_checker +{ + +enum class PolygonType : int8_t { Vehicle = 0, Collision }; + +enum class PointType : int8_t { Stop = 0 }; + +enum class PoseType : int8_t { Stop = 0, Collision }; + +class PredictedPathCheckerDebugNode +{ +public: + explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front); + + ~PredictedPathCheckerDebugNode() {} + + bool pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + + bool pushPolygon(const std::vector & polygon, const PolygonType & type); + + bool pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type); + + bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); + + bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); + + bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); + + visualization_msgs::msg::MarkerArray makeVirtualWallMarker(); + + visualization_msgs::msg::MarkerArray makeVisualizationMarker(); + + void publish(); + +private: + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Node * node_; + double base_link2front_; + + std::shared_ptr stop_pose_ptr_; + std::shared_ptr collision_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::vector> vehicle_polygons_; + std::vector> collision_polygons_; + std::vector> vehicle_polyhedrons_; + std::vector> collision_polyhedrons_; +}; + +} // namespace predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp new file mode 100644 index 0000000000000..cee982bdcba8b --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -0,0 +1,175 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct NodeParam +{ + double update_rate; + double delay_time; + double max_deceleration; + double resample_interval; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + double stop_margin; + double min_trajectory_check_length; + double trajectory_check_time; + double distinct_point_distance_threshold; + double distinct_point_yaw_threshold; +}; + +enum class State { + DRIVE = 0, + EMERGENCY = 1, + PAUSE = 2, +}; + +class PredictedPathCheckerNode : public rclcpp::Node +{ +public: + explicit PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + + // Subscriber + std::shared_ptr self_pose_listener_; + std::shared_ptr transform_listener_; + tf2_ros::Buffer tf_buffer_{get_clock()}; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::SharedPtr + sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_trajectory_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_accel_; + component_interface_utils::Subscription::SharedPtr sub_pause_state_; + + // Client + component_interface_utils::Client::SharedPtr cli_set_pause_; + + // Data Buffer + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; + PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + control_interface::IsPaused::Message::ConstSharedPtr is_paused_ptr_{nullptr}; + + // Core + std::unique_ptr collision_checker_; + std::shared_ptr debug_ptr_; + + // Variables + State current_state_{State::DRIVE}; + vehicle_info_util::VehicleInfo vehicle_info_; + bool is_calling_set_pause_{false}; + bool is_paused_by_node_{false}; + + // Callback + void onDynamicObjects(PredictedObjects::ConstSharedPtr 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); + void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void onIsPaused(const control_interface::IsPaused::Message::ConstSharedPtr msg); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + // Functions + bool isDataReady(); + + bool isDataTimeout(); + + bool isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array); + + void onTimer(); + + void checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat); + + TrajectoryPoints trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const; + + void sendRequest(bool make_stop_vehicle); + + bool checkDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const; + + static Trajectory cutTrajectory(const Trajectory & trajectory, const double length); + + size_t insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point); + + void extendTrajectoryPointsArray(TrajectoryPoints & trajectory); + + static std::pair calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point); + + // Parameters + CollisionCheckerParam collision_checker_param_; + NodeParam node_param_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; +}; +} // namespace predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp new file mode 100644 index 0000000000000..ddf8456868b8e --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -0,0 +1,94 @@ +// Copyright 2023 LeoDrive A.Ş. 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 PREDICTED_PATH_CHECKER__UTILS_HPP_ +#define PREDICTED_PATH_CHECKER__UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using std_msgs::msg::Header; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; +using PointArray = std::vector; + +using TrajectoryPoints = std::vector; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point); + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist); + +std::pair findStopPoint( + TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, + const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec); + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point); + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max); + +void getNearestPointForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point); + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +} // namespace utils + +#endif // PREDICTED_PATH_CHECKER__UTILS_HPP_ diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml new file mode 100755 index 0000000000000..f4c7f5dbb27a4 --- /dev/null +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml new file mode 100644 index 0000000000000..db53811f360af --- /dev/null +++ b/control/predicted_path_checker/package.xml @@ -0,0 +1,45 @@ + + + + predicted_path_checker + 0.1.0 + The predicted_path_checker package + Berkay Karaman + Apache License 2.0 + + ament_cmake + + autoware_cmake + + autoware_auto_planning_msgs + autoware_auto_perception_msgs + boost + component_interface_specs + component_interface_utils + diagnostic_updater + eigen + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_control_msgs + tier4_external_api_msgs + tier4_planning_msgs + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp new file mode 100644 index 0000000000000..ff88c96115098 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -0,0 +1,314 @@ +// Copyright 2022 LeoDrive A.Ş. 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 "predicted_path_checker/collision_checker.hpp" + +#include +#include + +#include +#include +#include + +namespace predicted_path_checker +{ +CollisionChecker::CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr) +: debug_ptr_(std::move(debug_ptr)), + node_(node), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) +{ +} + +void CollisionChecker::setParam(const CollisionCheckerParam & param) +{ + param_ = param; +} + +boost::optional> +CollisionChecker::checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, PredictedObjects::ConstSharedPtr dynamic_objects) +{ + // It checks the collision, if there is a collision, it updates the predicted_trajectory_array and + // returns the index of the stop point. + // If there is no collision, it returns boost::none. + const auto now = node_->now(); + + updatePredictedObjectHistory(now); + if (dynamic_objects->objects.empty() && predicted_object_history_.empty()) { + return boost::none; + } + + for (size_t i = 0; i < predicted_trajectory_array.size() - 1; i++) { + // create one step circle center for vehicle + const auto & p_front = predicted_trajectory_array.at(i).pose; + const auto & p_back = predicted_trajectory_array.at(i + 1).pose; + const auto z_min = std::min(p_front.position.z, p_back.position.z); + const auto z_max = std::max(p_front.position.z, p_back.position.z) + + vehicle_info_.max_height_offset_m + param_.z_axis_filtering_buffer; + + // create one-step polygon for vehicle + Polygon2d one_step_move_vehicle_polygon2d = + utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Vehicle); + } + + // check obstacle history + auto found_collision_at_history = + checkObstacleHistory(p_front, one_step_move_vehicle_polygon2d, z_min, z_max); + + auto found_collision_at_dynamic_objects = + checkDynamicObjects(p_front, dynamic_objects, one_step_move_vehicle_polygon2d, z_min, z_max); + + if (found_collision_at_dynamic_objects || found_collision_at_history) { + double distance_to_current = std::numeric_limits::max(); + double distance_to_history = std::numeric_limits::max(); + if (found_collision_at_dynamic_objects) { + distance_to_current = tier4_autoware_utils::calcDistance2d( + p_front, found_collision_at_dynamic_objects.get().first); + } + if (found_collision_at_history) { + distance_to_history = + tier4_autoware_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + } else { + predicted_object_history_.clear(); + } + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Collision); + } + + if (distance_to_current > distance_to_history) { + debug_ptr_->pushObstaclePoint(found_collision_at_history->first, PointType::Stop); + return found_collision_at_history; + } + + predicted_object_history_.emplace_back( + now, found_collision_at_dynamic_objects.get().first, + found_collision_at_dynamic_objects.get().second); + debug_ptr_->pushObstaclePoint(found_collision_at_dynamic_objects->first, PointType::Stop); + return found_collision_at_dynamic_objects; + } + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max) +{ + if (predicted_object_history_.empty()) { + return boost::none; + } + + std::vector> collision_points_in_history; + for (const auto & obj_history : predicted_object_history_) { + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) { + continue; + } + } + const auto & point = obj_history.point; + const Point2d point2d(point.x, point.y); + if (bg::within(point2d, one_step_move_vehicle_polygon2d)) { + collision_points_in_history.emplace_back(point, obj_history.object); + } + } + if (!collision_points_in_history.empty()) { + double min_norm = 0.0; + bool is_init = false; + std::pair nearest_collision_object_with_point; + for (const auto & p : collision_points_in_history) { + double norm = tier4_autoware_utils::calcDistance2d(p.first, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + nearest_collision_object_with_point = p; + is_init = true; + } + } + return boost::make_optional(nearest_collision_object_with_point); + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max) +{ + if (dynamic_objects->objects.empty()) { + return boost::none; + } + for (const auto & obj : dynamic_objects->objects) { + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj, z_min, z_max)) { + continue; + } + } + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + Polygon2d object_polygon{}; + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + const auto found_collision_points = + bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + + if (found_collision_points) { + geometry_msgs::msg::Point nearest_collision_point; + + std::vector collision_points; + PointArray collision_point_array; + bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_points); + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + + // Also check the corner points + + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_vehicle_polygon2d)) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + } + + utils::getNearestPointForPredictedObject( + collision_point_array, base_pose, &nearest_collision_point); + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron(object_polygon, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + object_polygon, obj.kinematics.initial_pose_with_covariance.pose.position.z, + PolygonType::Collision); + } + + return boost::make_optional(std::make_pair(nearest_collision_point, obj)); + } + + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + Polygon2d object_polygon{}; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + + const auto found_collision_points = + bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + + if (found_collision_points) { + geometry_msgs::msg::Point nearest_collision_point; + + std::vector collision_points; + PointArray collision_point_array; + bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_points); + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + + // Also check the corner points + + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_vehicle_polygon2d)) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + } + + utils::getNearestPointForPredictedObject( + collision_point_array, base_pose, &nearest_collision_point); + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron(object_polygon, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + object_polygon, obj.kinematics.initial_pose_with_covariance.pose.position.z, + PolygonType::Collision); + } + + return boost::make_optional(std::make_pair(nearest_collision_point, obj)); + } + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + Polygon2d object_polygon{}; + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + + const auto found_collision_points = + bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + + if (found_collision_points) { + geometry_msgs::msg::Point nearest_collision_point; + + std::vector collision_points; + PointArray collision_point_array; + bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_points); + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + + // Also check the corner points + + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_vehicle_polygon2d)) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + } + utils::getNearestPointForPredictedObject( + collision_point_array, base_pose, &nearest_collision_point); + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron(object_polygon, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + object_polygon, obj.kinematics.initial_pose_with_covariance.pose.position.z, + PolygonType::Collision); + } + + return boost::make_optional(std::make_pair(nearest_collision_point, obj)); + } + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 3000, "Object type is not supported. type: %d", + obj.shape.type); + } + } + return boost::none; +} + +} // namespace predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp new file mode 100644 index 0000000000000..24d3d3d3bb4f0 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -0,0 +1,329 @@ +// Copyright 2023 LeoDrive A.Ş. 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 "predicted_path_checker/debug_marker.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +using motion_utils::createDeletedStopVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; + +namespace predicted_path_checker +{ +PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( + rclcpp::Node * node, const double base_link2front) +: node_(node), base_link2front_(base_link2front) +{ + virtual_wall_pub_ = + node->create_publisher("~/predicted/virtual_wall", 1); + debug_viz_pub_ = + node_->create_publisher("~/predicted/debug/marker", 1); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) +{ + std::vector eigen_polygon; + for (const auto & point : polygon.outer()) { + Eigen::Vector3d eigen_point; + eigen_point << point.x(), point.y(), z; + eigen_polygon.push_back(eigen_point); + } + return pushPolygon(eigen_polygon, type); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const std::vector & polygon, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polygon.empty()) { + vehicle_polygons_.push_back(polygon); + } + return true; + case PolygonType::Collision: + if (!polygon.empty()) { + collision_polygons_.push_back(polygon); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type) +{ + std::vector eigen_polyhedron; + for (const auto & point : polyhedron.outer()) { + eigen_polyhedron.emplace_back(point.x(), point.y(), z_min); + eigen_polyhedron.emplace_back(point.x(), point.y(), z_max); + } + + return pushPolyhedron(eigen_polyhedron, type); +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const std::vector & polyhedron, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polyhedron.empty()) { + vehicle_polyhedrons_.push_back(polyhedron); + } + return true; + case PolygonType::Collision: + if (!polyhedron.empty()) { + collision_polyhedrons_.push_back(polyhedron); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPose( + const geometry_msgs::msg::Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::Stop: + stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::Collision: + collision_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushObstaclePoint( + const geometry_msgs::msg::Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::Stop: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +void PredictedPathCheckerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto virtual_wall_msg = makeVirtualWallMarker(); + virtual_wall_pub_->publish(virtual_wall_msg); + + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* reset variables */ + vehicle_polygons_.clear(); + collision_polygons_.clear(); + vehicle_polyhedrons_.clear(); + collision_polyhedrons_.clear(); + collision_pose_ptr_ = nullptr; + stop_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVirtualWallMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + if (stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = + createStopVirtualWallMarker(p, "obstacle on predicted path", current_time, 0); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (collision_pose_ptr_ != nullptr) { + const auto markers = + createStopVirtualWallMarker(*collision_pose_ptr_, "collision_point", current_time, 1); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 1); + appendMarkerArray(markers, &msg); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualizationMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + // cube + if (!vehicle_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = vehicle_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = vehicle_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + if (!collision_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = collision_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = collision_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + // polygon + if (!vehicle_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + { + const auto & p = vehicle_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == vehicle_polygons_.at(i).size()) { + const auto & p = vehicle_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = vehicle_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!collision_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polygons_.size(); ++i) { + for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + { + const auto & p = collision_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == collision_polygons_.at(i).size()) { + const auto & p = collision_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = collision_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_text", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp new file mode 100644 index 0000000000000..d1faf7bb473a6 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -0,0 +1,525 @@ +// Copyright 2023 LeoDrive A.Ş. 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 "predicted_path_checker/predicted_path_checker_node.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace predicted_path_checker +{ +PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("predicted_path_checker_node", node_options), updater_(this) +{ + using std::placeholders::_1; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_set_pause_, group_cli_); + adaptor.init_sub(sub_pause_state_, this, &PredictedPathCheckerNode::onIsPaused); + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + node_param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold", 3.0); + node_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold", 1.046); + node_param_.max_deceleration = declare_parameter("max_deceleration", 1.5); + node_param_.delay_time = declare_parameter("delay_time", 0.5); + node_param_.stop_margin = declare_parameter("stop_margin", 1.0); + node_param_.min_trajectory_check_length = declare_parameter("min_trajectory_check_length", 1.5); + node_param_.trajectory_check_time = declare_parameter("trajectory_check_time", 3.0); + node_param_.resample_interval = declare_parameter("resample_interval", 0.5); + node_param_.distinct_point_distance_threshold = + declare_parameter("distinct_point_distance_threshold", 0.3); + node_param_.distinct_point_yaw_threshold = declare_parameter("distinct_point_yaw_threshold", 5.0); + + // Collision Checker Parameter + collision_checker_param_.width_margin = + declare_parameter("collision_checker_params.width_margin", 0.2); + collision_checker_param_.enable_z_axis_obstacle_filtering = + declare_parameter("collision_checker_params.enable_z_axis_obstacle_filtering", true); + collision_checker_param_.z_axis_filtering_buffer = + declare_parameter("collision_checker_params.z_axis_filtering_buffer", 1.0); + collision_checker_param_.chattering_threshold = + declare_parameter("collision_checker_params.chattering_threshold", 0.2); + + // Subscriber + self_pose_listener_ = std::make_shared(this); + + sub_dynamic_objects_ = create_subscription( + "input/objects", rclcpp::SensorDataQoS(), + std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); + sub_reference_trajectory_ = create_subscription( + "input/reference_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = create_subscription( + "input/predicted_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); + sub_odom_ = create_subscription( + "input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1)); + sub_accel_ = create_subscription( + "input/current_accel", rclcpp::QoS{1}, std::bind(&PredictedPathCheckerNode::onAccel, this, _1)); + + debug_ptr_ = + std::make_shared(this, vehicle_info_.max_longitudinal_offset_m); + + // Core + collision_checker_ = std::make_unique(this, debug_ptr_); + collision_checker_->setParam(collision_checker_param_); + + // Diagnostic Updater + updater_.setHardwareID("predicted_path_checker"); + updater_.add("predicted_path_checker", this, &PredictedPathCheckerNode::checkVehicleState); + + // Wait for first self pose + self_pose_listener_->waitForFirstPose(); + + // Timer + initTimer(1.0 / node_param_.update_rate); +} + +void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) +{ + object_ptr_ = msg; +} + +void PredictedPathCheckerNode::onReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_twist_ = std::make_shared(msg->twist.twist); +} + +void PredictedPathCheckerNode::onAccel( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + current_accel_ = msg; +} + +void PredictedPathCheckerNode::onIsPaused( + const control_interface::IsPaused::Message::ConstSharedPtr msg) +{ + is_paused_ptr_ = msg; + + is_paused_by_node_ = + is_paused_ptr_->data && + std::find( + is_paused_ptr_->requested_sources.begin(), is_paused_ptr_->requested_sources.end(), + "predicted_path_checker") != is_paused_ptr_->requested_sources.end(); +} + +void PredictedPathCheckerNode::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PredictedPathCheckerNode::onTimer, this)); +} + +bool PredictedPathCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_pose..."); + return false; + } + + if (!object_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic objects msg..."); + 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; + } + + if (!current_accel_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_accel msg..."); + return false; + } + + if (!is_paused_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for is_paused msg..."); + return false; + } + + if (!cli_set_pause_->service_is_ready()) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pause service..."); + return false; + } + + return true; +} + +bool PredictedPathCheckerNode::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(), 5000 /* ms */, "pose is timeout..."); + return true; + } + + return false; +} + +void PredictedPathCheckerNode::onTimer() +{ + current_pose_ = self_pose_listener_->getCurrentPose(); + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + // Cut trajectory + const auto cut_trajectory = cutTrajectory( + *predicted_trajectory_, std::max( + node_param_.min_trajectory_check_length, + current_twist_->linear.x * node_param_.trajectory_check_time)); + + // Convert to trajectory array + + TrajectoryPoints predicted_trajectory_array = motion_utils::convertToTrajectoryPointArray( + motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + + // Check collision + + const auto collision_checker_output = + collision_checker_->checkTrajectoryForCollision(predicted_trajectory_array, object_ptr_); + + if (!collision_checker_output) { + // There is no need to stop + if (is_paused_by_node_) { + sendRequest(false); + } + current_state_ = State::DRIVE; + updater_.force_update(); + debug_ptr_->publish(); + + return; + } + + // Extend trajectory + + extendTrajectoryPointsArray(predicted_trajectory_array); + + // Insert collision and stop points + + const auto stop_idx = + insertStopPoint(predicted_trajectory_array, collision_checker_output.get().first); + + // Check ego vehicle is stopped or not + constexpr double th_stopped_velocity = 0.001; + const bool is_ego_vehicle_stopped = current_twist_->linear.x < th_stopped_velocity; + + // If ego vehicle is not stopped, check obstacle is in the brake distance + if (!is_ego_vehicle_stopped) { + // Calculate projected velocity and acceleration of the object on the trajectory point + + const auto projected_obj_vel_acc = calculateProjectedVelAndAcc( + collision_checker_output->second, predicted_trajectory_array.at(stop_idx)); + + // Calculate relative velocity and acceleration wrt the object + + const double relative_velocity = current_twist_->linear.x - projected_obj_vel_acc.first; + const double relative_acceleration = + current_accel_->accel.accel.linear.x - projected_obj_vel_acc.second; + + // Check if the vehicle is in the brake distance + + const bool is_in_brake_distance = utils::isInBrakeDistance( + predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration, + node_param_.max_deceleration, node_param_.delay_time); + + if (is_in_brake_distance) { + // Send emergency and stop request + current_state_ = State::EMERGENCY; + updater_.force_update(); + if (!is_paused_by_node_) { + sendRequest(true); + } + debug_ptr_->publish(); + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle in the brake distance. Sending emergency and stop request..."); + return; + } + } + + // If it is not in the brake distance, check if the collision point is discrete from the reference + // trajectory or not + + const auto reference_trajectory_array = + motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); + + const auto is_discrete_point = + checkDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx)); + + if (is_discrete_point) { + // Check reference trajectory has stop point or not + + const auto is_there_stop_in_ref_trajectory = isThereStopPointOnReferenceTrajectory( + predicted_trajectory_array.at(stop_idx).pose, reference_trajectory_array); + + if (!is_there_stop_in_ref_trajectory) { + // Send pause + current_state_ = State::PAUSE; + updater_.force_update(); + if (!is_paused_by_node_) { + sendRequest(true); + } + + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle on predicted path. Sending stop request..."); + + debug_ptr_->publish(); + return; + } + } + + // If it is not discrete point, planning should handle it. Send drive. + current_state_ = State::DRIVE; + updater_.force_update(); + + if (is_paused_by_node_) { + sendRequest(false); + } + + debug_ptr_->publish(); +} + +TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const +{ + TrajectoryPoints output{}; + + const size_t min_distance_index = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold) + + 1; + + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + + return output; +} + +bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array) +{ + const auto trimmed_reference_trajectory_array = + trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); + + const auto nearest_stop_point_on_ref_trajectory = + motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + + const auto stop_point_on_trajectory = motion_utils::searchZeroVelocityIndex( + trimmed_reference_trajectory_array, 0, *nearest_stop_point_on_ref_trajectory); + + return !!stop_point_on_trajectory; +} + +void PredictedPathCheckerNode::checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (current_state_ == State::EMERGENCY) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "vehicle will collide with obstacles"; + } + if (current_state_ == State::PAUSE) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "vehicle will stop due to obstacle"; + } + + stat.summary(level, msg); +} + +void PredictedPathCheckerNode::sendRequest(bool make_stop_vehicle) +{ + if (!is_calling_set_pause_ && cli_set_pause_->service_is_ready()) { + const auto req = std::make_shared(); + req->pause = make_stop_vehicle; + req->request_source = "predicted_path_checker"; + is_calling_set_pause_ = true; + cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; }); + } +} +bool PredictedPathCheckerNode::checkDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const +{ + const auto nearest_segment = + motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); + + const auto nearest_point = utils::calcInterpolatedPoint( + reference_trajectory, collision_point.pose.position, *nearest_segment, false); + + const auto distance = tier4_autoware_utils::calcDistance2d( + nearest_point.pose.position, collision_point.pose.position); + + const auto yaw_diff = + std::abs(tier4_autoware_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + return distance >= node_param_.distinct_point_distance_threshold || + yaw_diff >= tier4_autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); +} + +Trajectory PredictedPathCheckerNode::cutTrajectory( + const Trajectory & trajectory, const double length) +{ + Trajectory cut; + cut.header = trajectory.header; + if (trajectory.points.empty()) { + return cut; + } + 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 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = tier4_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.001) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + 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; + } + motion_utils::removeOverlapPoints(cut.points); + + return cut; +} +void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & trajectory) +{ + // It extends the trajectory to the end of the footprint of the vehicle to get better distance to + // collision_point. + const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin; + const auto & goal_point = trajectory.back(); + const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point); + trajectory.push_back(trajectory_point_extend); +} + +size_t PredictedPathCheckerNode::insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) +{ + const auto nearest_collision_segment = + motion_utils::findNearestSegmentIndex(trajectory, collision_point); + + const auto nearest_collision_point = + utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); + + const size_t collision_idx = nearest_collision_segment + 1; + + trajectory.insert(trajectory.begin() + static_cast(collision_idx), nearest_collision_point); + + const auto stop_point = + utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_); + + const size_t stop_idx = stop_point.first + 1; + trajectory.insert(trajectory.begin() + static_cast(stop_idx), stop_point.second); + + debug_ptr_->pushPose(nearest_collision_point.pose, PoseType::Collision); + debug_ptr_->pushPose(stop_point.second.pose, PoseType::Stop); + return stop_idx; +} + +std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point) +{ + const auto & orientation_obj = object.kinematics.initial_pose_with_covariance.pose.orientation; + const auto & orientation_stop_point = trajectory_point.pose.orientation; + const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto acceleration_obj = + object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + const auto k = std::cos(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); + const auto projected_velocity = velocity_obj * k; + const auto projected_acceleration = acceleration_obj * k; + return std::make_pair(projected_velocity, projected_acceleration); +} + +} // namespace predicted_path_checker + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(predicted_path_checker::PredictedPathCheckerNode) diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp new file mode 100644 index 0000000000000..148b1d3b78a1e --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -0,0 +1,345 @@ +// Copyright 2023 LeoDrive A.Ş. 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 "predicted_path_checker/utils.hpp" + +namespace utils +{ + +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getRPY; + +// Utils Functions +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + { // base step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + } + + { // next step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + } + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + boost::geometry::convex_hull(polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; +} + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist) +{ + if (trajectory.empty()) { + TrajectoryPoint interpolated_point{}; + interpolated_point.pose.position = target_point; + return interpolated_point; + } else if (trajectory.size() == 1) { + return trajectory.front(); + } + + // Calculate interpolation ratio + const auto & curr_pt = trajectory.at(segment_idx); + const auto & next_pt = trajectory.at(segment_idx + 1); + const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_point); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + TrajectoryPoint interpolated_point{}; + + // pose interpolation + interpolated_point.pose = + tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; + interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + interpolated_point.acceleration_mps2 = + interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.heading_rate_rps = + interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation + interpolated_point.front_wheel_angle_rad = interpolation::lerp( + curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = + interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation + const double interpolated_time = interpolation::lerp( + rclcpp::Duration(curr_pt.time_from_start).seconds(), + rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + return interpolated_point; +} + +std::pair findStopPoint( + TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, + vehicle_info_util::VehicleInfo & vehicle_info) +{ + // It returns the stop point and segment of the point on trajectory. + + double desired_distance_base_link_to_collision = + vehicle_info.max_longitudinal_offset_m + stop_margin; + size_t stop_segment_idx = 0; + bool found_stop_point = false; + double distance_point_to_collision = 0.0; + + for (size_t i = collision_idx; i > 0; i--) { + distance_point_to_collision = + motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); + if (distance_point_to_collision >= desired_distance_base_link_to_collision) { + stop_segment_idx = i - 1; + found_stop_point = true; + break; + } + } + if (found_stop_point) { + const auto & base_point = trajectory_array.at(stop_segment_idx); + const auto & next_point = trajectory_array.at(stop_segment_idx + 1); + + double ratio = (distance_point_to_collision - desired_distance_base_link_to_collision) / + (std::hypot( + base_point.pose.position.x - next_point.pose.position.x, + base_point.pose.position.y - next_point.pose.position.y)); + + geometry_msgs::msg::Pose interpolated_pose = + tier4_autoware_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + TrajectoryPoint output; + output.set__pose(interpolated_pose); + return std::make_pair(stop_segment_idx, output); + } else { + // It means that there is no enough distance between vehicle and collision point. + // So, we insert a stop at the first point of the trajectory. + return std::make_pair(0, trajectory_array.front()); + } +} + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec) +{ + if (relative_velocity < 0.0) { + return false; + } + + const auto distance_to_obstacle = motion_utils::calcSignedArcLength( + trajectory, trajectory.front().pose.position, trajectory.at(stop_idx).pose.position); + + const double distance_in_delay = relative_velocity * delay_time_sec + + relative_acceleration * delay_time_sec * delay_time_sec * 0.5; + + const double velocity_after_delay = relative_velocity + relative_acceleration * delay_time_sec; + + const double time_to_stop = velocity_after_delay / std::abs(max_deceleration); + const double distance_after_delay = + velocity_after_delay * time_to_stop - 0.5 * abs(max_deceleration) * time_to_stop * time_to_stop; + const double brake_distance = distance_in_delay + distance_after_delay; + + return brake_distance > distance_to_obstacle; +} + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance; + const auto & obj_height = object.shape.dimensions.z; + return obj_pose.pose.position.z - obj_height / 2.0 <= z_max && + obj_pose.pose.position.z + obj_height / 2.0 >= z_min; +} + +void getNearestPointForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point) +{ + double min_norm = 0.0; + bool is_init = false; + + for (const auto & p : points) { + double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = p; + is_init = true; + } + } +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + boost::geometry::append(polygon.outer(), point); +} + +} // namespace utils