diff --git a/planning/adaptive_cruise_controller/CMakeLists.txt b/planning/adaptive_cruise_controller/CMakeLists.txt new file mode 100644 index 0000000000000..4d2adde6f1197 --- /dev/null +++ b/planning/adaptive_cruise_controller/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.5) +project(adaptive_cruise_controller) + +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() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +ament_auto_add_library(adaptive_cruise_controller SHARED + src/adaptive_cruise_control.cpp + src/adaptive_cruise_control_core.cpp + src/acc_pid.cpp + src/debug_marker.cpp + src/utilities.cpp +) + +target_include_directories(adaptive_cruise_controller + PUBLIC + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries(adaptive_cruise_controller + ${OpenCV_LIBRARIES} +) + + +rclcpp_components_register_node(adaptive_cruise_controller + PLUGIN "motion_planning::AdaptiveCruiseControllerNode" + EXECUTABLE adaptive_cruise_controller_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/planning/adaptive_cruise_controller/README.md b/planning/adaptive_cruise_controller/README.md new file mode 100644 index 0000000000000..9e241706c6302 --- /dev/null +++ b/planning/adaptive_cruise_controller/README.md @@ -0,0 +1,25 @@ +# Adaptive Cruise Controller + +## Overview + +## Input topics + +| Name | Type | Description | +| --------------------------- | ----------------------------------------------- | ----------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| `~/input/predicted_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | + +## Output topics + +| Name | Type | Description | +| ---------------------- | --------------------------------------- | -------------------------------------- | +| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | + + +## Role + +## Flowchart + +## Known Limits diff --git a/planning/adaptive_cruise_controller/config/adaptive_cruise_control.param.yaml b/planning/adaptive_cruise_controller/config/adaptive_cruise_control.param.yaml new file mode 100644 index 0000000000000..e744f1c540a11 --- /dev/null +++ b/planning/adaptive_cruise_controller/config/adaptive_cruise_control.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + decimate_step_length: 1.0 + trajectory_extend_length: 5.0 + rough_detection_area_expand_length: 3.0 + detection_area_expand_length: 0.0 + object_threshold_angle: 0.523 + predicted_path_threshold_angle: 0.523 + mininum_overlap_time_of_predicted_path: 2.0 + + + + object_low_velocity_thresh: 3.0 # if object velocity is lower than this value, adaptive cruise becomes disable. + object_stop_velocity_thresh: 1.0 # if object velocity is lower than this value, ignore the sign of object orientation + object_velocity_hysteresis_margin: 1.0 # hysteresis margin to change acc state for preventing chattering + reset_time_to_acc_state: 1.0 + velocity_to_accleration_weight: 0.3 # target accleration is calculated by (target_velocity - current_velocity) * velocity_to_accleration_weight + acc_min_acceleration: -1.5 + acc_min_jerk: -1.0 + stop_min_acceleration: -3.0 + object_min_acceleration: -3.0 + minimum_margin_distance: 5.0 + idling_time: 1.0 + breaking_delay_time: 0.5 + p_term_in_velocity_pid: 0.3 diff --git a/planning/adaptive_cruise_controller/config/plot_jugger_adaptive_cruise.control.xml b/planning/adaptive_cruise_controller/config/plot_jugger_adaptive_cruise.control.xml new file mode 100644 index 0000000000000..b3f8719e8c43c --- /dev/null +++ b/planning/adaptive_cruise_controller/config/plot_jugger_adaptive_cruise.control.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/acc_pid.hpp b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/acc_pid.hpp new file mode 100644 index 0000000000000..e45705035cf43 --- /dev/null +++ b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/acc_pid.hpp @@ -0,0 +1,53 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ADAPTIVE_CRUISE_CONTROLLER__ACC_PID_HPP_ +#define ADAPTIVE_CRUISE_CONTROLLER__ACC_PID_HPP_ + +#include +#include + +namespace motion_planning +{ + +class AccPidNode +{ +public: + explicit AccPidNode(const double baselink2front, const AccParam & acc_param); + + void updateState( + const rclcpp::Time & current_time, const double ego_velocity, const double obstacle_velocity, + const double dist_to_obstacle); + State getState() { return current_state_; } + void calculate(const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion); + +private: + double calcStoppingDistFromCurrentVel(const double current_velocity); + double calcEmergencyDistFromVel(const double current_velocity, const double obj_velocity); + void calculateTargetMotion(const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion); + void calcTrajectoryWithStopPoints( + const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion); + TrajectoryPoints insertStopPoint( + const double stop_distance_from_ego, const TrajectoryPoints & trajectory_points, + const geometry_msgs::msg::Pose & ego_pose, geometry_msgs::msg::Pose & stop_pose); + + double baselink2front_; + AccParam acc_param_; + State current_state_ = State::NONE; + std::shared_ptr prev_acc_info_ptr_; +}; + +} // namespace motion_planning + +#endif // ADAPTIVE_CRUISE_CONTROLLER__ACC_PID_HPP_ diff --git a/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/adaptive_cruise_control.hpp b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/adaptive_cruise_control.hpp new file mode 100644 index 0000000000000..99e184b3c4124 --- /dev/null +++ b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/adaptive_cruise_control.hpp @@ -0,0 +1,174 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ADAPTIVE_CRUISE_CONTROLLER__ADAPTIVE_CRUISE_CONTROL_HPP_ +#define ADAPTIVE_CRUISE_CONTROLLER__ADAPTIVE_CRUISE_CONTROL_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace motion_planning +{ +namespace bg = boost::geometry; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using TrajectoryPoints = std::vector; +using nav_msgs::msg::Odometry; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; + +enum class CUT_IN_OUT { + NONE = 0, + CUT_IN = 1, + CUT_OUT = 2, +}; + +class AdaptiveCruiseControllerNode : public rclcpp::Node +{ +public: + explicit AdaptiveCruiseControllerNode(const rclcpp::NodeOptions & options); + +private: + struct Param + { + // step length of decimated trajectory + double decimate_step_length; + + // length to extend trajectory forward for + double trajectory_extend_length; + + // length to expand rough detection area + double rough_detection_area_expand_length; + + // length to expand detection area + double detection_area_expand_length; + + // minimum diff-angle between object and trajectory to be considered as a target objects + double object_threshold_angle; + + // minimum diff-angle between preidcted_path and trajectory to be considered as a target objects + double predicted_path_threshold_angle; + + // minimum overlap time of predicted path and trajectory to be considered as a target objects + double mininum_overlap_time_of_predicted_path; + }; + Param param_; + + AccParam acc_param_; + + rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_odometry_; + + tier4_autoware_utils::SelfPoseListener self_pose_listener_; + + void onTrajectory(const Trajectory::ConstSharedPtr msg); + void onPredictedObjects(const PredictedObjects::ConstSharedPtr msg); + void onOdometry(const Odometry::ConstSharedPtr msg); + + bool isDataReady(); + TrajectoryPoints trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const geometry_msgs::msg::PoseStamped & self_pose); + TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point); + TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance); + TrajectoryPoints decimateTrajectory(const TrajectoryPoints & input, const double step_length); + TrajectoryPoint getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length); + void createPolygonFromTrajectoryPoints( + const TrajectoryPoints & trajectory_points, const double expand_width, + std::vector & polygon); + void extractVehicleObjectsInPolygon( + const PredictedObjects & objects, const std::vector & target_polygons, + PredictedObjects & objects_in_polygon); + void getTargetObjects( + const PredictedObjects & objects, const std::vector & target_polygons, + const TrajectoryPoints & target_trajectory, PredictedObjects & target_objects); + void getNearestObject( + const PredictedObjects & objects, const TrajectoryPoints & trajectory_points, + PredictedObject & nearest_object); + VelocityLimit createVelocityLimitMsg( + const double velocity, const double acceleration, const double jerk); + VelocityLimitClearCommand createVelocityLimitClearCommandMsg(); + + PredictedPath getHighestConfidencePathFromObject(const PredictedObject & object); + geometry_msgs::msg::Pose getObjectPose(const PredictedObject & object); + bool isObjectVelocityHigh(const PredictedObject & object); + bool isTargetObjectType(const PredictedObject & object); + + Odometry::ConstSharedPtr current_odometry_ptr_; + PredictedObjects::ConstSharedPtr current_objects_ptr_; + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + VehicleInfo vehicle_info_; + + std::shared_ptr controller_ptr_; + + // only for debug + void fillAndPublishDebugOutput(const PredictedObject & target_object); + void publishDebugOutputWithNoTarget(); + double calcAcc( + const geometry_msgs::msg::TwistStamped & twist, + std::shared_ptr & prev_twist, const double prev_acc, + const double lowpass_gain = 0.6, const double timeout = 1.0); + + void resetObjectTwistHistory(); + CUT_IN_OUT detectCutInAndOut( + AdaptiveCruiseInformation acc_info, const double threshold_length = 10.0); + + geometry_msgs::msg::TwistStamped toTwistStamped( + const std_msgs::msg::Header & header, const geometry_msgs::msg::Twist & twist); + + std::shared_ptr debug_node_ptr_; + std::shared_ptr prev_acc_info_; + std::shared_ptr prev_ego_twist_; + double ego_accel_ = 0.0; + std::shared_ptr prev_object_twist_; + double obj_accel_ = 0.0; + + bool need_to_clear_velocity_limit_ = false; +}; + +} // namespace motion_planning + +#endif // ADAPTIVE_CRUISE_CONTROLLER__ADAPTIVE_CRUISE_CONTROL_HPP_ diff --git a/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/adaptive_cruise_control_core.hpp b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/adaptive_cruise_control_core.hpp new file mode 100644 index 0000000000000..6b1e31559b93a --- /dev/null +++ b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/adaptive_cruise_control_core.hpp @@ -0,0 +1,69 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ADAPTIVE_CRUISE_CONTROLLER__ADAPTIVE_CRUISE_CONTROL_CORE_HPP_ +#define ADAPTIVE_CRUISE_CONTROLLER__ADAPTIVE_CRUISE_CONTROL_CORE_HPP_ + +#include +#include +#include + +#include +#include +#include + +namespace motion_planning +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using nav_msgs::msg::Odometry; + +class AdaptiveCruiseControlCore +{ +public: + explicit AdaptiveCruiseControlCore(const double baselink2front, const AccParam & acc_param); + std::shared_ptr getAccInfoPtr() { return acc_info_ptr_; } + std::shared_ptr getPrevAccInfoPtr() { return prev_acc_info_ptr_; } + + void calcInformationForAdaptiveCruise( + const TrajectoryPoints & trajectory_points, const geometry_msgs::msg::PoseStamped & pose, + const Odometry & odometry, const PredictedObject & object, const rclcpp::Time & object_time); + + void calculate(); + bool getTargetMotion(double & target_velocity, double & target_acc, double & target_jerk); + TrajectoryPoints getAccTrajectory(bool & emergency_flag); + State getState(); + AccMotion getAccMotion() { return acc_motion_; }; + +private: + // parameter + double baselink2front_; + AccParam acc_param_; + + // variables + std::shared_ptr acc_pid_node_ptr_; + double prev_target_velocity_; + std::shared_ptr acc_info_ptr_; + std::shared_ptr prev_acc_info_ptr_; + AccMotion acc_motion_; + + // functions + double getIdealDistanceToObject(const double current_velocity, const double object_velocity); +}; + +} // namespace motion_planning + +#endif // ADAPTIVE_CRUISE_CONTROLLER__ADAPTIVE_CRUISE_CONTROL_CORE_HPP_ diff --git a/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/debug_marker.hpp b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/debug_marker.hpp new file mode 100644 index 0000000000000..0e7ba84f77356 --- /dev/null +++ b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/debug_marker.hpp @@ -0,0 +1,122 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef ADAPTIVE_CRUISE_CONTROLLER__DEBUG_MARKER_HPP_ +#define ADAPTIVE_CRUISE_CONTROLLER__DEBUG_MARKER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace motion_planning +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::Shape; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +class DebugValues +{ +public: + enum class TYPE { + CURRENT_VEL = 0, + CURRENT_ACC = 1, + CURRENT_OBJECT_VEL = 2, + CURRENT_OBJECT_VEL_DIFF_DIST = 3, + CURRENT_OBJECT_ACC = 4, + CURRENT_OBJECT_DISTANCE = 5, + IDEAL_OBJECT_DISTANCE = 6, + FLAG_CUTIN_OBJECT = 7, + FLAG_CUTOUT_OBJECT = 8, + FLAG_FIND_OBJECT = 9, + FLAG_ADAPTIVE_CRUISE = 10, + FLAG_STOP = 11, + FLAG_NONE = 12, + CURRENT_TARGET_VELOCITY = 13, + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getValuesIdx(const TYPE type) const { return static_cast(type); } + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> getValues() const { return values_; } + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE type, const double val) { values_.at(static_cast(type)) = val; } + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void setValues(const int type, const double val) { values_.at(type) = val; } + + void resetValues() { values_.fill(0.0); } + +private: + static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); + std::array(TYPE::SIZE)> values_; +}; + +class AdaptiveCruiseControllerDebugNode +{ +public: + explicit AdaptiveCruiseControllerDebugNode(rclcpp::Node * node); + void setDebugValues(const DebugValues::TYPE type, const double val) + { + debug_values_.setValues(type, val); + } + void resetDebugValues(){ + debug_values_.resetValues(); + } + void setVirtualWall( + const geometry_msgs::msg::Pose & stop_pose, const double offset, const bool is_stop_wall); + void setTargetPolygon(const PredictedObject & object); + void clearMarker(); + void clearVirtualWall(); + void clearTargetPolygon(); + void publish(); + void publishMarker(); + void publishDebugValues(); + +private: + Float32MultiArrayStamped convertDebugValuesToMsg(const DebugValues debug_value); + + rclcpp::Node * node_; + + rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_debug_value_; + DebugValues debug_values_; + std::shared_ptr target_vehicle_marker_ptr_; + std::shared_ptr virtual_wall_marker_ptr_; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ diff --git a/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/utilities.hpp b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/utilities.hpp new file mode 100644 index 0000000000000..2182f3b19d705 --- /dev/null +++ b/planning/adaptive_cruise_controller/include/adaptive_cruise_controller/utilities.hpp @@ -0,0 +1,118 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ADAPTIVE_CRUISE_CONTROLLER__UTILITIES_HPP_ +#define ADAPTIVE_CRUISE_CONTROLLER__UTILITIES_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace motion_planning +{ + +namespace bg = boost::geometry; + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct AccParam +{ + double object_low_velocity_thresh; + double object_stop_velocity_thresh; + double object_velocity_hysteresis_margin; + double reset_time_to_acc_state; + double velocity_to_accleration_weight; + double acc_min_acceleration; + double acc_min_jerk; + double stop_min_acceleration; + double object_min_acceleration; + double minimum_margin_distance; + double idling_time; + double breaking_delay_time; + double p_term_in_velocity_pid; +}; + +enum State { NONE = 0, ACC = 1, STOP = 2 }; + +struct AccMotion +{ + bool use_target_motion; + double target_velocity; + double target_acceleration; + double target_jerk; + bool use_trajectory; + TrajectoryPoints planned_trajectory; + geometry_msgs::msg::Pose stop_pose; + bool emergency; // true when ACC makes a plan to collide with a car in front +}; + +struct AdaptiveCruiseInformation +{ + rclcpp::Time info_time; // current time of pose stamped + double current_ego_velocity; + double current_object_velocity; + double current_distance_to_object; + double ideal_distance_to_object; + geometry_msgs::msg::Pose current_ego_pose; + PredictedObject target_object; + TrajectoryPoints original_trajectory; +}; + +void convexHull(const std::vector & points, std::vector & polygon_points); +bool isObjectWithinPolygon( + const std::vector & target_polygons, const Polygon2d & object_polygon, + const double overlap_threshold = 0.1); +bool getDiffAngleWithTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & trajectory_points, + double & diff_angle); +bool isAngleAlignedWithTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & trajectory_points, + const double threshold_angle); +bool isPoseNearTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & trajectory_points, + const double threshold_dist, const double threshold_angle); +bool isPathNearTrajectory( + const PredictedPath & path, const TrajectoryPoints & trajectory_points, + const double threshold_dist, const double threshold_angle, const double minmum_overlap_time); + +void convertObjectToBoostPolygon(const PredictedObject & object, Polygon2d & object_polygon); +void convertcvPointsToBoostPolygon( + const std::vector & points, Polygon2d & object_polygon); +bool isClockWise(const Polygon2d & polygon); +Polygon2d inverseClockWise(const Polygon2d & polygon); +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t); + +} // namespace motion_planning + +#endif // ADAPTIVE_CRUISE_CONTROLLER__UTILITIES_HPP_ diff --git a/planning/adaptive_cruise_controller/launch/adaptive_cruise_controller.launch.xml b/planning/adaptive_cruise_controller/launch/adaptive_cruise_controller.launch.xml new file mode 100644 index 0000000000000..84edd6eac030b --- /dev/null +++ b/planning/adaptive_cruise_controller/launch/adaptive_cruise_controller.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/adaptive_cruise_controller/package.xml b/planning/adaptive_cruise_controller/package.xml new file mode 100644 index 0000000000000..5f3796b26957a --- /dev/null +++ b/planning/adaptive_cruise_controller/package.xml @@ -0,0 +1,37 @@ + + + adaptive_cruise_controller + 0.1.0 + The adaptive_cruise_controller package + + Tomoya Kimura + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + diagnostic_msgs + nav_msgs + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/planning/adaptive_cruise_controller/src/acc_pid.cpp b/planning/adaptive_cruise_controller/src/acc_pid.cpp new file mode 100644 index 0000000000000..dfbbcf4a5bc5c --- /dev/null +++ b/planning/adaptive_cruise_controller/src/acc_pid.cpp @@ -0,0 +1,207 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace motion_planning +{ + +AccPidNode::AccPidNode(const double baselink2front, const AccParam & acc_param) +: baselink2front_(baselink2front), acc_param_(acc_param) +{ +} + +void AccPidNode::updateState( + const rclcpp::Time & current_time, const double ego_velocity, const double obstacle_velocity, + const double dist_to_obstacle) +{ + if (!prev_acc_info_ptr_) { + current_state_ = State::NONE; + return; + } + + if ( + (current_time - prev_acc_info_ptr_->info_time).seconds() > acc_param_.reset_time_to_acc_state) { + // reset current_state if previous acc_time is too old + current_state_ = State::NONE; + return; + } + + double thresh_velocity = acc_param_.object_low_velocity_thresh; + + // provide a hysteresis in the state to prevent chattering + if (current_state_ == State::STOP) { + thresh_velocity += acc_param_.object_velocity_hysteresis_margin; + } + + // change state depending on obstacle velocity + if (obstacle_velocity >= thresh_velocity) { + current_state_ = State::ACC; + } else { + current_state_ = State::STOP; + } + + // if current distance to object is too short, change state to STOP + if (calcEmergencyDistFromVel(obstacle_velocity, ego_velocity) > dist_to_obstacle) { + current_state_ = State::STOP; + } +} + +void AccPidNode::calculate(const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion) +{ + updateState( + acc_info.info_time, acc_info.current_ego_velocity, acc_info.current_object_velocity, + acc_info.current_distance_to_object); + + if (current_state_ == State::STOP) { + acc_motion.use_target_motion = false; + calcTrajectoryWithStopPoints(acc_info, acc_motion); + } else if (current_state_ == State::ACC) { + calculateTargetMotion(acc_info, acc_motion); + acc_motion.use_trajectory = false; + } + + prev_acc_info_ptr_ = std::make_shared(acc_info); +} + +double AccPidNode::calcStoppingDistFromCurrentVel(const double current_velocity) +{ + const double idling_travel_distance = current_velocity * acc_param_.breaking_delay_time; + const double braking_distance = + (current_velocity * current_velocity) / (2.0 * -acc_param_.stop_min_acceleration); + return idling_travel_distance + braking_distance; +} + +double AccPidNode::calcEmergencyDistFromVel( + const double current_velocity, const double obj_velocity) +{ + // when the target object is faster than ego-vehicle + if (current_velocity < obj_velocity) { + return acc_param_.minimum_margin_distance; + } + + const double idling_travel_distance = current_velocity * acc_param_.breaking_delay_time; + const double braking_distance = + (current_velocity * current_velocity) / (2.0 * acc_param_.stop_min_acceleration); + const double object_braking_distance = + (obj_velocity * obj_velocity) / (2.0 * acc_param_.stop_min_acceleration); + const double emergency_dist = idling_travel_distance + braking_distance - object_braking_distance; + return std::max(acc_param_.minimum_margin_distance, emergency_dist); +} + +void AccPidNode::calculateTargetMotion( + const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion) +{ + const double diff_distance_to_object = + acc_info.current_distance_to_object - acc_info.ideal_distance_to_object; + const double min_target_velocity = acc_param_.object_low_velocity_thresh; + const double additional_velocity = acc_param_.p_term_in_velocity_pid * diff_distance_to_object; + + double target_velocity = acc_info.current_ego_velocity + additional_velocity; + target_velocity = std::max(min_target_velocity, target_velocity); + acc_motion.target_velocity = target_velocity; + + const double target_accleration = + -acc_param_.velocity_to_accleration_weight * additional_velocity; + acc_motion.target_acceleration = std::max(target_accleration, acc_param_.acc_min_acceleration); + acc_motion.target_jerk = acc_param_.acc_min_jerk; + acc_motion.use_target_motion = true; +} + +void AccPidNode::calcTrajectoryWithStopPoints( + const AdaptiveCruiseInformation & acc_info, AccMotion & acc_motion) +{ + const double original_stop_dist = + acc_info.current_distance_to_object - acc_param_.minimum_margin_distance; + const double min_stop_dist = calcStoppingDistFromCurrentVel(acc_info.current_ego_velocity); + + double target_stop_dist; + if (original_stop_dist >= min_stop_dist) { + target_stop_dist = original_stop_dist; + acc_motion.emergency = false; + } else { + target_stop_dist = min_stop_dist; + acc_motion.emergency = true; + } + + acc_motion.planned_trajectory = insertStopPoint( + target_stop_dist, acc_info.original_trajectory, acc_info.current_ego_pose, + acc_motion.stop_pose); + acc_motion.use_trajectory = true; +} + +TrajectoryPoints AccPidNode::insertStopPoint( + const double stop_distance_from_ego, const TrajectoryPoints & trajectory_points, + const geometry_msgs::msg::Pose & ego_pose, geometry_msgs::msg::Pose & stop_pose) +{ + TrajectoryPoints trajectory_with_stop_point = trajectory_points; + + if (trajectory_points.empty()) { + return trajectory_points; + } + + // calculate arc length from trajectory.front() to ego pose + const auto arc_length_to_ego = tier4_autoware_utils::calcSignedArcLength( + trajectory_points, trajectory_points.front().pose.position, ego_pose.position); + + // calulcate stop index and stop point + const double stop_distance = arc_length_to_ego + stop_distance_from_ego; + double accumulated_length = 0; + double insert_idx = 0; + bool find_idx = false; + for (size_t i = 1; i < trajectory_points.size(); i++) { + const auto prev_pose = trajectory_points.at(i - 1).pose; + const auto curr_pose = trajectory_points.at(i).pose; + const double segment_length = tier4_autoware_utils::calcDistance3d(prev_pose, curr_pose); + accumulated_length += segment_length; + if (accumulated_length > stop_distance) { + insert_idx = i; + const double ratio = 1 - (accumulated_length - stop_distance) / segment_length; + stop_pose = lerpByPose(prev_pose, curr_pose, ratio); + find_idx = true; + break; + } + } + + if (!find_idx) { + // insert stop velocity to the last point of trajectory + stop_pose = trajectory_points.back().pose; + trajectory_with_stop_point.back().longitudinal_velocity_mps = 0.0; + trajectory_with_stop_point.back().lateral_velocity_mps = 0.0; + return trajectory_with_stop_point; + } + + // insert stop point + TrajectoryPoint stop_point; + stop_point.longitudinal_velocity_mps = 0.0; + stop_point.lateral_velocity_mps = 0.0; + stop_point.pose = stop_pose; + trajectory_with_stop_point.insert(trajectory_with_stop_point.begin() + insert_idx, stop_point); + // set 0 velocity to points after the stop point + for (size_t i = insert_idx; i < trajectory_with_stop_point.size(); i++) { + trajectory_with_stop_point.at(i).longitudinal_velocity_mps = 0.0; + trajectory_with_stop_point.at(i).lateral_velocity_mps = 0.0; + } + return trajectory_with_stop_point; +} + +} // namespace motion_planning \ No newline at end of file diff --git a/planning/adaptive_cruise_controller/src/adaptive_cruise_control.cpp b/planning/adaptive_cruise_controller/src/adaptive_cruise_control.cpp new file mode 100644 index 0000000000000..8b14bdbfdeb8c --- /dev/null +++ b/planning/adaptive_cruise_controller/src/adaptive_cruise_control.cpp @@ -0,0 +1,730 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace motion_planning +{ +AdaptiveCruiseControllerNode::AdaptiveCruiseControllerNode(const rclcpp::NodeOptions & options) +: Node("adaptive_cruise_controller", options), self_pose_listener_(this) +{ + using std::placeholders::_1; + + // set param + param_.decimate_step_length = declare_parameter("decimate_step_length", 1.0); + param_.trajectory_extend_length = declare_parameter("trajectory_extend_length", 5.0); + param_.rough_detection_area_expand_length = + declare_parameter("rough_detection_area_expand_length", 3.0); + param_.detection_area_expand_length = declare_parameter("detection_area_expand_length", 1.0); + param_.object_threshold_angle = declare_parameter("object_threshold_angle", M_PI / 6.0); + param_.predicted_path_threshold_angle = + declare_parameter("predicted_path_threshold_angle", M_PI / 6.0); + param_.mininum_overlap_time_of_predicted_path = + declare_parameter("mininum_overlap_time_of_predicted_path", 1.0); + + // set acc param + acc_param_.object_low_velocity_thresh = declare_parameter("object_low_velocity_thresh", 3.0); + acc_param_.object_stop_velocity_thresh = declare_parameter("object_stop_velocity_thresh", 1.0); + acc_param_.object_velocity_hysteresis_margin = + declare_parameter("object_velocity_hysteresis_margin", 1.0); + acc_param_.reset_time_to_acc_state = declare_parameter("reset_time_to_acc_state", 1.0); + acc_param_.velocity_to_accleration_weight = + declare_parameter("velocity_to_accleration_weight", 0.3); + acc_param_.acc_min_acceleration = declare_parameter("acc_min_acceleration", -1.5); + acc_param_.acc_min_jerk = declare_parameter("acc_min_jerk", -1.0); + acc_param_.stop_min_acceleration = declare_parameter("stop_min_acceleration", -3.0); + acc_param_.object_min_acceleration = declare_parameter("object_min_acceleration", -3.0); + acc_param_.minimum_margin_distance = declare_parameter("minimum_margin_distance", 5.0); + acc_param_.idling_time = declare_parameter("idling_time", 1.0); + acc_param_.breaking_delay_time = declare_parameter("breaking_delay_time", 0.5); + acc_param_.p_term_in_velocity_pid = declare_parameter("p_term_in_velocity_pid", 0.3); + + // vehicle parameters + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + // debug node + debug_node_ptr_ = std::make_shared(this); + + // controller + controller_ptr_ = std::make_shared( + vehicle_info_.max_longitudinal_offset_m, acc_param_); + + // wait for self pose + self_pose_listener_.waitForFirstPose(); + + // publishers, subscribers + pub_trajectory_ = create_publisher("~/output/trajectory", 1); + pub_velocity_limit_ = create_publisher("~/output/velocity_limit", 1); + pub_clear_velocity_limit_ = + create_publisher("~/output/clear_velocity_limit", 1); + + sub_trajectory_ = create_subscription( + "~/input/trajectory", 1, std::bind(&AdaptiveCruiseControllerNode::onTrajectory, this, _1)); + sub_odometry_ = create_subscription( + "~/input/odometry", 1, std::bind(&AdaptiveCruiseControllerNode::onOdometry, this, _1)); + sub_objects_ = create_subscription( + "~/input/objects", 1, std::bind(&AdaptiveCruiseControllerNode::onPredictedObjects, this, _1)); +} + +void AdaptiveCruiseControllerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + // initialize + debug_node_ptr_->resetDebugValues(); + + current_pose_ = self_pose_listener_.getCurrentPose(); + if (!isDataReady()) { + return; + } + + TrajectoryPoints trajectory_points = tier4_autoware_utils::convertToTrajectoryPointArray(*msg); + + // trim trajectory from self pose + const auto base_trajectory = + trimTrajectoryWithIndexFromSelfPose(trajectory_points, *current_pose_); + // extend trajectory to consider objects after the goal + const auto extend_trajectory = extendTrajectory(base_trajectory, param_.trajectory_extend_length); + // decimate trajectory for calculation cost + const auto decimate_trajectory = + decimateTrajectory(extend_trajectory, param_.decimate_step_length); + + if (decimate_trajectory.size() < 2) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "trajectory is too short."); + pub_trajectory_->publish(*msg); + return; + } + + // create rough detection area + std::vector rough_detection_area; + createPolygonFromTrajectoryPoints( + decimate_trajectory, param_.rough_detection_area_expand_length, rough_detection_area); + // extract objects in rough detection area + PredictedObjects objects_in_rough_detection_area; + extractVehicleObjectsInPolygon( + *current_objects_ptr_, rough_detection_area, objects_in_rough_detection_area); + + // create detection area + std::vector detection_area; + createPolygonFromTrajectoryPoints( + decimate_trajectory, param_.detection_area_expand_length, detection_area); + + // extract target objects of adaptive cruise controller + PredictedObjects target_objects; + getTargetObjects( + objects_in_rough_detection_area, detection_area, decimate_trajectory, target_objects); + + if (target_objects.objects.empty()) { + // no target object + publishDebugOutputWithNoTarget(); + pub_trajectory_->publish(*msg); + if (need_to_clear_velocity_limit_) { + pub_clear_velocity_limit_->publish(createVelocityLimitClearCommandMsg()); + need_to_clear_velocity_limit_ = false; + } + return; + } + + // get nearest object from target object + PredictedObject nearest_target_object; + getNearestObject(target_objects, decimate_trajectory, nearest_target_object); + + // send velocity / insert velocity to trajectory + TrajectoryPoints output; + { + // get information for adaptive cruise control + const auto object_time = rclcpp::Time(target_objects.header.stamp); + controller_ptr_->calcInformationForAdaptiveCruise( + trajectory_points, *current_pose_, *current_odometry_ptr_, nearest_target_object, + object_time); + + // calculate target motion (target_velocity, and trajectory) + controller_ptr_->calculate(); + + // send target velocity + double target_velocity; + double target_acceleration; + double target_jerk; + if (controller_ptr_->getTargetMotion(target_velocity, target_acceleration, target_jerk)) { + pub_velocity_limit_->publish( + createVelocityLimitMsg(target_velocity, target_acceleration, target_jerk)); + need_to_clear_velocity_limit_ = true; + } else { + if (need_to_clear_velocity_limit_) { + pub_clear_velocity_limit_->publish(createVelocityLimitClearCommandMsg()); + need_to_clear_velocity_limit_ = false; + } + } + + // get trajectory (If necessary, insert stop velocity.) + bool emergency; + output = controller_ptr_->getAccTrajectory(emergency); + } + + // publish trajectory + auto output_trajectory = tier4_autoware_utils::convertToTrajectory(output); + output_trajectory.header = msg->header; + pub_trajectory_->publish(output_trajectory); + + // TODO(kimura) send emergency flag + + // for debug + fillAndPublishDebugOutput(nearest_target_object); +} + +void AdaptiveCruiseControllerNode::onOdometry(const Odometry::ConstSharedPtr msg) +{ + current_odometry_ptr_ = msg; +} + +void AdaptiveCruiseControllerNode::onPredictedObjects(const PredictedObjects::ConstSharedPtr msg) +{ + current_objects_ptr_ = msg; +} + +bool AdaptiveCruiseControllerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "waiting for current_pose..."); + return false; + } + + if (!current_odometry_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "waiting for odometry msg..."); + return false; + } + + if (!current_objects_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "waiting for objects msg..."); + return false; + } + + return true; +} + +TrajectoryPoints AdaptiveCruiseControllerNode::trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const geometry_msgs::msg::PoseStamped & self_pose) +{ + TrajectoryPoints output{}; + + double min_distance = 0.0; + size_t min_distance_index = 0; + bool is_init = false; + for (size_t i = 0; i < input.size(); ++i) { + const double x = input.at(i).pose.position.x - self_pose.pose.position.x; + const double y = input.at(i).pose.position.y - self_pose.pose.position.y; + const double squared_distance = x * x + y * y; + if (!is_init || squared_distance < min_distance * min_distance) { + is_init = true; + min_distance = std::sqrt(squared_distance); + min_distance_index = i; + } + } + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + + return output; +} + +TrajectoryPoint AdaptiveCruiseControllerNode::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; + geometry_msgs::msg::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; +} + +TrajectoryPoints AdaptiveCruiseControllerNode::extendTrajectory( + const TrajectoryPoints & input, const double extend_distance) +{ + TrajectoryPoints output = input; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output; + } + + const auto goal_point = input.back(); + double interpolation_distance = 0.1; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + output.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + output.push_back(extend_trajectory_point); + + return output; +} + +TrajectoryPoints AdaptiveCruiseControllerNode::decimateTrajectory( + const TrajectoryPoints & input, const double step_length) +{ + TrajectoryPoints output{}; + + if (input.empty()) { + return output; + } + + double trajectory_length_sum = 0.0; + double next_length = 0.0; + + for (int i = 0; i < static_cast(input.size()) - 1; ++i) { + const auto & p_front = input.at(i); + const auto & p_back = input.at(i + 1); + constexpr double epsilon = 1e-3; + + if (next_length <= trajectory_length_sum + epsilon) { + const auto p_interpolate = + getBackwardPointFromBasePoint(p_front, p_back, p_back, next_length - trajectory_length_sum); + output.push_back(p_interpolate); + next_length += step_length; + continue; + } + + trajectory_length_sum += tier4_autoware_utils::calcDistance2d(p_front, p_back); + } + + output.push_back(input.back()); + + return output; +} + +TrajectoryPoint AdaptiveCruiseControllerNode::getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length) +{ + TrajectoryPoint output; + const double dx = p_to.pose.position.x - p_from.pose.position.x; + const double dy = p_to.pose.position.y - p_from.pose.position.y; + const double norm = std::hypot(dx, dy); + + output = p_base; + output.pose.position.x += backward_length * dx / norm; + output.pose.position.y += backward_length * dy / norm; + + return output; +} + +VelocityLimit AdaptiveCruiseControllerNode::createVelocityLimitMsg( + const double velocity, const double acceleration, const double jerk) +{ + VelocityLimit msg; + msg.stamp = this->now(); + msg.sender = "adaptive_cruise_controller"; + msg.max_velocity = velocity; + msg.use_constraints = true; + msg.constraints.min_acceleration = std::min(0.0, acceleration); + msg.constraints.min_jerk = std::min(0.0, jerk); + msg.constraints.max_jerk = -msg.constraints.min_jerk; + return msg; +} + +VelocityLimitClearCommand AdaptiveCruiseControllerNode::createVelocityLimitClearCommandMsg() +{ + VelocityLimitClearCommand msg; + msg.stamp = this->now(); + msg.sender = "adaptive_cruise_controller"; + msg.command = true; + return msg; +} + +void AdaptiveCruiseControllerNode::createPolygonFromTrajectoryPoints( + const TrajectoryPoints & trajectory_points, const double expand_width, + std::vector & polygons) +{ + std::vector vehicle_corner_points; + + const auto & i = vehicle_info_; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + expand_width; + const auto & back_m = i.rear_overhang_m; + + for (size_t i = 0; i < trajectory_points.size() - 1; i++) { + std::vector polygon; + vehicle_corner_points.clear(); + // create convex hull polygon from trajectory points i and i+1 + for (size_t j = i; j <= i + 1; j++) { + const auto point = trajectory_points.at(i); + const auto yaw = tf2::getYaw(point.pose.orientation); + vehicle_corner_points.push_back(cv::Point2d( + point.pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + point.pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + vehicle_corner_points.push_back(cv::Point2d( + point.pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + point.pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + vehicle_corner_points.push_back(cv::Point2d( + point.pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + point.pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + vehicle_corner_points.push_back(cv::Point2d( + point.pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + point.pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + convexHull(vehicle_corner_points, polygon); + // convert cv-polygon to boost-polygon + Polygon2d boost_polygon; + convertcvPointsToBoostPolygon(polygon, boost_polygon); + polygons.emplace_back(boost_polygon); + } +} + +void AdaptiveCruiseControllerNode::extractVehicleObjectsInPolygon( + const PredictedObjects & objects, const std::vector & target_polygons, + PredictedObjects & objects_in_polygon) +{ + objects_in_polygon.header = objects.header; + for (const auto object : objects.objects) { + if (!isTargetObjectType(object)) { + // the object is not target vehicle type + continue; + } + + Polygon2d object_polygon; + convertObjectToBoostPolygon(object, object_polygon); + if (isObjectWithinPolygon(target_polygons, object_polygon)) { + objects_in_polygon.objects.emplace_back(object); + } + } +} + +void AdaptiveCruiseControllerNode::getTargetObjects( + const PredictedObjects & objects, const std::vector & target_polygons, + const TrajectoryPoints & target_trajectory, PredictedObjects & target_objects) +{ + target_objects.header = objects.header; + + for (const auto object : objects.objects) { + // check if the object is in the target area + // and if the object angle is aligned with the trajectory angle. + Polygon2d object_polygon; + convertObjectToBoostPolygon(object, object_polygon); + bool object_in_target_area = isObjectWithinPolygon(target_polygons, object_polygon); + bool align_object_angle = isAngleAlignedWithTrajectory( + getObjectPose(object), target_trajectory, param_.object_threshold_angle); + + if (object_in_target_area && align_object_angle) { + target_objects.objects.emplace_back(object); + continue; + } + + // check if the object velocity is high + // and if the predicted_path and trajectory overlap for a certain period of time. + bool object_velocity_high = isObjectVelocityHigh(object); + double threshold_dist_of_path_and_trajectory = vehicle_info_.vehicle_width_m / 2.0 + + object.shape.dimensions.y / 2.0 + + param_.detection_area_expand_length; + bool overlap_predicted_path_with_trajectory = isPathNearTrajectory( + getHighestConfidencePathFromObject(object), target_trajectory, + threshold_dist_of_path_and_trajectory, param_.predicted_path_threshold_angle, + param_.mininum_overlap_time_of_predicted_path); + + if (object_velocity_high && overlap_predicted_path_with_trajectory) { + target_objects.objects.emplace_back(object); + continue; + } + } +} + +void AdaptiveCruiseControllerNode::getNearestObject( + const PredictedObjects & objects, const TrajectoryPoints & trajectory_points, + PredictedObject & nearest_object) +{ + double minimum_length_to_object = std::numeric_limits::max(); + + for (const auto object : objects.objects) { + const auto object_point = object.kinematics.initial_pose_with_covariance.pose.position; + + const auto length_to_object = tier4_autoware_utils::calcSignedArcLength( + trajectory_points, current_pose_->pose.position, object_point); + + if (length_to_object < minimum_length_to_object) { + minimum_length_to_object = length_to_object; + nearest_object = object; + } + } +} + +PredictedPath AdaptiveCruiseControllerNode::getHighestConfidencePathFromObject( + const PredictedObject & object) +{ + double highest_confidence = 0; + PredictedPath highest_confidence_path; + + for (const auto path : object.kinematics.predicted_paths) { + if (highest_confidence < path.confidence) { + highest_confidence = path.confidence; + highest_confidence_path = path; + } + } + return highest_confidence_path; +} + +geometry_msgs::msg::Pose AdaptiveCruiseControllerNode::getObjectPose(const PredictedObject & object) +{ + const double object_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; + auto obj_pose = object.kinematics.initial_pose_with_covariance.pose; + double obj_yaw, obj_pitch, obj_roll; + tf2::getEulerYPR(obj_pose.orientation, obj_yaw, obj_pitch, obj_roll); + + // If the object is stopped, align object orientation to ego vehicle + if (std::fabs(object_velocity) < acc_param_.object_stop_velocity_thresh) { + const auto ego_yaw = tf2::getYaw(current_pose_->pose.orientation); + const auto diff_yaw = tier4_autoware_utils::normalizeRadian(ego_yaw - obj_yaw); + if (std::fabs(diff_yaw) < M_PI / 2.0) { + return obj_pose; + } + // If the pose of the object and ego-vehicle are in opposite directions, + // invert yaw-angle + tf2::Quaternion inv_q; + inv_q.setRPY(obj_roll, obj_pitch, obj_yaw + M_PI); + obj_pose.orientation = tf2::toMsg(inv_q); + return obj_pose; + } + + if (object_velocity >= 0) { + return object.kinematics.initial_pose_with_covariance.pose; + } + + // If the object velocity is negative, invert yaw-angle + tf2::Quaternion inv_q; + inv_q.setRPY(obj_roll, obj_pitch, obj_yaw + M_PI); + obj_pose.orientation = tf2::toMsg(inv_q); + return obj_pose; +} + +bool AdaptiveCruiseControllerNode::isTargetObjectType(const PredictedObject & object) +{ + using Label = ObjectClassification; + + const auto object_label = object.classification.front().label; + if ( + object_label == Label::BUS || object_label == Label::CAR || object_label == Label::TRAILER || + object_label == Label::MOTORCYCLE || object_label == Label::TRUCK) { + return true; + } + return false; +} + +bool AdaptiveCruiseControllerNode::isObjectVelocityHigh(const PredictedObject & object) +{ + const auto object_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; + return object_velocity > acc_param_.object_low_velocity_thresh; +} + +/* only for debug */ + +void AdaptiveCruiseControllerNode::publishDebugOutputWithNoTarget() +{ + resetObjectTwistHistory(); + if (prev_acc_info_) { + debug_node_ptr_->setDebugValues(DebugValues::TYPE::FLAG_CUTOUT_OBJECT, 1.0); + } + prev_acc_info_.reset(); + debug_node_ptr_->clearMarker(); + const auto ego_twist_stamped = + toTwistStamped(current_odometry_ptr_->header, current_odometry_ptr_->twist.twist); + ego_accel_ = calcAcc(ego_twist_stamped, prev_ego_twist_, ego_accel_); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::CURRENT_VEL, current_odometry_ptr_->twist.twist.linear.x); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_ACC, ego_accel_); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::FLAG_NONE, 1.0); + debug_node_ptr_->publish(); +} + +void AdaptiveCruiseControllerNode::fillAndPublishDebugOutput(const PredictedObject & target_object) +{ + const auto acc_info_ptr = controller_ptr_->getAccInfoPtr(); + const auto prev_acc_info_ptr = controller_ptr_->getPrevAccInfoPtr(); + const auto acc_motion = controller_ptr_->getAccMotion(); + const auto acc_state = controller_ptr_->getState(); + + // correct object twist + auto target_object_abs_twist = target_object; + auto & obj_twist = + target_object_abs_twist.kinematics.initial_twist_with_covariance.twist.linear.x; + obj_twist = std::fabs(obj_twist); + + // calculate ego-vehicle acceleration + const auto ego_twist_stamped = + toTwistStamped(current_odometry_ptr_->header, current_odometry_ptr_->twist.twist); + ego_accel_ = calcAcc(ego_twist_stamped, prev_ego_twist_, ego_accel_); + + // calculate target-object acceleration + const auto obj_twist_stamped = toTwistStamped( + current_objects_ptr_->header, + target_object_abs_twist.kinematics.initial_twist_with_covariance.twist); + + const auto cut_in_out = !acc_info_ptr ? CUT_IN_OUT::NONE : detectCutInAndOut(*acc_info_ptr); + + if (cut_in_out == CUT_IN_OUT::NONE) { + obj_accel_ = calcAcc(obj_twist_stamped, prev_object_twist_, obj_accel_); + } else { + prev_object_twist_.reset(); + obj_accel_ = 0.0; + } + + // calculate target-object velocity by differential of distance to object from ego-vehicle + double object_vel_by_diff_target_dist = 0.0; + if (acc_info_ptr && prev_acc_info_ptr) { + if (cut_in_out == CUT_IN_OUT::NONE) { + const double diff_dist = + acc_info_ptr->current_distance_to_object - prev_acc_info_ptr->current_distance_to_object; + const double dt = (acc_info_ptr->info_time - prev_acc_info_ptr->info_time).seconds(); + // calculate the speed of change of target distance + object_vel_by_diff_target_dist = dt > 0 ? diff_dist / dt : 0.0; + // remove the effect of own vehicle speed + object_vel_by_diff_target_dist += current_odometry_ptr_->twist.twist.linear.x; + } + } + + const auto target_velocity = acc_state == State::STOP ? 0.0 : acc_motion.target_velocity; + + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::CURRENT_VEL, current_odometry_ptr_->twist.twist.linear.x); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_ACC, ego_accel_); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::CURRENT_OBJECT_VEL, acc_info_ptr->current_object_velocity); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::CURRENT_OBJECT_VEL_DIFF_DIST, object_vel_by_diff_target_dist); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_OBJECT_ACC, obj_accel_); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::CURRENT_OBJECT_DISTANCE, acc_info_ptr->current_distance_to_object); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::IDEAL_OBJECT_DISTANCE, acc_info_ptr->ideal_distance_to_object); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_CUTIN_OBJECT, cut_in_out == CUT_IN_OUT::CUT_IN); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_CUTOUT_OBJECT, cut_in_out == CUT_IN_OUT::CUT_OUT); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::FLAG_FIND_OBJECT, 1.0); + debug_node_ptr_->setDebugValues( + DebugValues::TYPE::FLAG_ADAPTIVE_CRUISE, (acc_state == State::ACC)); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::FLAG_STOP, (acc_state == State::STOP)); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::FLAG_NONE, 0.0); + debug_node_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_TARGET_VELOCITY, target_velocity); + + debug_node_ptr_->clearMarker(); + debug_node_ptr_->setTargetPolygon(target_object); + if (acc_state == State::ACC) { + const auto target_object_pose = target_object.kinematics.initial_pose_with_covariance.pose; + const auto closet_pose = + tier4_autoware_utils::findNearestIndex(acc_info_ptr->original_trajectory, target_object_pose); + if (closet_pose) { + const auto offset = + acc_param_.minimum_margin_distance + target_object.shape.dimensions.x / 2.0; + debug_node_ptr_->setVirtualWall( + acc_info_ptr->original_trajectory.at(*closet_pose).pose, -offset, false); + } + + } else if (acc_state == State::STOP) { + const auto stop_pose = controller_ptr_->getAccMotion().stop_pose; + debug_node_ptr_->setVirtualWall(stop_pose, vehicle_info_.max_longitudinal_offset_m, true); + } + debug_node_ptr_->publish(); +} + +double AdaptiveCruiseControllerNode::calcAcc( + const geometry_msgs::msg::TwistStamped & twist, + std::shared_ptr & prev_twist, const double prev_acc, + const double lowpass_gain, const double timeout) +{ + if (!prev_twist) { + prev_twist = std::make_shared(twist); + return 0.0; + } + + const double dt = + (rclcpp::Time(twist.header.stamp) - rclcpp::Time(prev_twist->header.stamp)).seconds(); + const double dv = twist.twist.linear.x - prev_twist->twist.linear.x; + prev_twist = std::make_shared(twist); + + if (dt > timeout) { + return 0.0; + } + + if (dt <= 0.0) { + return prev_acc; + } + + const double acc = dv / dt; + return prev_acc * lowpass_gain + acc * (1.0 - lowpass_gain); +} + +void AdaptiveCruiseControllerNode::resetObjectTwistHistory() +{ + prev_object_twist_.reset(); + obj_accel_ = 0.0; +} + +geometry_msgs::msg::TwistStamped AdaptiveCruiseControllerNode::toTwistStamped( + const std_msgs::msg::Header & header, const geometry_msgs::msg::Twist & twist) +{ + geometry_msgs::msg::TwistStamped twist_stamped; + twist_stamped.header = header; + twist_stamped.twist = twist; + return twist_stamped; +} + +CUT_IN_OUT AdaptiveCruiseControllerNode::detectCutInAndOut( + AdaptiveCruiseInformation acc_info, const double threshold_length) +{ + if (!prev_acc_info_) { + prev_acc_info_ = std::make_shared(acc_info); + return CUT_IN_OUT::CUT_IN; + } + + const double d_dist = + acc_info.current_distance_to_object - prev_acc_info_->current_distance_to_object; + prev_acc_info_ = std::make_shared(acc_info); + + const double dt = (this->now() - prev_acc_info_->info_time).seconds(); + if (dt > acc_param_.reset_time_to_acc_state) { + // previous acc info is too old. + return CUT_IN_OUT::CUT_IN; + } + + if (d_dist > threshold_length) return CUT_IN_OUT::CUT_OUT; + if (d_dist < -threshold_length) return CUT_IN_OUT::CUT_IN; + return CUT_IN_OUT::NONE; +} + +} // namespace motion_planning + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::AdaptiveCruiseControllerNode) \ No newline at end of file diff --git a/planning/adaptive_cruise_controller/src/adaptive_cruise_control_core.cpp b/planning/adaptive_cruise_controller/src/adaptive_cruise_control_core.cpp new file mode 100644 index 0000000000000..82d012fc46d98 --- /dev/null +++ b/planning/adaptive_cruise_controller/src/adaptive_cruise_control_core.cpp @@ -0,0 +1,155 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace motion_planning +{ + +AdaptiveCruiseControlCore::AdaptiveCruiseControlCore( + const double baselink2front, const AccParam & acc_param) +: baselink2front_(baselink2front), acc_param_(acc_param) +{ + acc_pid_node_ptr_ = std::make_shared(baselink2front, acc_param); +} + +void AdaptiveCruiseControlCore::calcInformationForAdaptiveCruise( + const TrajectoryPoints & trajectory_points, const geometry_msgs::msg::PoseStamped & pose, + const Odometry & odometry, const PredictedObject & object, const rclcpp::Time & object_time) +{ + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const double object_velocity = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); + double object_diff_angle = 0.0; + getDiffAngleWithTrajectory(object_pose, trajectory_points, object_diff_angle); + + /* calculate the object velocity along trajectory */ + const double object_velocity_along_traj = object_velocity * std::cos(object_diff_angle); + + /* calculate distance to object */ + // calculate raw distance to the center-position of object from the base_link of ego-car + double dist_to_object = tier4_autoware_utils::calcSignedArcLength( + trajectory_points, pose.pose.position, object_pose.position); + + // considering the size of ego-car and object + dist_to_object -= (baselink2front_ + object.shape.dimensions.x / 2.0); + + // delay compensation ( add object's travling distance in the delay time ) + if (object_velocity_along_traj > acc_param_.object_stop_velocity_thresh) { + const double delay_time = (rclcpp::Time(pose.header.stamp) - object_time).seconds(); + const double running_distance_in_delay = delay_time * object_velocity_along_traj; + dist_to_object += running_distance_in_delay; + } + + /* input acc information */ + if (acc_info_ptr_) { + prev_acc_info_ptr_ = std::make_shared(*acc_info_ptr_); + } + + acc_info_ptr_ = std::make_shared(); + acc_info_ptr_->current_distance_to_object = dist_to_object; + acc_info_ptr_->current_ego_velocity = odometry.twist.twist.linear.x; + acc_info_ptr_->current_object_velocity = object_velocity_along_traj; + acc_info_ptr_->ideal_distance_to_object = + getIdealDistanceToObject(odometry.twist.twist.linear.x, object_velocity_along_traj); + acc_info_ptr_->info_time = rclcpp::Time(pose.header.stamp); + acc_info_ptr_->current_ego_pose = pose.pose; + acc_info_ptr_->target_object = object; + acc_info_ptr_->original_trajectory = trajectory_points; +} + +double AdaptiveCruiseControlCore::getIdealDistanceToObject( + const double current_velocity, const double object_velocity) +{ + // ego current velocity ... v_ego + // ego minimum deceleration ... a_ego + // object velocity ... v_obj + // object minimum deceleration ... a_obj + // idling_time ... t_idling + // ideal_distance ... D + // minimum_margin_distance ... d_margin + // D = d_margin + v_e * t_idling + v_ego^2/(2*|a_ego|) - v_obj^2/(2*|a_obj|) + + const double margin_distance = acc_param_.minimum_margin_distance; + const double idle_traving_distance = current_velocity * acc_param_.idling_time; + const double ego_braking_distance = + (current_velocity * current_velocity) / (2.0 * std::fabs(acc_param_.acc_min_acceleration)); + const double object_braking_distance = + (object_velocity * object_velocity) / (2.0 * std::fabs(acc_param_.object_min_acceleration)); + + return margin_distance + idle_traving_distance + ego_braking_distance - object_braking_distance; +} + +void AdaptiveCruiseControlCore::calculate() +{ + if (!acc_info_ptr_) { + return; + } + + acc_pid_node_ptr_->calculate(*acc_info_ptr_, acc_motion_); +} + +State AdaptiveCruiseControlCore::getState() +{ + if (!acc_pid_node_ptr_) { + return State::NONE; + } + return acc_pid_node_ptr_->getState(); +} + +bool AdaptiveCruiseControlCore::getTargetMotion( + double & target_velocity, double & target_acc, double & target_jerk) +{ + if (!acc_pid_node_ptr_ || !acc_info_ptr_) { + return false; + } + + if (!acc_motion_.use_target_motion) { + // no target motion + return false; + } + + target_velocity = acc_motion_.target_velocity; + target_acc = acc_motion_.target_acceleration; + target_jerk = acc_motion_.target_jerk; + + return true; +} + +TrajectoryPoints AdaptiveCruiseControlCore::getAccTrajectory(bool & emergency_flag) +{ + if (!acc_pid_node_ptr_ || !acc_info_ptr_) { + emergency_flag = false; + return acc_info_ptr_->original_trajectory; + } + + if (!acc_motion_.use_trajectory) { + // no trajectory update + emergency_flag = false; + return acc_info_ptr_->original_trajectory; + } + + emergency_flag = acc_motion_.emergency; + return acc_motion_.planned_trajectory; +} + +} // namespace motion_planning \ No newline at end of file diff --git a/planning/adaptive_cruise_controller/src/debug_marker.cpp b/planning/adaptive_cruise_controller/src/debug_marker.cpp new file mode 100644 index 0000000000000..ae2254ddcd8b3 --- /dev/null +++ b/planning/adaptive_cruise_controller/src/debug_marker.cpp @@ -0,0 +1,121 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace motion_planning +{ + +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; +using tier4_autoware_utils::createSlowDownVirtualWallMarker; +using tier4_autoware_utils::createStopVirtualWallMarker; + +AdaptiveCruiseControllerDebugNode::AdaptiveCruiseControllerDebugNode(rclcpp::Node * node) +: node_(node) +{ + pub_debug_marker_ = + node_->create_publisher("~/debug/marker", 1); + + pub_debug_value_ = node_->create_publisher("~/debug/debug_values", 1); +} + +void AdaptiveCruiseControllerDebugNode::setVirtualWall( + const geometry_msgs::msg::Pose & stop_pose, const double offset, const bool is_stop_wall) +{ + const auto p = calcOffsetPose(stop_pose, offset, 0.0, 0.0); + MarkerArray markers; + + if (is_stop_wall) { + markers = createStopVirtualWallMarker(p, "adaptive cruise", node_->now(), 0); + } else { + markers = createSlowDownVirtualWallMarker(p, "adaptive cruise", node_->now(), 0); + } + + virtual_wall_marker_ptr_ = std::make_shared(markers); +} +void AdaptiveCruiseControllerDebugNode::setTargetPolygon(const PredictedObject & object) +{ + auto marker = createDefaultMarker( + "map", node_->now(), "adaptive_cruise_polygons", 0, Marker::CUBE, + createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + target_vehicle_marker_ptr_ = std::make_shared(marker); +} + +void AdaptiveCruiseControllerDebugNode::clearMarker() +{ + clearVirtualWall(); + clearTargetPolygon(); +} + +void AdaptiveCruiseControllerDebugNode::clearVirtualWall() { virtual_wall_marker_ptr_.reset(); } + +void AdaptiveCruiseControllerDebugNode::clearTargetPolygon() { target_vehicle_marker_ptr_.reset(); } + +void AdaptiveCruiseControllerDebugNode::publish() +{ + publishMarker(); + publishDebugValues(); +} + +void AdaptiveCruiseControllerDebugNode::publishMarker() +{ + MarkerArray marker_array; + + // virtual wall marker + if (virtual_wall_marker_ptr_) { + for (const auto marker : virtual_wall_marker_ptr_->markers) { + marker_array.markers.emplace_back(marker); + } + } + + // target vehicle marker + if (target_vehicle_marker_ptr_) { + marker_array.markers.emplace_back(*target_vehicle_marker_ptr_); + } + + pub_debug_marker_->publish(marker_array); +} + +void AdaptiveCruiseControllerDebugNode::publishDebugValues() +{ + pub_debug_value_->publish(convertDebugValuesToMsg(debug_values_)); +} + +Float32MultiArrayStamped AdaptiveCruiseControllerDebugNode::convertDebugValuesToMsg( + const DebugValues debug_value) +{ + Float32MultiArrayStamped debug_msg{}; + debug_msg.stamp = node_->now(); + for (const auto & v : debug_value.getValues()) { + debug_msg.data.push_back(v); + } + return debug_msg; +} + +} // namespace motion_planning diff --git a/planning/adaptive_cruise_controller/src/utilities.cpp b/planning/adaptive_cruise_controller/src/utilities.cpp new file mode 100644 index 0000000000000..69a743f4bf028 --- /dev/null +++ b/planning/adaptive_cruise_controller/src/utilities.cpp @@ -0,0 +1,254 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace motion_planning +{ + +void convexHull(const std::vector & points, std::vector & polygon_points) +{ + cv::Point2d centroid; + centroid.x = 0; + centroid.y = 0; + for (const auto & point : points) { + centroid.x += point.x; + centroid.y += point.y; + } + centroid.x = centroid.x / static_cast(points.size()); + centroid.y = centroid.y / static_cast(points.size()); + + std::vector normalized_points; + std::vector normalized_polygon_points; + for (size_t i = 0; i < points.size(); ++i) { + // 1mm-order + normalized_points.push_back( + cv::Point((points.at(i).x - centroid.x) * 1000.0, (points.at(i).y - centroid.y) * 1000.0)); + } + cv::convexHull(normalized_points, normalized_polygon_points); + + for (size_t i = 0; i < normalized_polygon_points.size(); ++i) { + cv::Point2d polygon_point; + polygon_point.x = (normalized_polygon_points.at(i).x / 1000.0 + centroid.x); + polygon_point.y = (normalized_polygon_points.at(i).y / 1000.0 + centroid.y); + polygon_points.push_back(polygon_point); + } +} + +bool isObjectWithinPolygon( + const std::vector & target_polygons, const Polygon2d & object_polygon, + const double overlap_threshold) +{ + Polygon2d boost_target_polygon; + + for (const auto & target_polygon : target_polygons) { + std::vector overlap_polygons; + bg::intersection(target_polygon, object_polygon, overlap_polygons); + double overlap_polygon_area = 0.0; + for (const auto overlap_polygon : overlap_polygons) { + overlap_polygon_area += bg::area(overlap_polygon); + } + + const double overlap_area_rate = overlap_polygon_area / bg::area(object_polygon); + + if (overlap_area_rate > overlap_threshold) { + return true; + } + } + return false; +} + +bool getDiffAngleWithTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & trajectory_points, + double & diff_angle) +{ + const auto nearest_index = tier4_autoware_utils::findNearestIndex(trajectory_points, pose); + if (!nearest_index) { + return false; + } + + const auto pose_angle = tf2::getYaw(pose.orientation); + const auto trajectory_angle = tf2::getYaw(trajectory_points.at(*nearest_index).pose.orientation); + + diff_angle = tier4_autoware_utils::normalizeRadian(pose_angle - trajectory_angle); + return true; +} + +bool isAngleAlignedWithTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & trajectory_points, + const double threshold_angle) +{ + double diff_angle; + + if (!getDiffAngleWithTrajectory(pose, trajectory_points, diff_angle)) { + return false; + } + + return std::fabs(diff_angle) <= threshold_angle; +} + +bool isPoseNearTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & trajectory_points, + const double threshold_dist, const double threshold_angle) +{ + return static_cast(tier4_autoware_utils::findNearestIndex( + trajectory_points, pose, threshold_dist, threshold_angle)); +} + +bool isPathNearTrajectory( + const PredictedPath & path, const TrajectoryPoints & trajectory_points, + const double threshold_dist, const double threshold_angle, const double minmum_overlap_time) +{ + std::vector overlap_path_point_index_que; + for (size_t i = 0; i < path.path.size(); i++) { + const auto path_pose = path.path.at(i); + if (isPoseNearTrajectory(path_pose, trajectory_points, threshold_dist, threshold_angle)) { + overlap_path_point_index_que.emplace_back(i); + } + } + + if (overlap_path_point_index_que.size() < 2) { + // overlap path is too short. + return false; + } + + const double timespan_of_overlap_path_point = + static_cast( + (overlap_path_point_index_que.back() - overlap_path_point_index_que.front())) * + rclcpp::Duration(path.time_step).seconds(); + + if (timespan_of_overlap_path_point < minmum_overlap_time) { + // overlap time of path and trajectory is too short. + return false; + } + + return true; +} + +void convertObjectToBoostPolygon(const PredictedObject & object, Polygon2d & object_polygon) +{ + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const auto & pose = object.kinematics.initial_pose_with_covariance.pose; + const double yaw = tier4_autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation)); + Eigen::Matrix2d rotation; + rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + Eigen::Vector2d offset0, offset1, offset2, offset3; + offset0 = rotation * + Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + offset1 = rotation * + Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + offset2 = rotation * + Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f); + offset3 = rotation * + Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + object_polygon.outer().push_back(boost::geometry::make( + pose.position.x + offset0.x(), pose.position.y + offset0.y())); + object_polygon.outer().push_back(boost::geometry::make( + pose.position.x + offset1.x(), pose.position.y + offset1.y())); + object_polygon.outer().push_back(boost::geometry::make( + pose.position.x + offset2.x(), pose.position.y + offset2.y())); + object_polygon.outer().push_back(boost::geometry::make( + pose.position.x + offset3.x(), pose.position.y + offset3.y())); + object_polygon.outer().push_back(object_polygon.outer().front()); + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + const auto & center = object.kinematics.initial_pose_with_covariance.pose; + const auto & radius = object.shape.dimensions.x * 0.5; + constexpr int n = 6; + for (int i = 0; i < n; ++i) { + Eigen::Vector2d point; + point.x() = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.position.x; + point.y() = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.position.y; + object_polygon.outer().push_back( + boost::geometry::make(point.x(), point.y())); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const auto & pose = object.kinematics.initial_pose_with_covariance.pose; + for (const auto & point : object.shape.footprint.points) { + object_polygon.outer().push_back(boost::geometry::make( + pose.position.x + point.x, pose.position.y + point.y)); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + } + object_polygon = isClockWise(object_polygon) ? object_polygon : inverseClockWise(object_polygon); +} + +void convertcvPointsToBoostPolygon( + const std::vector & points, Polygon2d & boost_polygon) +{ + for (const auto & point : points) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back(bg::make(points.front().x, points.front().y)); +} + +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) +{ + tf2::Transform tf_transform1, tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + const auto & tf_quaternion = + tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); + + geometry_msgs::msg::Pose pose{}; + pose.position.x = tf_point.x(); + pose.position.y = tf_point.y(); + pose.position.z = tf_point.z(); + pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +bool isClockWise(const Polygon2d & polygon) +{ + const auto n = polygon.outer().size(); + + const double x_offset = polygon.outer().at(0).x(); + const double y_offset = polygon.outer().at(0).y(); + double sum = 0.0; + for (std::size_t i = 0; i < polygon.outer().size(); ++i) { + sum += + (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - + (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); + } + return sum < 0.0; +} + +Polygon2d inverseClockWise(const Polygon2d & polygon) +{ + const auto & poly = polygon.outer(); + Polygon2d inverted_polygon; + inverted_polygon.outer().reserve(poly.size()); + std::reverse_copy(poly.begin(), poly.end(), std::back_inserter(inverted_polygon.outer())); + return inverted_polygon; +} + +} // namespace motion_planning \ No newline at end of file