From 259a4de508cf88f833bda2018fe8ead7c0c90e04 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 25 Feb 2021 19:25:35 +0900 Subject: [PATCH 01/62] wip --- .../include/obstacle_stop_planner/util.hpp | 37 +++++++++++++++++++ .../src/adaptive_cruise_control.cpp | 23 ------------ obstacle_stop_planner_refine/src/node.cpp | 35 ++++++------------ 3 files changed, 49 insertions(+), 46 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp new file mode 100644 index 00000000..940b7f10 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -0,0 +1,37 @@ +// Copyright 2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_STOP_PLANNER__UTIL_HPP_ +#define OBSTACLE_STOP_PLANNER__UTIL_HPP_ + +#include "opencv2/core/core.hpp" + +namespace motion_planning +{ + cv::Point2d calcCentroid(const std::vector pointcloud) + { + cv::Point2d centroid; + centroid.x = 0; + centroid.y = 0; + for (const auto &point : pointcloud) + { + centroid.x += point.x; + centroid.y += point.y; + } + centroid.x = centroid.x / static_cast(pointcloud.size()); + centroid.y = centroid.y / static_cast(pointcloud.size()); + return centroid; + } +} + +#endif // OBSTACLE_STOP_PLANNER__UTIL_HPP_ diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index fc2debb5..9e9bb0ae 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -88,29 +88,6 @@ double getDistanceFromTwoPoint( const double dist = std::hypot(dx, dy); return dist; } - -constexpr double normalizeRadian( - const double rad, const double min_rad = -boost::math::constants::pi(), - const double max_rad = boost::math::constants::pi()) -{ - const auto value = std::fmod(rad, 2 * boost::math::constants::pi()); - if (min_rad < value && value <= max_rad) { - return value; - } else { - return value - std::copysign(2 * boost::math::constants::pi(), value); - } -} - -constexpr double sign(const double value) -{ - if (value > 0) { - return 1.0; - } else if (value < 0) { - return -1.0; - } else { - return 0.0; - } -} } // namespace namespace motion_planning diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index b73ee896..d480f417 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -32,6 +32,7 @@ #include "boost/geometry/geometries/linestring.hpp" #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" +#include "obstacle_stop_planner/util.hpp" #include "vehicle_info_util/vehicle_info.hpp" #define EIGEN_MPL2_ONLY @@ -641,8 +642,8 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) { return min_slow_down_vel_ + (max_slow_down_vel_ - min_slow_down_vel_) * - std::max(lateral_deviation - vehicle_width_ / 2, 0.0) / - expand_slow_down_range_; + std::max(lateral_deviation - vehicle_width_ / 2, 0.0) / + expand_slow_down_range_; } void ObstacleStopPlannerNode::dynamicObjectCallback( @@ -736,13 +737,9 @@ bool ObstacleStopPlannerNode::decimateTrajectory( next_length += step_length; continue; } - const double x = input_trajectory.points.at(i).pose.position.x - - input_trajectory.points.at(i + 1).pose.position.x; - const double y = input_trajectory.points.at(i).pose.position.y - - input_trajectory.points.at(i + 1).pose.position.y; - const double distance = std::sqrt(x * x + y * y); - - trajectory_length_sum += distance; + trajectory_length_sum += std::hypot( + input_trajectory.points.at(i).pose.position.x - input_trajectory.points.at(i + 1).pose.position.x, + input_trajectory.points.at(i).pose.position.y - input_trajectory.points.at(i + 1).pose.position.y); } if (!input_trajectory.points.empty()) { output_trajectory.points.push_back(input_trajectory.points.back()); @@ -761,12 +758,12 @@ bool ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( size_t min_distance_index = 0; bool is_init = false; for (size_t i = 0; i < input_trajectory.points.size(); ++i) { - const double x = input_trajectory.points.at(i).pose.position.x - self_pose.position.x; - const double y = input_trajectory.points.at(i).pose.position.y - self_pose.position.y; - const double squared_distance = x * x + y * y; - if (!is_init || squared_distance < min_distance * min_distance) { + const double distance = std::hypot( + input_trajectory.points.at(i).pose.position.x - self_pose.position.x, + input_trajectory.points.at(i).pose.position.y - self_pose.position.y); + if (!is_init || distance < min_distance) { is_init = true; - min_distance = std::sqrt(squared_distance); + min_distance = distance; min_distance_index = i; } } @@ -873,15 +870,7 @@ void ObstacleStopPlannerNode::createOneStepPolygon( bool ObstacleStopPlannerNode::convexHull( const std::vector pointcloud, std::vector & polygon_points) { - cv::Point2d centroid; - centroid.x = 0; - centroid.y = 0; - for (const auto & point : pointcloud) { - centroid.x += point.x; - centroid.y += point.y; - } - centroid.x = centroid.x / static_cast(pointcloud.size()); - centroid.y = centroid.y / static_cast(pointcloud.size()); + auto centroid = calcCentroid(pointcloud); std::vector normalized_pointcloud; std::vector normalized_polygon_points; From 1ed8da2c8de7a62d742d98df048079162cc069b3 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 25 Feb 2021 19:45:08 +0900 Subject: [PATCH 02/62] wip: add memo --- .../design/class_diagram.pu | 7 ++ obstacle_stop_planner_refine/design/design.md | 67 +++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 obstacle_stop_planner_refine/design/class_diagram.pu create mode 100644 obstacle_stop_planner_refine/design/design.md diff --git a/obstacle_stop_planner_refine/design/class_diagram.pu b/obstacle_stop_planner_refine/design/class_diagram.pu new file mode 100644 index 00000000..63634f7d --- /dev/null +++ b/obstacle_stop_planner_refine/design/class_diagram.pu @@ -0,0 +1,7 @@ +@startuml + +Node <|-- ObstacleStopPlannerNode +ObstacleStopPlannerNode *-- AdaptiveCruiseController +ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode + +@enduml \ No newline at end of file diff --git a/obstacle_stop_planner_refine/design/design.md b/obstacle_stop_planner_refine/design/design.md new file mode 100644 index 00000000..0c9699ee --- /dev/null +++ b/obstacle_stop_planner_refine/design/design.md @@ -0,0 +1,67 @@ +class ObstacleStopPlannerNode + +Key input: pathCallback(input/trajectory) + +Class diagram +```plantuml +@startuml + +Node <|-- ObstacleStopPlannerNode +ObstacleStopPlannerNode *-- AdaptiveCruiseController +ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode + +@enduml +``` + +Proposal class diagram +```plantuml +@startuml + +Node <|-- ObstacleStopPlannerNode +ObstacleStopPlannerNode *-- ObstacleStopPlanner +ObstacleStopPlanner *-- AdaptiveCruiseController +ObstacleStopPlanner *-- ObstacleStopPlannerDebugNode + +@enduml +``` + +Callback + +void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); + +void pathCallback(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg); + +``` +path_pub_->publish(output_msg); +stop_reason_diag_pub_->publish(stop_reason_diag); +debug_ptr_->publish(); +``` + +基本はこのメソッドで処理を行っている。長いが、構造はシンプル。 + +void dynamicObjectCallback( +const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg); +void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg); +void externalExpandStopRangeCallback( +const autoware_debug_msgs::msg::Float32Stamped::ConstSharedPtr input_msg); + +Publisher + +rclcpp::Publisher::SharedPtr path_pub*; +rclcpp::Publisher::SharedPtr stop_reason_diag_pub*; + + +--- + +class AdaptiveCruiseController + +publicメソッドは1つのみ。insertAdaptiveCruiseVelocity()というメソッドでTrajectoryを出力していることから、速度を調整しているっぽい。 +Publisherはdebug用のメッセージのみ。 + + +--- + +class ObstacleStopPlannerDebugNode + +図形を保持してPublishしている？ +PublishするタイミングはpathCallback。 From caa5297b18e1bb99023fa928280f0f6264248028 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Fri, 26 Feb 2021 13:55:55 +0900 Subject: [PATCH 03/62] refactoring --- .../adaptive_cruise_control.hpp | 6 +- .../include/obstacle_stop_planner/util.hpp | 86 ++++++++++++--- .../src/adaptive_cruise_control.cpp | 104 +++--------------- obstacle_stop_planner_refine/src/node.cpp | 2 +- 4 files changed, 94 insertions(+), 104 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 40726502..a5447068 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -43,8 +43,8 @@ class AdaptiveCruiseController const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, - bool * need_to_stop, - autoware_planning_msgs::msg::Trajectory * output_trajectory); + bool & need_to_stop, + autoware_planning_msgs::msg::Trajectory & output_trajectory); private: rclcpp::Publisher::SharedPtr pub_debug_; @@ -200,7 +200,7 @@ class AdaptiveCruiseController void insertMaxVelocityToPath( const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, const double dist_to_collision_point, - autoware_planning_msgs::msg::Trajectory * output_trajectory); + autoware_planning_msgs::msg::Trajectory & output_trajectory); void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); /* Debug */ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 940b7f10..4522b1d8 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -15,23 +15,81 @@ #define OBSTACLE_STOP_PLANNER__UTIL_HPP_ #include "opencv2/core/core.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "boost/geometry.hpp" -namespace motion_planning +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +namespace +{ +cv::Point2d calcCentroid(const std::vector pointcloud) { - cv::Point2d calcCentroid(const std::vector pointcloud) + cv::Point2d centroid; + centroid.x = 0; + centroid.y = 0; + for (const auto &point : pointcloud) { - cv::Point2d centroid; - centroid.x = 0; - centroid.y = 0; - for (const auto &point : pointcloud) - { - centroid.x += point.x; - centroid.y += point.y; - } - centroid.x = centroid.x / static_cast(pointcloud.size()); - centroid.y = centroid.y / static_cast(pointcloud.size()); - return centroid; + centroid.x += point.x; + centroid.y += point.y; } + centroid.x = centroid.x / static_cast(pointcloud.size()); + centroid.y = centroid.y / static_cast(pointcloud.size()); + return centroid; +} + +Point convertPointRosToBoost(const geometry_msgs::msg::Point &point) +{ + const Point point2d(point.x, point.y); + return point2d; +} + +geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion &q) +{ + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + geometry_msgs::msg::Vector3 rpy; + rpy.x = roll; + rpy.y = pitch; + rpy.z = yaw; + return rpy; +} + +Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size, + const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) +{ + Polygon obj_poly; + geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); + + double l = size.x * std::cos(obj_rpy.y) + l_margin; + double w = size.y * std::cos(obj_rpy.x) + w_margin; + double co = center_offset; + bg::exterior_ring(obj_poly) = boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( + -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); + + // rotate polygon + bg::strategy::transform::rotate_transformer rotate(-obj_rpy.z); // original:clockwise + // rotation + Polygon rotate_obj_poly; + bg::transform(obj_poly, rotate_obj_poly, rotate); + // translate polygon + bg::strategy::transform::translate_transformer translate(pose.position.x, pose.position.y); + Polygon translate_obj_poly; + bg::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +double getDistanceFromTwoPoint(const geometry_msgs::msg::Point &point1, const geometry_msgs::msg::Point &point2) +{ + const double dx = point1.x - point2.x; + const double dy = point1.y - point2.y; + const double dist = std::hypot(dx, dy); + return dist; } +} // namespace -#endif // OBSTACLE_STOP_PLANNER__UTIL_HPP_ +#endif // OBSTACLE_STOP_PLANNER__UTIL_HPP_ diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 9e9bb0ae..997215e9 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -26,69 +26,7 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/adaptive_cruise_control.hpp" - -namespace bg = boost::geometry; -using Point = bg::model::d2::point_xy; -using Polygon = bg::model::polygon; -using Line = bg::model::linestring; - -namespace -{ -Point convertPointRosToBoost(const geometry_msgs::msg::Point & point) -{ - const Point point2d(point.x, point.y); - return point2d; -} - -geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) -{ - tf2::Quaternion quat(q.x, q.y, q.z, q.w); - tf2::Matrix3x3 mat(quat); - double roll, pitch, yaw; - mat.getRPY(roll, pitch, yaw); - geometry_msgs::msg::Vector3 rpy; - rpy.x = roll; - rpy.y = pitch; - rpy.z = yaw; - return rpy; -} - -Polygon getPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, - const double center_offset, - const double l_margin = 0.0, const double w_margin = 0.0) -{ - Polygon obj_poly; - geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); - - double l = size.x * std::cos(obj_rpy.y) + l_margin; - double w = size.y * std::cos(obj_rpy.x) + w_margin; - double co = center_offset; - bg::exterior_ring(obj_poly) = boost::assign::list_of(l / 2.0 + co, w / 2.0)( - -l / 2.0 + co, w / 2.0)(-l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); - - // rotate polygon - bg::strategy::transform::rotate_transformer rotate( - -obj_rpy.z); // original:clockwise rotation - Polygon rotate_obj_poly; - bg::transform(obj_poly, rotate_obj_poly, rotate); - // translate polygon - bg::strategy::transform::translate_transformer translate( - pose.position.x, pose.position.y); - Polygon translate_obj_poly; - bg::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} - -double getDistanceFromTwoPoint( - const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2) -{ - const double dx = point1.x - point2.x; - const double dy = point1.y - point2.y; - const double dist = std::hypot(dx, dy); - return dist; -} -} // namespace +#include "obstacle_stop_planner/util.hpp" namespace motion_planning { @@ -169,8 +107,8 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, - const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, - autoware_planning_msgs::msg::Trajectory * output_trajectory) + const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, bool & need_to_stop, + autoware_planning_msgs::msg::Trajectory & output_trajectory) { debug_values_.data.clear(); debug_values_.data.resize(num_debug_values_, 0.0); @@ -195,19 +133,13 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( * estimate velocity of collision point */ if (param_.use_pcl_to_est_vel) { - if (estimatePointVelocityFromPcl( - traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity)) - { - success_estm_vel = true; - } + success_estm_vel = estimatePointVelocityFromPcl( + traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity); } if (param_.use_object_to_est_vel) { - if (estimatePointVelocityFromObject( - object_ptr, traj_yaw, nearest_collision_point, &point_velocity)) - { - success_estm_vel = true; - } + success_estm_vel = estimatePointVelocityFromObject( + object_ptr, traj_yaw, nearest_collision_point, &point_velocity); } if (param_.use_rough_est_vel && !success_estm_vel) { @@ -220,7 +152,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "Failed to estimate velocity of forward vehicle. Insert stop line."); - *need_to_stop = true; + need_to_stop = true; prev_upper_velocity_ = current_velocity; // reset prev_upper_velocity prev_target_velocity_ = 0.0; pub_debug_->publish(debug_values_); @@ -237,7 +169,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "Upper velocity is too low. Insert stop line."); - *need_to_stop = true; + need_to_stop = true; return; } @@ -246,7 +178,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( */ insertMaxVelocityToPath( self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); - *need_to_stop = false; + need_to_stop = false; } void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( @@ -551,15 +483,15 @@ double AdaptiveCruiseController::calcTargetVelocityByPID( void AdaptiveCruiseController::insertMaxVelocityToPath( const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, - const double dist_to_collision_point, autoware_planning_msgs::msg::Trajectory * output_trajectory) + const double dist_to_collision_point, autoware_planning_msgs::msg::Trajectory & output_trajectory) { // plus distance from self to next nearest point double dist = dist_to_collision_point; double dist_to_first_point = 0.0; - if (output_trajectory->points.size() > 1) { + if (output_trajectory.points.size() > 1) { dist_to_first_point = boost::geometry::distance( convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(output_trajectory->points.at(1).pose.position)); + convertPointRosToBoost(output_trajectory.points.at(1).pose.position)); } dist += dist_to_first_point; @@ -571,10 +503,10 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration); double pre_vel = current_vel; double total_dist = 0.0; - for (size_t i = 1; i < output_trajectory->points.size(); i++) { + for (size_t i = 1; i < output_trajectory.points.size(); i++) { // calc velocity of each point by gradient deceleration - const auto current_p = output_trajectory->points[i]; - const auto prev_p = output_trajectory->points[i - 1]; + const auto current_p = output_trajectory.points[i]; + const auto prev_p = output_trajectory.points[i - 1]; const double p_dist = getDistanceFromTwoPoint(current_p.pose.position, prev_p.pose.position); total_dist += p_dist; if (current_p.twist.linear.x > target_vel && total_dist >= 0) { @@ -593,8 +525,8 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( if (total_dist >= margin_to_insert) { const double max_velocity = std::max(target_vel, next_pre_vel); - if (output_trajectory->points[i].twist.linear.x > max_velocity) { - output_trajectory->points[i].twist.linear.x = max_velocity; + if (output_trajectory.points[i].twist.linear.x > max_velocity) { + output_trajectory.points[i].twist.linear.x = max_velocity; } } pre_vel = next_pre_vel; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index d480f417..d3f678fb 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -391,7 +391,7 @@ void ObstacleStopPlannerNode::pathCallback( if (is_collision) { acc_controller_->insertAdaptiveCruiseVelocity( decimate_trajectory, decimate_trajectory_collision_index, self_pose, nearest_collision_point, - nearest_collision_point_time, object_ptr_, current_velocity_ptr_, &need_to_stop, &output_msg); + nearest_collision_point_time, object_ptr_, current_velocity_ptr_, need_to_stop, output_msg); } /* From db5748a60371ed2110d3e67751fe4df612274816 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 1 Mar 2021 13:29:26 +0900 Subject: [PATCH 04/62] create vehicle class --- obstacle_stop_planner_refine/CMakeLists.txt | 1 + .../adaptive_cruise_control.hpp | 4 +- .../include/obstacle_stop_planner/node.hpp | 21 +- .../obstacle_point_cloud.hpp | 59 +++++ .../include/obstacle_stop_planner/util.hpp | 39 +++ .../include/obstacle_stop_planner/vehicle.hpp | 89 +++++++ .../src/adaptive_cruise_control.cpp | 24 +- obstacle_stop_planner_refine/src/node.cpp | 249 +++++------------- .../src/obstacle_point_cloud.cpp | 120 +++++++++ 9 files changed, 398 insertions(+), 208 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp create mode 100644 obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 57cf938c..43ac10c6 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -23,6 +23,7 @@ ament_auto_add_executable(obstacle_stop_planner_node src/node.cpp src/main.cpp src/adaptive_cruise_control.cpp + src/obstacle_point_cloud.cpp ) target_include_directories(obstacle_stop_planner_node diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index a5447068..9b6283f9 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -174,10 +174,10 @@ class AdaptiveCruiseController double getMedianVel(const std::vector vel_que); double lowpass_filter(const double current_value, const double prev_value, const double gain); - void calcDistanceToNearestPointOnPath( + double calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * distance); + const rclcpp::Time & nearest_collision_point_time); double calcTrajYaw( const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx); bool estimatePointVelocityFromObject( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index d2e280f3..182f8d7e 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -39,6 +39,8 @@ #include "opencv2/imgproc/imgproc.hpp" #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" +#include "obstacle_stop_planner/obstacle_point_cloud.hpp" +#include "obstacle_stop_planner/vehicle.hpp" namespace motion_planning { @@ -78,31 +80,18 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::shared_ptr debug_ptr_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + // ObstacleStopPlanner impl_; + ObstaclePointCloud obstacle_pointcloud_; + VehicleInfo vehicle_info_; /* * Parameter */ std::unique_ptr acc_controller_; - sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_; - double wheel_base_, front_overhang_, rear_overhang_, left_overhang_, right_overhang_, - vehicle_width_, vehicle_length_; - double stop_margin_; - double slow_down_margin_; - double min_behavior_stop_margin_; rclcpp::Time prev_col_point_time_; pcl::PointXYZ prev_col_point_; - double expand_slow_down_range_; - double expand_stop_range_; - double max_slow_down_vel_; - double min_slow_down_vel_; - double max_deceleration_; - bool enable_slow_down_; - double extend_distance_; - double step_length_; - double stop_search_radius_; - double slow_down_search_radius_; void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); void pathCallback(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg); void dynamicObjectCallback( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp new file mode 100644 index 00000000..39901735 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -0,0 +1,59 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ +#define OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ + +#include + +#include +#include +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_cloud.h" +#include "pcl/common/transforms.h" +#include "pcl/filters/voxel_grid.h" +#include "tf2_ros/transform_listener.h" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "obstacle_stop_planner/vehicle.hpp" + +namespace motion_planning { + +class ObstaclePointCloud +{ +public: + explicit ObstaclePointCloud(rclcpp::Logger logger); + + void SetPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void SetSearchRadius(const double value); + void SetVehicleInfo(const VehicleInfo vehicle_info); + + bool IsDataReceived(); + pcl::PointCloud::Ptr SearchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory); + +private: + bool searchPointcloudNearTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const pcl::PointCloud::Ptr input_pointcloud_ptr, + pcl::PointCloud::Ptr output_pointcloud_ptr); + + sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; + rclcpp::Logger logger_; + double search_radius_; + std::shared_ptr vehicle_info_; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 4522b1d8..cb41faf7 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -14,9 +14,16 @@ #ifndef OBSTACLE_STOP_PLANNER__UTIL_HPP_ #define OBSTACLE_STOP_PLANNER__UTIL_HPP_ +#include + #include "opencv2/core/core.hpp" #include "geometry_msgs/msg/point.hpp" #include "boost/geometry.hpp" +#include "boost/format.hpp" +#include "boost/assign/list_of.hpp" +#include "tf2/utils.h" +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_msgs/msg/key_value.hpp" namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; @@ -25,6 +32,38 @@ using Line = bg::model::linestring; namespace { +double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & quat) +{ + tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); + double roll, pitch, yaw; + tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw); + + return yaw; +} +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) +{ + diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; + diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; + stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stop_reason_diag.name = "stop_reason"; + stop_reason_diag.message = stop_reason; + stop_reason_diag_kv.key = "stop_pose"; + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag.values.push_back(stop_reason_diag_kv); + return stop_reason_diag; +} + cv::Point2d calcCentroid(const std::vector pointcloud) { cv::Point2d centroid; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp new file mode 100644 index 00000000..b349aa54 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -0,0 +1,89 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ +#define OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "vehicle_info_util/vehicle_info.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "obstacle_stop_planner/util.hpp" + +namespace motion_planning { + +class VehicleInfo : public vehicle_info_util::VehicleInfo +{ +public: + VehicleInfo(vehicle_info_util::VehicleInfo super, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter) + : vehicle_info_util::VehicleInfo(super) + { + // Parameters + stop_margin_ = parameter->declare_parameter("stop_planner.stop_margin", rclcpp::ParameterValue(5.0)).get(); + min_behavior_stop_margin_ = parameter->declare_parameter("stop_planner.min_behavior_stop_margin", rclcpp::ParameterValue(2.0)).get(); + step_length_ = parameter->declare_parameter("stop_planner.step_length", rclcpp::ParameterValue(1.0)).get(); + extend_distance_ = parameter->declare_parameter("stop_planner.extend_distance", rclcpp::ParameterValue(0.0)).get(); + expand_stop_range_ = parameter->declare_parameter("stop_planner.expand_stop_range", rclcpp::ParameterValue(0.0)).get(); + + slow_down_margin_ = parameter->declare_parameter("slow_down_planner.slow_down_margin", rclcpp::ParameterValue(5.0)).get(); + expand_slow_down_range_ = parameter->declare_parameter("slow_down_planner.expand_slow_down_range", rclcpp::ParameterValue(1.0)).get(); + max_slow_down_vel_ = parameter->declare_parameter("slow_down_planner.max_slow_down_vel", rclcpp::ParameterValue(4.0)).get(); + min_slow_down_vel_ = parameter->declare_parameter("slow_down_planner.min_slow_down_vel", rclcpp::ParameterValue(2.0)).get(); + max_deceleration_ = parameter->declare_parameter("slow_down_planner.max_deceleration", rclcpp::ParameterValue(2.0)).get(); + enable_slow_down_ = parameter->declare_parameter("enable_slow_down", rclcpp::ParameterValue(false)).get(); + + stop_margin_ += wheel_base_m_ + front_overhang_m_; + min_behavior_stop_margin_ += wheel_base_m_ + front_overhang_m_; + slow_down_margin_ += wheel_base_m_ + front_overhang_m_; + stop_search_radius_ = + step_length_ + std::hypot(vehicle_width_m_ / 2.0 + expand_stop_range_, vehicle_length_m_ / 2.0); + slow_down_search_radius_ = + step_length_ + + std::hypot(vehicle_width_m_ / 2.0 + expand_slow_down_range_, vehicle_length_m_ / 2.0); + } + + geometry_msgs::msg::Pose getVehicleCenterFromBase( + const geometry_msgs::msg::Pose & base_pose) + { + geometry_msgs::msg::Pose center_pose; + const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); + center_pose.position.x = + base_pose.position.x + (vehicle_length_m_ / 2.0 - rear_overhang_m_) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (vehicle_length_m_ / 2.0 - rear_overhang_m_) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; + } + + double stop_margin_; + double min_behavior_stop_margin_; + double step_length_; + double extend_distance_; + double expand_stop_range_; + double slow_down_margin_; + double expand_slow_down_range_; + double max_slow_down_vel_; + double min_slow_down_vel_; + double max_deceleration_; + bool enable_slow_down_; + double stop_search_radius_; + double slow_down_search_radius_; + +private: +}; + +} + +#endif // OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 997215e9..e2426750 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -114,15 +114,14 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( debug_values_.data.resize(num_debug_values_, 0.0); const double current_velocity = current_velocity_ptr->twist.linear.x; - double col_point_distance; double point_velocity; bool success_estm_vel = false; /* * calc distance to collision point */ - calcDistanceToNearestPointOnPath( + double col_point_distance = calcDistanceToNearestPointOnPath( trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point, - nearest_collision_point_time, &col_point_distance); + nearest_collision_point_time); /* * calc yaw of trajectory at collision point @@ -181,17 +180,17 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( need_to_stop = false; } -void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( +double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * distance) + const rclcpp::Time & nearest_collision_point_time) { + double distance; if (trajectory.points.size() == 0) { RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "input path is too short(size=0)"); - *distance = 0; - return; + return 0; } // get self polygon @@ -206,9 +205,9 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( if (nearest_point_idx <= 2) { // if too nearest collision point, return direct distance - *distance = boost::geometry::distance(self_poly, nearest_point2d); - debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; - return; + distance = boost::geometry::distance(self_poly, nearest_point2d); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = distance; + return distance; } /* get total distance to collision point */ @@ -239,8 +238,9 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( dist_to_point += prev_target_velocity_ * delay_time; } - *distance = std::max(0.0, dist_to_point); - debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance; + distance = std::max(0.0, dist_to_point); + debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = distance; + return distance; } double AdaptiveCruiseController::calcTrajYaw( diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index d3f678fb..eb84650b 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -33,45 +33,10 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" -#include "vehicle_info_util/vehicle_info.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" -namespace -{ -double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & quat) -{ - tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); - double roll, pitch, yaw; - tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw); - - return yaw; -} -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; - stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stop_reason_diag.name = "stop_reason"; - stop_reason_diag.message = stop_reason; - stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); - stop_reason_diag.values.push_back(stop_reason_diag_kv); - return stop_reason_diag; -} -} // namespace namespace motion_planning { @@ -81,46 +46,18 @@ using Polygon = bg::model::polygon; using Line = bg::model::linestring; ObstacleStopPlannerNode::ObstacleStopPlannerNode() -: Node("obstacle_stop_planner"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +: Node("obstacle_stop_planner") +, tf_buffer_(this->get_clock()) +, tf_listener_(tf_buffer_) +, obstacle_pointcloud_(this->get_logger()) +, vehicle_info_(vehicle_info_util::VehicleInfo::create(*this), this->get_node_parameters_interface()) { - // Vehicle Parameters - auto vehicle_info(vehicle_info_util::VehicleInfo::create(*this)); - - wheel_base_ = vehicle_info.wheel_base_m_; - front_overhang_ = vehicle_info.front_overhang_m_; - rear_overhang_ = vehicle_info.rear_overhang_m_; - left_overhang_ = vehicle_info.left_overhang_m_; - right_overhang_ = vehicle_info.right_overhang_m_; - vehicle_width_ = vehicle_info.vehicle_width_m_; - vehicle_length_ = vehicle_info.vehicle_length_m_; - - // Parameters - stop_margin_ = declare_parameter("stop_planner.stop_margin", 5.0); - min_behavior_stop_margin_ = declare_parameter("stop_planner.min_behavior_stop_margin", 2.0); - step_length_ = declare_parameter("stop_planner.step_length", 1.0); - extend_distance_ = declare_parameter("stop_planner.extend_distance", 0.0); - expand_stop_range_ = declare_parameter("stop_planner.expand_stop_range", 0.0); - - slow_down_margin_ = declare_parameter("slow_down_planner.slow_down_margin", 5.0); - expand_slow_down_range_ = declare_parameter("slow_down_planner.expand_slow_down_range", 1.0); - max_slow_down_vel_ = declare_parameter("slow_down_planner.max_slow_down_vel", 4.0); - min_slow_down_vel_ = declare_parameter("slow_down_planner.min_slow_down_vel", 2.0); - max_deceleration_ = declare_parameter("slow_down_planner.max_deceleration", 2.0); - enable_slow_down_ = declare_parameter("enable_slow_down", false); - - stop_margin_ += wheel_base_ + front_overhang_; - min_behavior_stop_margin_ += wheel_base_ + front_overhang_; - slow_down_margin_ += wheel_base_ + front_overhang_; - stop_search_radius_ = - step_length_ + std::hypot(vehicle_width_ / 2.0 + expand_stop_range_, vehicle_length_ / 2.0); - slow_down_search_radius_ = - step_length_ + - std::hypot(vehicle_width_ / 2.0 + expand_slow_down_range_, vehicle_length_ / 2.0); - debug_ptr_ = std::make_shared(this, wheel_base_ + front_overhang_); + obstacle_pointcloud_.SetVehicleInfo(vehicle_info_); + debug_ptr_ = std::make_shared(this, vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_); // Initializer acc_controller_ = std::make_unique( - this, vehicle_width_, vehicle_length_, wheel_base_, front_overhang_); + this, vehicle_info_.vehicle_width_m_, vehicle_info_.vehicle_length_m_, vehicle_info_.wheel_base_m_, vehicle_info_.front_overhang_m_); // Publishers path_pub_ = @@ -151,35 +88,19 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() void ObstacleStopPlannerNode::obstaclePointcloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { - obstacle_ros_pointcloud_ptr_ = std::make_shared(); - pcl::VoxelGrid filter; - pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( - new pcl::PointCloud); - - pcl::fromROSMsg(*input_msg, *pointcloud_ptr); - - for (const auto & point : pointcloud_ptr->points) { - no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); - } - filter.setInputCloud(no_height_pointcloud_ptr); - filter.setLeafSize(0.05f, 0.05f, 100000.0f); - filter.filter(*no_height_filtered_pointcloud_ptr); - pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); - obstacle_ros_pointcloud_ptr_->header = input_msg->header; + obstacle_pointcloud_.SetPointCloud(input_msg); } void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) { - if (!obstacle_ros_pointcloud_ptr_) { + if (!obstacle_pointcloud_.IsDataReceived()) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for obstacle pointcloud..."); return; } - if (!current_velocity_ptr_ && enable_slow_down_) { + if (!current_velocity_ptr_ && vehicle_info_.enable_slow_down_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for current velocity..."); @@ -190,7 +111,7 @@ void ObstacleStopPlannerNode::pathCallback( * extend trajectory to consider obstacles after the goal */ autoware_planning_msgs::msg::Trajectory extended_trajectory; - extendTrajectory(*input_msg, extend_distance_, extended_trajectory); + extendTrajectory(*input_msg, vehicle_info_.extend_distance_, extended_trajectory); const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; @@ -211,7 +132,7 @@ void ObstacleStopPlannerNode::pathCallback( autoware_planning_msgs::msg::Trajectory decimate_trajectory; std::map decimate_trajectory_index_map; decimateTrajectory( - trim_trajectory, step_length_, decimate_trajectory, decimate_trajectory_index_map); + trim_trajectory, vehicle_info_.step_length_, decimate_trajectory, decimate_trajectory_index_map); autoware_planning_msgs::msg::Trajectory & trajectory = decimate_trajectory; @@ -220,40 +141,12 @@ void ObstacleStopPlannerNode::pathCallback( */ pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( new pcl::PointCloud); - { - // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_.lookupTransform( - trajectory.header.frame_id, obstacle_ros_pointcloud_ptr_->header.frame_id, - obstacle_ros_pointcloud_ptr_->header.stamp, rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "[obstacle_stop_planner] Failed to look up transform from " << - trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr_->header.frame_id); - // do not publish path - return; - } - Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl::PointCloud::Ptr obstacle_pointcloud_pcl_ptr_( - new pcl::PointCloud); - pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_pointcloud_pcl_ptr_); - pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( - new pcl::PointCloud); - pcl::transformPointCloud( - *obstacle_pointcloud_pcl_ptr_, *transformed_obstacle_pointcloud_ptr, - affine_matrix); - - // search obstacle candidate pointcloud to reduce calculation cost - const double search_radius = enable_slow_down_ ? slow_down_search_radius_ : stop_search_radius_; - searchPointcloudNearTrajectory( - trajectory, search_radius, transformed_obstacle_pointcloud_ptr, - obstacle_candidate_pointcloud_ptr); - obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; - } + // search obstacle candidate pointcloud to reduce calculation cost + const double search_radius = vehicle_info_.enable_slow_down_ ? vehicle_info_.slow_down_search_radius_ : vehicle_info_.stop_search_radius_; + obstacle_pointcloud_.SetSearchRadius(search_radius); + + obstacle_pointcloud_.SearchCandidateObstacle(tf_buffer_, trajectory); /* * check collision, slow_down @@ -285,7 +178,7 @@ void ObstacleStopPlannerNode::pathCallback( std::vector one_step_move_vehicle_polygon; createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, one_step_move_vehicle_polygon, - expand_stop_range_); + vehicle_info_.expand_stop_range_); debug_ptr_->pushPolygon( one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); // convert boost polygon @@ -299,13 +192,13 @@ void ObstacleStopPlannerNode::pathCallback( std::vector one_step_move_slow_down_range_polygon; Polygon boost_one_step_move_slow_down_range_polygon; - if (enable_slow_down_) { + if (vehicle_info_.enable_slow_down_) { /* * create one step polygon for slow_down range */ createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - one_step_move_slow_down_range_polygon, expand_slow_down_range_); + one_step_move_slow_down_range_polygon, vehicle_info_.expand_slow_down_range_); debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); @@ -324,13 +217,13 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - if (!is_slow_down && enable_slow_down_) { + if (!is_slow_down && vehicle_info_.enable_slow_down_) { for (size_t j = 0; j < obstacle_candidate_pointcloud_ptr->size(); ++j) { Point point( obstacle_candidate_pointcloud_ptr->at(j).x, obstacle_candidate_pointcloud_ptr->at(j).y); if ( - bg::distance(prev_center_point, point) < slow_down_search_radius_ || - bg::distance(next_center_point, point) < slow_down_search_radius_) + bg::distance(prev_center_point, point) < vehicle_info_.slow_down_search_radius_ || + bg::distance(next_center_point, point) < vehicle_info_.slow_down_search_radius_) { if (bg::within(point, boost_one_step_move_slow_down_range_polygon)) { slow_down_pointcloud_ptr->push_back(obstacle_candidate_pointcloud_ptr->at(j)); @@ -344,8 +237,8 @@ void ObstacleStopPlannerNode::pathCallback( for (size_t j = 0; j < slow_down_pointcloud_ptr->size(); ++j) { Point point(slow_down_pointcloud_ptr->at(j).x, slow_down_pointcloud_ptr->at(j).y); if ( - bg::distance(prev_center_point, point) < stop_search_radius_ || - bg::distance(next_center_point, point) < stop_search_radius_) + bg::distance(prev_center_point, point) < vehicle_info_.stop_search_radius_ || + bg::distance(next_center_point, point) < vehicle_info_.stop_search_radius_) { if (bg::within(point, boost_one_step_move_vehicle_polygon)) { collision_pointcloud_ptr->push_back(slow_down_pointcloud_ptr->at(j)); @@ -449,7 +342,7 @@ void ObstacleStopPlannerNode::pathCallback( { const double slow_down_target_vel = calcSlowDownTargetVel(lateral_deviation); const auto slow_down_start_point = createSlowDownStartPoint( - i, slow_down_margin_, slow_down_target_vel, trajectory_vec, slow_down_point_vec, + i, vehicle_info_.slow_down_margin_, slow_down_target_vel, trajectory_vec, slow_down_point_vec, base_path); if (slow_down_start_point.index <= output_msg.points.size()) { @@ -470,9 +363,9 @@ void ObstacleStopPlannerNode::pathCallback( void ObstacleStopPlannerNode::externalExpandStopRangeCallback( const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg) { - expand_stop_range_ = input_msg->expand_stop_range; - stop_search_radius_ = - step_length_ + std::hypot(vehicle_width_ / 2.0 + expand_stop_range_, vehicle_length_ / 2.0); + vehicle_info_.expand_stop_range_ = input_msg->expand_stop_range; + vehicle_info_.stop_search_radius_ = + vehicle_info_.step_length_ + std::hypot(vehicle_info_.vehicle_width_m_ / 2.0 + vehicle_info_.expand_stop_range_, vehicle_info_.vehicle_length_m_ / 2.0); } void ObstacleStopPlannerNode::insertStopPoint( @@ -498,9 +391,9 @@ StopPoint ObstacleStopPlannerNode::searchInsertPoint( const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) { const auto max_dist_stop_point = - createTargetPoint(idx, stop_margin_, trajectory_vec, collision_point_vec, base_path); + createTargetPoint(idx, vehicle_info_.stop_margin_, trajectory_vec, collision_point_vec, base_path); const auto min_dist_stop_point = createTargetPoint( - idx, min_behavior_stop_margin_, trajectory_vec, collision_point_vec, base_path); + idx, vehicle_info_.stop_margin_, trajectory_vec, collision_point_vec, base_path); // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; @@ -584,7 +477,7 @@ SlowDownPoint ObstacleStopPlannerNode::createSlowDownStartPoint( } slow_down_point.velocity = std::max( - std::sqrt(slow_down_target_vel * slow_down_target_vel + 2 * max_deceleration_ * length_sum), + std::sqrt(slow_down_target_vel * slow_down_target_vel + 2 * vehicle_info_.max_deceleration_ * length_sum), current_velocity_ptr_->twist.linear.x); return slow_down_point; } @@ -626,7 +519,7 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( output_path.points.at(j).pose.position.y - output_path.points.at(j + 1).pose.position.y); slow_down_vel = std::max( slow_down_target_vel, - std::sqrt(std::max(slow_down_vel * slow_down_vel - 2 * max_deceleration_ * dist, 0.0))); + std::sqrt(std::max(slow_down_vel * slow_down_vel - 2 * vehicle_info_.max_deceleration_ * dist, 0.0))); if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { slow_down_end_trajectory_point = output_path.points.at(j + 1); is_slow_down_end = true; @@ -641,9 +534,9 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) { - return min_slow_down_vel_ + (max_slow_down_vel_ - min_slow_down_vel_) * - std::max(lateral_deviation - vehicle_width_ / 2, 0.0) / - expand_slow_down_range_; + return vehicle_info_.min_slow_down_vel_ + (vehicle_info_.max_slow_down_vel_ - vehicle_info_.min_slow_down_vel_) * + std::max(lateral_deviation - vehicle_info_.vehicle_width_m_ / 2, 0.0) / + vehicle_info_.expand_slow_down_range_; } void ObstacleStopPlannerNode::dynamicObjectCallback( @@ -813,56 +706,56 @@ void ObstacleStopPlannerNode::createOneStepPolygon( double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); + base_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - + std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), + base_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + + std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); + base_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - + std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), + base_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + + std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); + base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - + std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), + base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + + std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); + base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - + std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), + base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + + std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); } // next step { double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); + next_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - + std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), + next_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + + std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (wheel_base_ + front_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (wheel_base_ + front_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); + next_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - + std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), + next_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + + std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (-vehicle_width_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (-vehicle_width_ / 2.0 - expand_width))); + next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - + std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), + next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + + std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); one_step_move_vehicle_corner_points.push_back( cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-rear_overhang_) - - std::sin(yaw) * (vehicle_width_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (-rear_overhang_) + - std::cos(yaw) * (vehicle_width_ / 2.0 + expand_width))); + next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - + std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), + next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + + std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); } convexHull(one_step_move_vehicle_corner_points, polygon); } @@ -974,9 +867,9 @@ geometry_msgs::msg::Pose ObstacleStopPlannerNode::getVehicleCenterFromBase( geometry_msgs::msg::Pose center_pose; const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); center_pose.position.x = - base_pose.position.x + (vehicle_length_ / 2.0 - rear_overhang_) * std::cos(yaw); + base_pose.position.x + (vehicle_info_.vehicle_length_m_ / 2.0 - vehicle_info_.rear_overhang_m_) * std::cos(yaw); center_pose.position.y = - base_pose.position.y + (vehicle_length_ / 2.0 - rear_overhang_) * std::sin(yaw); + base_pose.position.y + (vehicle_info_.vehicle_length_m_ / 2.0 - vehicle_info_.rear_overhang_m_) * std::sin(yaw); center_pose.position.z = base_pose.position.z; center_pose.orientation = base_pose.orientation; return center_pose; diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp new file mode 100644 index 00000000..55cffcad --- /dev/null +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -0,0 +1,120 @@ +// Copyright 2021 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 "obstacle_stop_planner/obstacle_point_cloud.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2/utils.h" +#include "tf2_eigen/tf2_eigen.h" + +namespace motion_planning +{ +ObstaclePointCloud::ObstaclePointCloud(rclcpp::Logger logger) +: logger_(logger) +{ +} + +void ObstaclePointCloud::SetPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::VoxelGrid filter; + pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( + new pcl::PointCloud); + + pcl::fromROSMsg(*msg, *pointcloud_ptr); + + for (const auto & point : pointcloud_ptr->points) { + no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); + } + filter.setInputCloud(no_height_pointcloud_ptr); + filter.setLeafSize(0.05f, 0.05f, 100000.0f); + filter.filter(*no_height_filtered_pointcloud_ptr); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = msg->header; +} + +void ObstaclePointCloud::SetSearchRadius(const double value) +{ + search_radius_ = value; +} + +void ObstaclePointCloud::SetVehicleInfo(const VehicleInfo vehicle_info) +{ + vehicle_info_ = std::make_shared(vehicle_info); +} + +bool ObstaclePointCloud::IsDataReceived() +{ + return obstacle_ros_pointcloud_ptr_ != nullptr ? true : false; +} + +pcl::PointCloud::Ptr ObstaclePointCloud::SearchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory) +{ + pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( + new pcl::PointCloud); + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer.lookupTransform( + trajectory.header.frame_id, obstacle_ros_pointcloud_ptr_->header.frame_id, + obstacle_ros_pointcloud_ptr_->header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + logger_, + "[obstacle_stop_planner] Failed to look up transform from " << + trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr_->header.frame_id); + // do not publish path + return nullptr; + } + + Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl::PointCloud::Ptr obstacle_pointcloud_pcl_ptr_( + new pcl::PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_pointcloud_pcl_ptr_); + pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( + new pcl::PointCloud); + pcl::transformPointCloud( + *obstacle_pointcloud_pcl_ptr_, *transformed_obstacle_pointcloud_ptr, + affine_matrix); + + // search obstacle candidate pointcloud to reduce calculation cost + searchPointcloudNearTrajectory( + trajectory, transformed_obstacle_pointcloud_ptr, + obstacle_candidate_pointcloud_ptr); + obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; + return obstacle_candidate_pointcloud_ptr; +} + +bool ObstaclePointCloud::searchPointcloudNearTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const pcl::PointCloud::Ptr input_pointcloud_ptr, + pcl::PointCloud::Ptr output_pointcloud_ptr) +{ + const double squared_radius = search_radius_ * search_radius_; + for (const auto & trajectory_point : trajectory.points) { + const auto center_pose = vehicle_info_->getVehicleCenterFromBase(trajectory_point.pose); + for (const auto & point : input_pointcloud_ptr->points) { + const double x = center_pose.position.x - point.x; + const double y = center_pose.position.y - point.y; + const double squared_distance = x * x + y * y; + if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} + } + } + return true; +} + +} // namespace motion_planning From 17cc672e8948afdbb7693785b4cfc629625d1b37 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 1 Mar 2021 16:38:15 +0900 Subject: [PATCH 05/62] create trajectory, point_helper class --- obstacle_stop_planner_refine/CMakeLists.txt | 12 + .../include/obstacle_stop_planner/node.hpp | 71 +--- .../obstacle_stop_planner/point_helper.hpp | 94 +++++ .../obstacle_stop_planner/trajectory.hpp | 50 +++ .../include/obstacle_stop_planner/vehicle.hpp | 2 +- obstacle_stop_planner_refine/src/node.cpp | 343 +----------------- .../src/point_helper.cpp | 211 +++++++++++ .../src/trajectory.cpp | 106 ++++++ 8 files changed, 496 insertions(+), 393 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp create mode 100644 obstacle_stop_planner_refine/src/point_helper.cpp create mode 100644 obstacle_stop_planner_refine/src/trajectory.cpp diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 43ac10c6..517911de 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -24,6 +24,8 @@ ament_auto_add_executable(obstacle_stop_planner_node src/main.cpp src/adaptive_cruise_control.cpp src/obstacle_point_cloud.cpp + src/point_helper.cpp + src/trajectory.cpp ) target_include_directories(obstacle_stop_planner_node @@ -40,6 +42,16 @@ target_link_libraries(obstacle_stop_planner_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + # find_package(ament_cmake_gtest REQUIRED) + + # file(GLOB_RECURSE test_files test/**/*.cpp) + + # ament_add_gtest(test_obstacle_stop_planner test/test_obstacle_stop_planner.test ${test_files}) + + # target_link_libraries(test_obstacle_stop_planner + # obstacle_stop_planner_node + # ) endif() ament_auto_package( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 182f8d7e..a0557c40 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -41,22 +41,10 @@ #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/obstacle_point_cloud.hpp" #include "obstacle_stop_planner/vehicle.hpp" +#include "obstacle_stop_planner/trajectory.hpp" namespace motion_planning { -struct StopPoint -{ - size_t index; - Eigen::Vector2d point; -}; - -struct SlowDownPoint -{ - size_t index; - Eigen::Vector2d point; - double velocity; -}; - class ObstacleStopPlannerNode : public rclcpp::Node { public: @@ -83,6 +71,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node // ObstacleStopPlanner impl_; ObstaclePointCloud obstacle_pointcloud_; VehicleInfo vehicle_info_; + Trajectory trajectory_; /* * Parameter @@ -103,68 +92,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node private: bool convexHull( const std::vector pointcloud, std::vector & polygon_points); - bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - std::map & index_map); - bool trimTrajectoryFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - bool trimTrajectoryWithIndexFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - size_t & index); - bool searchPointcloudNearTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius, - const pcl::PointCloud::Ptr input_pointcloud_ptr, - pcl::PointCloud::Ptr output_pointcloud_ptr); void createOneStepPolygon( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, std::vector & polygon, const double expand_width = 0.0); bool getSelfPose( const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, geometry_msgs::msg::Pose & self_pose); - bool getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, - Eigen::Vector2d & output_point); - void getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); - void getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation); - geometry_msgs::msg::Pose getVehicleCenterFromBase(const geometry_msgs::msg::Pose & base_pose); - - void insertStopPoint( - const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag); - - StopPoint searchInsertPoint( - const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec); - - StopPoint createTargetPoint( - const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, - const Eigen::Vector2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path); - - SlowDownPoint createSlowDownStartPoint( - const int idx, const double margin, const double slow_down_target_vel, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path); - - void insertSlowDownStartPoint( - const SlowDownPoint & slow_down_start_point, - const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path); - void insertSlowDownVelocity( const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp new file mode 100644 index 00000000..1cb62038 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -0,0 +1,94 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ +#define OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "geometry_msgs/msg/pose.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/vehicle.hpp" + +#define EIGEN_MPL2_ONLY +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" + + +namespace motion_planning { + +struct StopPoint +{ + size_t index; + Eigen::Vector2d point; +}; + +struct SlowDownPoint +{ + size_t index; + Eigen::Vector2d point; + double velocity; +}; + +class PointHelper +{ +public: + void SetVehicleInfo(const VehicleInfo vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } + void SetVehicleInfo(std::shared_ptr vehicle_info) { vehicle_info_ = vehicle_info; } + + bool getBackwardPointFromBasePoint( + const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, + const Eigen::Vector2d & base_point, const double backward_length, + Eigen::Vector2d & output_point); + void getNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); + void getLateralNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * lateral_nearest_point, double * deviation); + + autoware_planning_msgs::msg::TrajectoryPoint insertStopPoint( + const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, + autoware_planning_msgs::msg::Trajectory & output_path); + + StopPoint searchInsertPoint( + const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, + const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec); + + StopPoint createTargetPoint( + const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, + const Eigen::Vector2d & collision_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path); + + SlowDownPoint createSlowDownStartPoint( + const int idx, const double margin, const double slow_down_target_vel, + const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path, + const double current_velocity_x); + + autoware_planning_msgs::msg::TrajectoryPoint insertSlowDownStartPoint( + const SlowDownPoint & slow_down_start_point, + const autoware_planning_msgs::msg::Trajectory & base_path, + autoware_planning_msgs::msg::Trajectory & output_path); + +private: + std::shared_ptr vehicle_info_; +}; +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp new file mode 100644 index 00000000..dd63f32f --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ +#define OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ + +#include +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "obstacle_stop_planner/vehicle.hpp" +#include "obstacle_stop_planner/point_helper.hpp" + +namespace motion_planning { + +class Trajectory +{ +public: + bool decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + autoware_planning_msgs::msg::Trajectory & output_trajectory); + bool decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + autoware_planning_msgs::msg::Trajectory & output_trajectory, + std::map & index_map); + bool trimTrajectoryFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose self_pose, + autoware_planning_msgs::msg::Trajectory & output_trajectory); + bool trimTrajectoryWithIndexFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose self_pose, + autoware_planning_msgs::msg::Trajectory & output_trajectory, + size_t & index); +}; +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp index b349aa54..f812cb6f 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -84,6 +84,6 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo private: }; -} +} // namespace motion_planning #endif // OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index eb84650b..ce4be13b 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -33,6 +33,7 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/point_helper.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" @@ -124,15 +125,15 @@ void ObstacleStopPlannerNode::pathCallback( getSelfPose(input_msg->header, tf_buffer_, self_pose); autoware_planning_msgs::msg::Trajectory trim_trajectory; size_t trajectory_trim_index; - trimTrajectoryWithIndexFromSelfPose(base_path, self_pose, trim_trajectory, trajectory_trim_index); + trajectory_.trimTrajectoryWithIndexFromSelfPose(base_path, self_pose, trim_trajectory, trajectory_trim_index); /* * decimate trajectory for calculation cost */ autoware_planning_msgs::msg::Trajectory decimate_trajectory; std::map decimate_trajectory_index_map; - decimateTrajectory( - trim_trajectory, vehicle_info_.step_length_, decimate_trajectory, decimate_trajectory_index_map); + trajectory_.decimateTrajectory( + trim_trajectory, vehicle_info_.step_length_, vehicle_info_, decimate_trajectory, decimate_trajectory_index_map); autoware_planning_msgs::msg::Trajectory & trajectory = decimate_trajectory; @@ -164,13 +165,16 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointXYZ lateral_nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; + PointHelper point_helper; + point_helper.SetVehicleInfo(vehicle_info_); + for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { /* * create one step circle center for vehicle */ - const auto prev_center_pose = getVehicleCenterFromBase(trajectory.points.at(i).pose); + const auto prev_center_pose = vehicle_info_.getVehicleCenterFromBase(trajectory.points.at(i).pose); Point prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = getVehicleCenterFromBase(trajectory.points.at(i + 1).pose); + const auto next_center_pose = vehicle_info_.getVehicleCenterFromBase(trajectory.points.at(i + 1).pose); Point next_center_point(next_center_pose.position.x, next_center_pose.position.y); /* * create one step polygon for vehicle @@ -255,10 +259,10 @@ void ObstacleStopPlannerNode::pathCallback( debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); - getNearestPoint( + point_helper.getNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_slow_down_point, &nearest_collision_point_time); - getLateralNearestPoint( + point_helper.getLateralNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &lateral_nearest_slow_down_point, &lateral_deviation); debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); @@ -268,7 +272,7 @@ void ObstacleStopPlannerNode::pathCallback( * search nearest collision point by beginning of path */ if (is_collision) { - getNearestPoint( + point_helper.getNearestPoint( *collision_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_collision_point, &nearest_collision_point_time); debug_ptr_->pushObstaclePoint(nearest_collision_point, PointType::Stop); @@ -310,9 +314,11 @@ void ObstacleStopPlannerNode::pathCallback( (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) { const auto stop_point = - searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); + point_helper.searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); if (stop_point.index <= output_msg.points.size()) { - insertStopPoint(stop_point, base_path, output_msg, stop_reason_diag); + const auto trajectory_point = point_helper.insertStopPoint(stop_point, base_path, output_msg); + stop_reason_diag = makeStopReasonDiag("obstacle", trajectory_point.pose); + debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); } break; } @@ -341,12 +347,13 @@ void ObstacleStopPlannerNode::pathCallback( (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) { const double slow_down_target_vel = calcSlowDownTargetVel(lateral_deviation); - const auto slow_down_start_point = createSlowDownStartPoint( + const auto slow_down_start_point = point_helper.createSlowDownStartPoint( i, vehicle_info_.slow_down_margin_, slow_down_target_vel, trajectory_vec, slow_down_point_vec, - base_path); + base_path, current_velocity_ptr_->twist.linear.x); if (slow_down_start_point.index <= output_msg.points.size()) { - insertSlowDownStartPoint(slow_down_start_point, base_path, output_msg); + const auto slowdown_trajectory_point = point_helper.insertSlowDownStartPoint(slow_down_start_point, base_path, output_msg); + debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); insertSlowDownVelocity( slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, output_msg); @@ -368,142 +375,6 @@ void ObstacleStopPlannerNode::externalExpandStopRangeCallback( vehicle_info_.step_length_ + std::hypot(vehicle_info_.vehicle_width_m_ / 2.0 + vehicle_info_.expand_stop_range_, vehicle_info_.vehicle_length_m_ / 2.0); } -void ObstacleStopPlannerNode::insertStopPoint( - const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag) -{ - autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = - base_path.points.at(std::max(static_cast(stop_point.index) - 1, 0)); - stop_trajectory_point.pose.position.x = stop_point.point.x(); - stop_trajectory_point.pose.position.y = stop_point.point.y(); - stop_trajectory_point.twist.linear.x = 0.0; - output_path.points.insert(output_path.points.begin() + stop_point.index, stop_trajectory_point); - for (size_t j = stop_point.index; j < output_path.points.size(); ++j) { - output_path.points.at(j).twist.linear.x = 0.0; - } - stop_reason_diag = makeStopReasonDiag("obstacle", stop_trajectory_point.pose); - debug_ptr_->pushPose(stop_trajectory_point.pose, PoseType::Stop); -} - -StopPoint ObstacleStopPlannerNode::searchInsertPoint( - const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) -{ - const auto max_dist_stop_point = - createTargetPoint(idx, vehicle_info_.stop_margin_, trajectory_vec, collision_point_vec, base_path); - const auto min_dist_stop_point = createTargetPoint( - idx, vehicle_info_.stop_margin_, trajectory_vec, collision_point_vec, base_path); - - // check if stop point is already inserted by behavior planner - bool is_inserted_already_stop_point = false; - for (int j = max_dist_stop_point.index - 1; j < static_cast(idx); ++j) { - if (base_path.points.at(std::max(j, 0)).twist.linear.x == 0.0) { - is_inserted_already_stop_point = true; - break; - } - } - // insert stop point - StopPoint stop_point; - stop_point.index = - !is_inserted_already_stop_point ? max_dist_stop_point.index : min_dist_stop_point.index; - stop_point.point = - !is_inserted_already_stop_point ? max_dist_stop_point.point : min_dist_stop_point.point; - return stop_point; -} - -StopPoint ObstacleStopPlannerNode::createTargetPoint( - const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, - const Eigen::Vector2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) -{ - double length_sum = 0.0; - length_sum += trajectory_vec.normalized().dot(collision_point_vec); - Eigen::Vector2d line_start_point, line_end_point; - { - line_start_point << base_path.points.at(0).pose.position.x, - base_path.points.at(0).pose.position.y; - const double yaw = getYawFromGeometryMsgsQuaternion(base_path.points.at(0).pose.orientation); - line_end_point << std::cos(yaw), std::sin(yaw); - } - - StopPoint stop_point{0, Eigen::Vector2d()}; - for (size_t j = idx; 0 < j; --j) { - line_start_point << base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y; - line_end_point << base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y; - if (margin < length_sum) { - stop_point.index = j; - break; - } - length_sum += (line_end_point - line_start_point).norm(); - } - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_start_point, length_sum - margin, stop_point.point); - - return stop_point; -} - -SlowDownPoint ObstacleStopPlannerNode::createSlowDownStartPoint( - const int idx, const double margin, const double slow_down_target_vel, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) -{ - double length_sum = 0.0; - length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); - Eigen::Vector2d line_start_point{}; - Eigen::Vector2d line_end_point{}; - SlowDownPoint slow_down_point{0, Eigen::Vector2d(), 0.0}; - for (size_t j = idx; 0 < j; --j) { - line_start_point << base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y; - line_end_point << base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y; - if (margin < length_sum) { - slow_down_point.index = j; - break; - } - length_sum += (line_end_point - line_start_point).norm(); - } - const double backward_length = length_sum - margin; - if (backward_length < 0) { - slow_down_point.index = 0; - slow_down_point.point = Eigen::Vector2d( - base_path.points.at(0).pose.position.x, base_path.points.at(0).pose.position.y); - } else { - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_start_point, backward_length, slow_down_point.point); - } - - slow_down_point.velocity = std::max( - std::sqrt(slow_down_target_vel * slow_down_target_vel + 2 * vehicle_info_.max_deceleration_ * length_sum), - current_velocity_ptr_->twist.linear.x); - return slow_down_point; -} - -void ObstacleStopPlannerNode::insertSlowDownStartPoint( - const SlowDownPoint & slow_down_start_point, - const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) -{ - autoware_planning_msgs::msg::TrajectoryPoint slow_down_start_trajectory_point = - base_path.points.at(std::max(static_cast(slow_down_start_point.index) - 1, 0)); - slow_down_start_trajectory_point.pose.position.x = slow_down_start_point.point.x(); - slow_down_start_trajectory_point.pose.position.y = slow_down_start_point.point.y(); - slow_down_start_trajectory_point.twist.linear.x = slow_down_start_point.velocity; - constexpr double epsilon = 0.001; - const auto & insert_target_point = output_path.points.at(slow_down_start_point.index); - if ( - autoware_utils::calcDistance2d(slow_down_start_trajectory_point, insert_target_point) > - epsilon) - { - output_path.points.insert( - output_path.points.begin() + slow_down_start_point.index, slow_down_start_trajectory_point); - } - debug_ptr_->pushPose(slow_down_start_trajectory_point.pose, PoseType::SlowDownStart); -} - void ObstacleStopPlannerNode::insertSlowDownVelocity( const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path) @@ -551,14 +422,6 @@ void ObstacleStopPlannerNode::currentVelocityCallback( current_velocity_ptr_ = input_msg; } -bool ObstacleStopPlannerNode::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - std::map index_map; - return decimateTrajectory(input_trajectory, step_length, output_trajectory, index_map); -} - autoware_planning_msgs::msg::TrajectoryPoint ObstacleStopPlannerNode::getExtendTrajectoryPoint( const double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) { @@ -600,102 +463,6 @@ bool ObstacleStopPlannerNode::extendTrajectory( return true; } -bool ObstacleStopPlannerNode::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - std::map & index_map) -{ - output_trajectory.header = input_trajectory.header; - double trajectory_length_sum = 0.0; - double next_length = 0.0; - const double epsilon = 0.001; - Eigen::Vector2d point1, point2; - - for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { - if (next_length <= trajectory_length_sum + epsilon) { - Eigen::Vector2d line_start_point, line_end_point, interpolated_point; - line_start_point << input_trajectory.points.at(i).pose.position.x, - input_trajectory.points.at(i).pose.position.y; - line_end_point << input_trajectory.points.at(i + 1).pose.position.x, - input_trajectory.points.at(i + 1).pose.position.y; - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_end_point, - -1.0 * (trajectory_length_sum - next_length), interpolated_point); - autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; - trajectory_point = input_trajectory.points.at(i); - trajectory_point.pose.position.x = interpolated_point.x(); - trajectory_point.pose.position.y = interpolated_point.y(); - output_trajectory.points.push_back(trajectory_point); - index_map.insert(std::make_pair(output_trajectory.points.size() - 1, size_t(i))); - next_length += step_length; - continue; - } - trajectory_length_sum += std::hypot( - input_trajectory.points.at(i).pose.position.x - input_trajectory.points.at(i + 1).pose.position.x, - input_trajectory.points.at(i).pose.position.y - input_trajectory.points.at(i + 1).pose.position.y); - } - if (!input_trajectory.points.empty()) { - output_trajectory.points.push_back(input_trajectory.points.back()); - index_map.insert( - std::make_pair(output_trajectory.points.size() - 1, input_trajectory.points.size() - 1)); - } - return true; -} - -bool ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index) -{ - double min_distance = 0.0; - size_t min_distance_index = 0; - bool is_init = false; - for (size_t i = 0; i < input_trajectory.points.size(); ++i) { - const double distance = std::hypot( - input_trajectory.points.at(i).pose.position.x - self_pose.position.x, - input_trajectory.points.at(i).pose.position.y - self_pose.position.y); - if (!is_init || distance < min_distance) { - is_init = true; - min_distance = distance; - min_distance_index = i; - } - } - for (size_t i = min_distance_index; i < input_trajectory.points.size(); ++i) { - output_trajectory.points.push_back(input_trajectory.points.at(i)); - } - output_trajectory.header = input_trajectory.header; - index = min_distance_index; - return true; -} - -bool ObstacleStopPlannerNode::trimTrajectoryFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - size_t index; - return trimTrajectoryWithIndexFromSelfPose( - output_trajectory, self_pose, output_trajectory, index); -} - -bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius, - const pcl::PointCloud::Ptr input_pointcloud_ptr, - pcl::PointCloud::Ptr output_pointcloud_ptr) -{ - const double squared_radius = radius * radius; - for (const auto & trajectory_point : trajectory.points) { - const auto center_pose = getVehicleCenterFromBase(trajectory_point.pose); - for (const auto & point : input_pointcloud_ptr->points) { - const double x = center_pose.position.x - point.x; - const double y = center_pose.position.y - point.y; - const double squared_distance = x * x + y * y; - if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} - } - } - return true; -} - void ObstacleStopPlannerNode::createOneStepPolygon( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, std::vector & polygon, const double expand_width) @@ -805,74 +572,4 @@ bool ObstacleStopPlannerNode::getSelfPose( return false; } } -bool ObstacleStopPlannerNode::getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) -{ - Eigen::Vector2d line_vec = line_point2 - line_point1; - Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); - output_point = base_point + backward_vec; - return true; -} - -void ObstacleStopPlannerNode::getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) -{ - double min_norm = 0.0; - bool is_init = false; - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); - Eigen::Vector2d base_pose_vec; - base_pose_vec << std::cos(yaw), std::sin(yaw); - - for (size_t i = 0; i < pointcloud.size(); ++i) { - Eigen::Vector2d pointcloud_vec; - pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y; - double norm = base_pose_vec.dot(pointcloud_vec); - if (norm < min_norm || !is_init) { - min_norm = norm; - *nearest_collision_point = pointcloud.at(i); - *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; - is_init = true; - } - } -} - -void ObstacleStopPlannerNode::getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation) -{ - double min_norm = std::numeric_limits::max(); - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); - Eigen::Vector2d base_pose_vec; - base_pose_vec << std::cos(yaw), std::sin(yaw); - for (size_t i = 0; i < pointcloud.size(); ++i) { - Eigen::Vector2d pointcloud_vec; - pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y; - double norm = - std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); - if (norm < min_norm) { - min_norm = norm; - *lateral_nearest_point = pointcloud.at(i); - } - } - *deviation = min_norm; -} - -geometry_msgs::msg::Pose ObstacleStopPlannerNode::getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose) -{ - geometry_msgs::msg::Pose center_pose; - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); - center_pose.position.x = - base_pose.position.x + (vehicle_info_.vehicle_length_m_ / 2.0 - vehicle_info_.rear_overhang_m_) * std::cos(yaw); - center_pose.position.y = - base_pose.position.y + (vehicle_info_.vehicle_length_m_ / 2.0 - vehicle_info_.rear_overhang_m_) * std::sin(yaw); - center_pose.position.z = base_pose.position.z; - center_pose.orientation = base_pose.orientation; - return center_pose; -} - } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp new file mode 100644 index 00000000..83f0d695 --- /dev/null +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -0,0 +1,211 @@ +// Copyright 2021 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 "obstacle_stop_planner/point_helper.hpp" +#include "pcl_conversions/pcl_conversions.h" + +namespace motion_planning +{ +bool PointHelper::getBackwardPointFromBasePoint( + const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, + const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) +{ + Eigen::Vector2d line_vec = line_point2 - line_point1; + Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); + output_point = base_point + backward_vec; + return true; +} + +void PointHelper::getNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) +{ + double min_norm = 0.0; + bool is_init = false; + const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); + Eigen::Vector2d base_pose_vec; + base_pose_vec << std::cos(yaw), std::sin(yaw); + + for (size_t i = 0; i < pointcloud.size(); ++i) { + Eigen::Vector2d pointcloud_vec; + pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, + pointcloud.at(i).y - base_pose.position.y; + double norm = base_pose_vec.dot(pointcloud_vec); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = pointcloud.at(i); + *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; + is_init = true; + } + } +} + +void PointHelper::getLateralNearestPoint( + const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, + pcl::PointXYZ * lateral_nearest_point, double * deviation) +{ + double min_norm = std::numeric_limits::max(); + const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); + Eigen::Vector2d base_pose_vec; + base_pose_vec << std::cos(yaw), std::sin(yaw); + for (size_t i = 0; i < pointcloud.size(); ++i) { + Eigen::Vector2d pointcloud_vec; + pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, + pointcloud.at(i).y - base_pose.position.y; + double norm = + std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); + if (norm < min_norm) { + min_norm = norm; + *lateral_nearest_point = pointcloud.at(i); + } + } + *deviation = min_norm; +} + +autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( + const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, + autoware_planning_msgs::msg::Trajectory & output_path) +{ + autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = + base_path.points.at(std::max(static_cast(stop_point.index) - 1, 0)); + stop_trajectory_point.pose.position.x = stop_point.point.x(); + stop_trajectory_point.pose.position.y = stop_point.point.y(); + stop_trajectory_point.twist.linear.x = 0.0; + output_path.points.insert(output_path.points.begin() + stop_point.index, stop_trajectory_point); + for (size_t j = stop_point.index; j < output_path.points.size(); ++j) { + output_path.points.at(j).twist.linear.x = 0.0; + } + return stop_trajectory_point; +} + +StopPoint PointHelper::searchInsertPoint( + const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, + const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) +{ + const auto max_dist_stop_point = + createTargetPoint(idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, base_path); + const auto min_dist_stop_point = createTargetPoint( + idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, base_path); + + // check if stop point is already inserted by behavior planner + bool is_inserted_already_stop_point = false; + for (int j = max_dist_stop_point.index - 1; j < static_cast(idx); ++j) { + if (base_path.points.at(std::max(j, 0)).twist.linear.x == 0.0) { + is_inserted_already_stop_point = true; + break; + } + } + // insert stop point + StopPoint stop_point; + stop_point.index = + !is_inserted_already_stop_point ? max_dist_stop_point.index : min_dist_stop_point.index; + stop_point.point = + !is_inserted_already_stop_point ? max_dist_stop_point.point : min_dist_stop_point.point; + return stop_point; +} + +StopPoint PointHelper::createTargetPoint( + const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, + const Eigen::Vector2d & collision_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path) +{ + double length_sum = 0.0; + length_sum += trajectory_vec.normalized().dot(collision_point_vec); + Eigen::Vector2d line_start_point, line_end_point; + { + line_start_point << base_path.points.at(0).pose.position.x, + base_path.points.at(0).pose.position.y; + const double yaw = getYawFromGeometryMsgsQuaternion(base_path.points.at(0).pose.orientation); + line_end_point << std::cos(yaw), std::sin(yaw); + } + + StopPoint stop_point{0, Eigen::Vector2d()}; + for (size_t j = idx; 0 < j; --j) { + line_start_point << base_path.points.at(j - 1).pose.position.x, + base_path.points.at(j - 1).pose.position.y; + line_end_point << base_path.points.at(j).pose.position.x, + base_path.points.at(j).pose.position.y; + if (margin < length_sum) { + stop_point.index = j; + break; + } + length_sum += (line_end_point - line_start_point).norm(); + } + getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_start_point, length_sum - margin, stop_point.point); + + return stop_point; +} + +SlowDownPoint PointHelper::createSlowDownStartPoint( + const int idx, const double margin, const double slow_down_target_vel, + const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, + const autoware_planning_msgs::msg::Trajectory & base_path, + const double current_velocity_x) +{ + double length_sum = 0.0; + length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); + Eigen::Vector2d line_start_point{}; + Eigen::Vector2d line_end_point{}; + + SlowDownPoint slow_down_point{0, Eigen::Vector2d(), 0.0}; + for (size_t j = idx; 0 < j; --j) { + line_start_point << base_path.points.at(j).pose.position.x, + base_path.points.at(j).pose.position.y; + line_end_point << base_path.points.at(j - 1).pose.position.x, + base_path.points.at(j - 1).pose.position.y; + if (margin < length_sum) { + slow_down_point.index = j; + break; + } + length_sum += (line_end_point - line_start_point).norm(); + } + const double backward_length = length_sum - margin; + if (backward_length < 0) { + slow_down_point.index = 0; + slow_down_point.point = Eigen::Vector2d( + base_path.points.at(0).pose.position.x, base_path.points.at(0).pose.position.y); + } else { + getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_start_point, backward_length, slow_down_point.point); + } + + slow_down_point.velocity = std::max( + std::sqrt(slow_down_target_vel * slow_down_target_vel + 2 * vehicle_info_->max_deceleration_ * length_sum), + current_velocity_x); + return slow_down_point; +} + +autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertSlowDownStartPoint( + const SlowDownPoint & slow_down_start_point, + const autoware_planning_msgs::msg::Trajectory & base_path, + autoware_planning_msgs::msg::Trajectory & output_path) +{ + autoware_planning_msgs::msg::TrajectoryPoint slow_down_start_trajectory_point = + base_path.points.at(std::max(static_cast(slow_down_start_point.index) - 1, 0)); + slow_down_start_trajectory_point.pose.position.x = slow_down_start_point.point.x(); + slow_down_start_trajectory_point.pose.position.y = slow_down_start_point.point.y(); + slow_down_start_trajectory_point.twist.linear.x = slow_down_start_point.velocity; + constexpr double epsilon = 0.001; + const auto & insert_target_point = output_path.points.at(slow_down_start_point.index); + if ( + autoware_utils::calcDistance2d(slow_down_start_trajectory_point, insert_target_point) > + epsilon) + { + output_path.points.insert( + output_path.points.begin() + slow_down_start_point.index, slow_down_start_trajectory_point); + } + return slow_down_start_trajectory_point; +} +} // namespace motion_planning diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp new file mode 100644 index 00000000..ceb41ee2 --- /dev/null +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -0,0 +1,106 @@ +// Copyright 2021 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 "obstacle_stop_planner/trajectory.hpp" + +namespace motion_planning +{ +bool Trajectory::decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + autoware_planning_msgs::msg::Trajectory & output_trajectory) +{ + std::map index_map; + return decimateTrajectory(input_trajectory, step_length, vehicle_info, output_trajectory, index_map); +} + +bool Trajectory::decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + autoware_planning_msgs::msg::Trajectory & output_trajectory, + std::map & index_map) +{ + output_trajectory.header = input_trajectory.header; + double trajectory_length_sum = 0.0; + double next_length = 0.0; + const double epsilon = 0.001; + Eigen::Vector2d point1, point2; + PointHelper point_helper; + point_helper.SetVehicleInfo(vehicle_info); + + for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { + if (next_length <= trajectory_length_sum + epsilon) { + Eigen::Vector2d line_start_point, line_end_point, interpolated_point; + line_start_point << input_trajectory.points.at(i).pose.position.x, + input_trajectory.points.at(i).pose.position.y; + line_end_point << input_trajectory.points.at(i + 1).pose.position.x, + input_trajectory.points.at(i + 1).pose.position.y; + point_helper.getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_end_point, + -1.0 * (trajectory_length_sum - next_length), interpolated_point); + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; + trajectory_point = input_trajectory.points.at(i); + trajectory_point.pose.position.x = interpolated_point.x(); + trajectory_point.pose.position.y = interpolated_point.y(); + output_trajectory.points.push_back(trajectory_point); + index_map.insert(std::make_pair(output_trajectory.points.size() - 1, size_t(i))); + next_length += step_length; + continue; + } + trajectory_length_sum += std::hypot( + input_trajectory.points.at(i).pose.position.x - input_trajectory.points.at(i + 1).pose.position.x, + input_trajectory.points.at(i).pose.position.y - input_trajectory.points.at(i + 1).pose.position.y); + } + if (!input_trajectory.points.empty()) { + output_trajectory.points.push_back(input_trajectory.points.back()); + index_map.insert( + std::make_pair(output_trajectory.points.size() - 1, input_trajectory.points.size() - 1)); + } + return true; +} + +bool Trajectory::trimTrajectoryWithIndexFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose self_pose, + autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index) +{ + double min_distance = 0.0; + size_t min_distance_index = 0; + bool is_init = false; + for (size_t i = 0; i < input_trajectory.points.size(); ++i) { + const double distance = std::hypot( + input_trajectory.points.at(i).pose.position.x - self_pose.position.x, + input_trajectory.points.at(i).pose.position.y - self_pose.position.y); + if (!is_init || distance < min_distance) { + is_init = true; + min_distance = distance; + min_distance_index = i; + } + } + for (size_t i = min_distance_index; i < input_trajectory.points.size(); ++i) { + output_trajectory.points.push_back(input_trajectory.points.at(i)); + } + output_trajectory.header = input_trajectory.header; + index = min_distance_index; + return true; +} + +bool Trajectory::trimTrajectoryFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose self_pose, + autoware_planning_msgs::msg::Trajectory & output_trajectory) +{ + size_t index; + return trimTrajectoryWithIndexFromSelfPose( + output_trajectory, self_pose, output_trajectory, index); +} +} // namespace motion_planning From b729ad2c692433dd1a921e55f05df1fad38bf52a Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 1 Mar 2021 16:42:08 +0900 Subject: [PATCH 06/62] add inline prefix --- .../include/obstacle_stop_planner/util.hpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index cb41faf7..4f498b34 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -32,7 +32,7 @@ using Line = bg::model::linestring; namespace { -double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & quat) +inline double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); double roll, pitch, yaw; @@ -40,7 +40,8 @@ double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & q return yaw; } -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) + +inline std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) { const std::string json_dumps_pose = (boost::format( @@ -50,7 +51,8 @@ std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) .str(); return json_dumps_pose; } -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + +inline diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; @@ -64,7 +66,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( return stop_reason_diag; } -cv::Point2d calcCentroid(const std::vector pointcloud) +inline cv::Point2d calcCentroid(const std::vector pointcloud) { cv::Point2d centroid; centroid.x = 0; @@ -79,13 +81,13 @@ cv::Point2d calcCentroid(const std::vector pointcloud) return centroid; } -Point convertPointRosToBoost(const geometry_msgs::msg::Point &point) +inline Point convertPointRosToBoost(const geometry_msgs::msg::Point &point) { const Point point2d(point.x, point.y); return point2d; } -geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion &q) +inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion &q) { tf2::Quaternion quat(q.x, q.y, q.z, q.w); tf2::Matrix3x3 mat(quat); @@ -98,7 +100,7 @@ geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion &q) return rpy; } -Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size, +inline Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size, const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) { Polygon obj_poly; @@ -122,7 +124,7 @@ Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_msgs::ms return translate_obj_poly; } -double getDistanceFromTwoPoint(const geometry_msgs::msg::Point &point1, const geometry_msgs::msg::Point &point2) +inline double getDistanceFromTwoPoint(const geometry_msgs::msg::Point &point1, const geometry_msgs::msg::Point &point2) { const double dx = point1.x - point2.x; const double dy = point1.y - point2.y; From d6a880ea1eca3b6f31b9f961c02572847e09ea82 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 1 Mar 2021 17:53:20 +0900 Subject: [PATCH 07/62] refactoring --- .../include/obstacle_stop_planner/node.hpp | 7 --- .../obstacle_stop_planner/point_helper.hpp | 3 + .../include/obstacle_stop_planner/polygon.hpp | 47 ++++++++++++++ .../obstacle_stop_planner/trajectory.hpp | 4 ++ .../include/obstacle_stop_planner/vehicle.hpp | 9 +++ obstacle_stop_planner_refine/src/node.cpp | 63 ++----------------- .../src/point_helper.cpp | 23 ++++++- .../src/trajectory.cpp | 23 +++++++ 8 files changed, 113 insertions(+), 66 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index a0557c40..a490f5bb 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -101,15 +101,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node void insertSlowDownVelocity( const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path); - double calcSlowDownTargetVel(const double lateral_deviation); - bool extendTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const double extend_distance, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( - double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); }; } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index 1cb62038..f22bf545 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -86,6 +86,9 @@ class PointHelper const autoware_planning_msgs::msg::Trajectory & base_path, autoware_planning_msgs::msg::Trajectory & output_path); + autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( + double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); + private: std::shared_ptr vehicle_info_; }; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp new file mode 100644 index 00000000..e3f2ec35 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp @@ -0,0 +1,47 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__POLYGON_HPP_ +#define OBSTACLE_STOP_PLANNER__POLYGON_HPP_ + +#include +#include "boost/assert.hpp" +#include "boost/assign/list_of.hpp" +#include "boost/format.hpp" +#include "boost/geometry.hpp" +#include "boost/geometry/geometries/linestring.hpp" +#include "boost/geometry/geometries/point_xy.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" + + +namespace motion_planning { + +inline static Polygon ConvertToBoostPolygon(std::vector& polygon) +{ + // convert boost polygon + Polygon boost_polygon; + for (const auto & point : polygon) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back( + bg::make( + polygon.front().x, polygon.front().y)); + return boost_polygon; +} + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__POLYGON_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index dd63f32f..743cc923 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -44,6 +44,10 @@ class Trajectory const geometry_msgs::msg::Pose self_pose, autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index); + bool extendTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const double extend_distance, + autoware_planning_msgs::msg::Trajectory & output_trajectory); }; } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp index f812cb6f..0d88ae3d 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -67,6 +67,15 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo return center_pose; } + double getSearchRadius() + { + if(enable_slow_down_) { + return slow_down_search_radius_; + } else { + return stop_search_radius_; + } + } + double stop_margin_; double min_behavior_stop_margin_; double step_length_; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index ce4be13b..81e0f113 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -34,6 +34,7 @@ #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" #include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/polygon.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" @@ -112,7 +113,7 @@ void ObstacleStopPlannerNode::pathCallback( * extend trajectory to consider obstacles after the goal */ autoware_planning_msgs::msg::Trajectory extended_trajectory; - extendTrajectory(*input_msg, vehicle_info_.extend_distance_, extended_trajectory); + trajectory_.extendTrajectory(*input_msg, vehicle_info_.extend_distance_, extended_trajectory); const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; @@ -144,7 +145,7 @@ void ObstacleStopPlannerNode::pathCallback( new pcl::PointCloud); // search obstacle candidate pointcloud to reduce calculation cost - const double search_radius = vehicle_info_.enable_slow_down_ ? vehicle_info_.slow_down_search_radius_ : vehicle_info_.stop_search_radius_; + const double search_radius = vehicle_info_.getSearchRadius(); obstacle_pointcloud_.SetSearchRadius(search_radius); obstacle_pointcloud_.SearchCandidateObstacle(tf_buffer_, trajectory); @@ -186,13 +187,7 @@ void ObstacleStopPlannerNode::pathCallback( debug_ptr_->pushPolygon( one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); // convert boost polygon - Polygon boost_one_step_move_vehicle_polygon; - for (const auto & point : one_step_move_vehicle_polygon) { - boost_one_step_move_vehicle_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_one_step_move_vehicle_polygon.outer().push_back( - bg::make( - one_step_move_vehicle_polygon.front().x, one_step_move_vehicle_polygon.front().y)); + auto boost_one_step_move_vehicle_polygon = ConvertToBoostPolygon(one_step_move_vehicle_polygon); std::vector one_step_move_slow_down_range_polygon; Polygon boost_one_step_move_slow_down_range_polygon; @@ -207,14 +202,7 @@ void ObstacleStopPlannerNode::pathCallback( one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); // convert boost polygon - for (const auto & point : one_step_move_slow_down_range_polygon) { - boost_one_step_move_slow_down_range_polygon.outer().push_back( - bg::make(point.x, point.y)); - } - boost_one_step_move_slow_down_range_polygon.outer().push_back( - bg::make( - one_step_move_slow_down_range_polygon.front().x, - one_step_move_slow_down_range_polygon.front().y)); + boost_one_step_move_slow_down_range_polygon = ConvertToBoostPolygon(one_step_move_slow_down_range_polygon); } // check within polygon @@ -422,47 +410,6 @@ void ObstacleStopPlannerNode::currentVelocityCallback( current_velocity_ptr_ = input_msg; } -autoware_planning_msgs::msg::TrajectoryPoint ObstacleStopPlannerNode::getExtendTrajectoryPoint( - const double extend_distance, const autoware_planning_msgs::msg::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); - autoware_planning_msgs::msg::TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = extend_pose; - extend_trajectory_point.twist = goal_point.twist; - extend_trajectory_point.accel = goal_point.accel; - return extend_trajectory_point; -} - -bool ObstacleStopPlannerNode::extendTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const double extend_distance, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - output_trajectory = input_trajectory; - const auto goal_point = input_trajectory.points.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_trajectory.points.push_back(extend_trajectory_point); - extend_sum += interpolation_distance; - } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); - output_trajectory.points.push_back(extend_trajectory_point); - - return true; -} - void ObstacleStopPlannerNode::createOneStepPolygon( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, std::vector & polygon, const double expand_width) diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 83f0d695..4d60d805 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -96,7 +96,7 @@ StopPoint PointHelper::searchInsertPoint( const auto max_dist_stop_point = createTargetPoint(idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, base_path); const auto min_dist_stop_point = createTargetPoint( - idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, base_path); + idx, vehicle_info_->min_behavior_stop_margin_, trajectory_vec, collision_point_vec, base_path); // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; @@ -208,4 +208,25 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertSlowDownStartPoi } return slow_down_start_trajectory_point; } + +autoware_planning_msgs::msg::TrajectoryPoint PointHelper::getExtendTrajectoryPoint( + const double extend_distance, const autoware_planning_msgs::msg::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); + autoware_planning_msgs::msg::TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.twist = goal_point.twist; + extend_trajectory_point.accel = goal_point.accel; + return extend_trajectory_point; +} + } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index ceb41ee2..96fc96e4 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -103,4 +103,27 @@ bool Trajectory::trimTrajectoryFromSelfPose( return trimTrajectoryWithIndexFromSelfPose( output_trajectory, self_pose, output_trajectory, index); } + +bool Trajectory::extendTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const double extend_distance, + autoware_planning_msgs::msg::Trajectory & output_trajectory) +{ + output_trajectory = input_trajectory; + const auto goal_point = input_trajectory.points.back(); + double interpolation_distance = 0.1; + PointHelper point_helper; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint(extend_sum, goal_point); + output_trajectory.points.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint(extend_distance, goal_point); + output_trajectory.points.push_back(extend_trajectory_point); + + return true; +} + } // namespace motion_planning From 3fb56504a060d372e95d1f98a695f2834a8e5d88 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 1 Mar 2021 18:10:28 +0900 Subject: [PATCH 08/62] refactoring --- .../include/obstacle_stop_planner/node.hpp | 8 +++---- obstacle_stop_planner_refine/src/node.cpp | 24 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index a490f5bb..49acfe75 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -90,11 +90,11 @@ class ObstacleStopPlannerNode : public rclcpp::Node const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); private: - bool convexHull( - const std::vector pointcloud, std::vector & polygon_points); - void createOneStepPolygon( + std::vector convexHull( + const std::vector pointcloud); + std::vector createOneStepPolygon( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, - std::vector & polygon, const double expand_width = 0.0); + const double expand_width = 0.0); bool getSelfPose( const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, geometry_msgs::msg::Pose & self_pose); diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 81e0f113..f7c4f30b 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -180,9 +180,8 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step polygon for vehicle */ - std::vector one_step_move_vehicle_polygon; - createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, one_step_move_vehicle_polygon, + std::vector one_step_move_vehicle_polygon = createOneStepPolygon( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_stop_range_); debug_ptr_->pushPolygon( one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); @@ -195,9 +194,9 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step polygon for slow_down range */ - createOneStepPolygon( + one_step_move_slow_down_range_polygon = createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - one_step_move_slow_down_range_polygon, vehicle_info_.expand_slow_down_range_); + vehicle_info_.expand_slow_down_range_); debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); @@ -410,9 +409,9 @@ void ObstacleStopPlannerNode::currentVelocityCallback( current_velocity_ptr_ = input_msg; } -void ObstacleStopPlannerNode::createOneStepPolygon( +std::vector ObstacleStopPlannerNode::createOneStepPolygon( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, - std::vector & polygon, const double expand_width) + const double expand_width) { std::vector one_step_move_vehicle_corner_points; // start step @@ -471,11 +470,11 @@ void ObstacleStopPlannerNode::createOneStepPolygon( next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); } - convexHull(one_step_move_vehicle_corner_points, polygon); + return convexHull(one_step_move_vehicle_corner_points); } -bool ObstacleStopPlannerNode::convexHull( - const std::vector pointcloud, std::vector & polygon_points) +std::vector ObstacleStopPlannerNode::convexHull( + const std::vector pointcloud) { auto centroid = calcCentroid(pointcloud); @@ -488,13 +487,14 @@ bool ObstacleStopPlannerNode::convexHull( } cv::convexHull(normalized_pointcloud, normalized_polygon_points); + std::vector polygon_points{normalized_polygon_points.size()}; 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); + polygon_points.emplace_back(polygon_point); } - return true; + return polygon_points; } bool ObstacleStopPlannerNode::getSelfPose( From 48178ed51e3bd96660a6b8d0c4631a1d48522ec9 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 2 Mar 2021 09:43:42 +0900 Subject: [PATCH 09/62] create OneStepPolygon class --- .../include/obstacle_stop_planner/node.hpp | 5 - .../one_step_polygon.hpp | 159 ++++++++++++++++++ .../include/obstacle_stop_planner/polygon.hpp | 47 ------ obstacle_stop_planner_refine/src/node.cpp | 119 ++----------- 4 files changed, 172 insertions(+), 158 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp delete mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 49acfe75..3af88ee1 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -90,11 +90,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); private: - std::vector convexHull( - const std::vector pointcloud); - std::vector createOneStepPolygon( - const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, - const double expand_width = 0.0); bool getSelfPose( const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, geometry_msgs::msg::Pose & self_pose); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp new file mode 100644 index 00000000..ed0922ad --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -0,0 +1,159 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ +#define OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ + +#include +#include +#include "boost/assert.hpp" +#include "boost/assign/list_of.hpp" +#include "boost/format.hpp" +#include "boost/geometry.hpp" +#include "boost/geometry/geometries/linestring.hpp" +#include "boost/geometry/geometries/point_xy.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/vehicle.hpp" + + +namespace motion_planning { + +class OneStepPolygon +{ +public: + void Create( + const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, + const double expand_width); + Polygon GetBoostPolygon() {return boost_polygon_;} + std::vector GetPolygon() {return polygon_;} + void SetVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} + +private: + std::vector polygon_; + Polygon boost_polygon_; + std::shared_ptr vehicle_info_; + + Polygon ConvertToBoostPolygon(const std::vector& polygon); + std::vector convexHull(const std::vector pointcloud); +}; + +inline Polygon OneStepPolygon::ConvertToBoostPolygon(const std::vector& polygon) +{ + // convert boost polygon + Polygon boost_polygon; + for (const auto & point : polygon) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back( + bg::make( + polygon.front().x, polygon.front().y)); + return boost_polygon; +} + +inline void OneStepPolygon::Create( + const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, + const double expand_width) +{ + std::vector one_step_move_vehicle_corner_points; + // start step + { + double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + } + // next step + { + double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + one_step_move_vehicle_corner_points.push_back( + cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + } + polygon_ = convexHull(one_step_move_vehicle_corner_points); + boost_polygon_ = ConvertToBoostPolygon(polygon_); +} + +inline std::vector OneStepPolygon::convexHull( + const std::vector pointcloud) +{ + auto centroid = calcCentroid(pointcloud); + + std::vector normalized_pointcloud; + std::vector normalized_polygon_points; + for (size_t i = 0; i < pointcloud.size(); ++i) { + normalized_pointcloud.push_back( + cv::Point( + (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0)); + } + cv::convexHull(normalized_pointcloud, normalized_polygon_points); + + std::vector polygon_points{normalized_polygon_points.size()}; + 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.emplace_back(polygon_point); + } + return polygon_points; +} + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp deleted file mode 100644 index e3f2ec35..00000000 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/polygon.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 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 OBSTACLE_STOP_PLANNER__POLYGON_HPP_ -#define OBSTACLE_STOP_PLANNER__POLYGON_HPP_ - -#include -#include "boost/assert.hpp" -#include "boost/assign/list_of.hpp" -#include "boost/format.hpp" -#include "boost/geometry.hpp" -#include "boost/geometry/geometries/linestring.hpp" -#include "boost/geometry/geometries/point_xy.hpp" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" - - -namespace motion_planning { - -inline static Polygon ConvertToBoostPolygon(std::vector& polygon) -{ - // convert boost polygon - Polygon boost_polygon; - for (const auto & point : polygon) { - boost_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_polygon.outer().push_back( - bg::make( - polygon.front().x, polygon.front().y)); - return boost_polygon; -} - -} // namespace motion_planning - -#endif // OBSTACLE_STOP_PLANNER__POLYGON_HPP_ diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index f7c4f30b..176d1efd 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -34,7 +34,7 @@ #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" #include "obstacle_stop_planner/point_helper.hpp" -#include "obstacle_stop_planner/polygon.hpp" +#include "obstacle_stop_planner/one_step_polygon.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" @@ -147,7 +147,6 @@ void ObstacleStopPlannerNode::pathCallback( // search obstacle candidate pointcloud to reduce calculation cost const double search_radius = vehicle_info_.getSearchRadius(); obstacle_pointcloud_.SetSearchRadius(search_radius); - obstacle_pointcloud_.SearchCandidateObstacle(tf_buffer_, trajectory); /* @@ -180,28 +179,24 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step polygon for vehicle */ - std::vector one_step_move_vehicle_polygon = createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + OneStepPolygon move_vehicle_polygon; + move_vehicle_polygon.SetVehicleInfo(vehicle_info_); + move_vehicle_polygon.Create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_stop_range_); debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); - // convert boost polygon - auto boost_one_step_move_vehicle_polygon = ConvertToBoostPolygon(one_step_move_vehicle_polygon); + move_vehicle_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); - std::vector one_step_move_slow_down_range_polygon; - Polygon boost_one_step_move_slow_down_range_polygon; + OneStepPolygon move_slow_down_range_polygon; + move_slow_down_range_polygon.SetVehicleInfo(vehicle_info_); if (vehicle_info_.enable_slow_down_) { /* * create one step polygon for slow_down range */ - one_step_move_slow_down_range_polygon = createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + move_slow_down_range_polygon.Create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_slow_down_range_); debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, + move_slow_down_range_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); - // convert boost polygon - boost_one_step_move_slow_down_range_polygon = ConvertToBoostPolygon(one_step_move_slow_down_range_polygon); } // check within polygon @@ -216,7 +211,7 @@ void ObstacleStopPlannerNode::pathCallback( bg::distance(prev_center_point, point) < vehicle_info_.slow_down_search_radius_ || bg::distance(next_center_point, point) < vehicle_info_.slow_down_search_radius_) { - if (bg::within(point, boost_one_step_move_slow_down_range_polygon)) { + if (bg::within(point, move_slow_down_range_polygon.GetBoostPolygon())) { slow_down_pointcloud_ptr->push_back(obstacle_candidate_pointcloud_ptr->at(j)); candidate_slow_down = true; } @@ -231,11 +226,11 @@ void ObstacleStopPlannerNode::pathCallback( bg::distance(prev_center_point, point) < vehicle_info_.stop_search_radius_ || bg::distance(next_center_point, point) < vehicle_info_.stop_search_radius_) { - if (bg::within(point, boost_one_step_move_vehicle_polygon)) { + if (bg::within(point, move_vehicle_polygon.GetBoostPolygon())) { collision_pointcloud_ptr->push_back(slow_down_pointcloud_ptr->at(j)); is_collision = true; debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, trajectory.points.at(i).pose.position.z, + move_vehicle_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::Collision); } } @@ -244,7 +239,7 @@ void ObstacleStopPlannerNode::pathCallback( is_slow_down = true; decimate_trajectory_slow_down_index = i; debug_ptr_->pushPolygon( - one_step_move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, + move_slow_down_range_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); point_helper.getNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_slow_down_point, @@ -409,94 +404,6 @@ void ObstacleStopPlannerNode::currentVelocityCallback( current_velocity_ptr_ = input_msg; } -std::vector ObstacleStopPlannerNode::createOneStepPolygon( - const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, - const double expand_width) -{ - std::vector one_step_move_vehicle_corner_points; - // start step - { - double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - - std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + - std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - - std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + - std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - - std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + - std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - - std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + - std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); - } - // next step - { - double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - - std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + - std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) - - std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_) + - std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - - std::sin(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + - std::cos(yaw) * (-vehicle_info_.vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_.rear_overhang_m_) - - std::sin(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_.rear_overhang_m_) + - std::cos(yaw) * (vehicle_info_.vehicle_width_m_ / 2.0 + expand_width))); - } - return convexHull(one_step_move_vehicle_corner_points); -} - -std::vector ObstacleStopPlannerNode::convexHull( - const std::vector pointcloud) -{ - auto centroid = calcCentroid(pointcloud); - - std::vector normalized_pointcloud; - std::vector normalized_polygon_points; - for (size_t i = 0; i < pointcloud.size(); ++i) { - normalized_pointcloud.push_back( - cv::Point( - (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0)); - } - cv::convexHull(normalized_pointcloud, normalized_polygon_points); - - std::vector polygon_points{normalized_polygon_points.size()}; - 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.emplace_back(polygon_point); - } - return polygon_points; -} - bool ObstacleStopPlannerNode::getSelfPose( const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, geometry_msgs::msg::Pose & self_pose) From d840ded4c60e47bd1e40d316ba0cbc6013ddac6d Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 2 Mar 2021 12:00:27 +0900 Subject: [PATCH 10/62] refactoring --- .../include/obstacle_stop_planner/node.hpp | 6 + .../one_step_polygon.hpp | 4 +- .../obstacle_stop_planner/point_helper.hpp | 18 +- .../include/obstacle_stop_planner/vehicle.hpp | 4 +- obstacle_stop_planner_refine/src/node.cpp | 196 ++++++++++-------- .../src/point_helper.cpp | 18 +- 6 files changed, 136 insertions(+), 110 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 3af88ee1..85ba0562 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -42,6 +42,8 @@ #include "obstacle_stop_planner/obstacle_point_cloud.hpp" #include "obstacle_stop_planner/vehicle.hpp" #include "obstacle_stop_planner/trajectory.hpp" +#include "obstacle_stop_planner/one_step_polygon.hpp" +#include "obstacle_stop_planner/point_helper.hpp" namespace motion_planning { @@ -97,6 +99,10 @@ class ObstacleStopPlannerNode : public rclcpp::Node const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path); double calcSlowDownTargetVel(const double lateral_deviation); + void getSlowDownPointcloud(const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const Polygon& one_step_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down); + void getCollisionPointcloud(const pcl::PointCloud::Ptr slow_down_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const OneStepPolygon& one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool& is_collision); + void insertStopPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_collision_point, const PointHelper& point_helper, autoware_planning_msgs::msg::Trajectory & output_msg, diagnostic_msgs::msg::DiagnosticStatus& stop_reason_diag); + void insertSlowDownPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_slow_down_point, const PointHelper& point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg); }; } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index ed0922ad..d58198cb 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -39,8 +39,8 @@ class OneStepPolygon void Create( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width); - Polygon GetBoostPolygon() {return boost_polygon_;} - std::vector GetPolygon() {return polygon_;} + Polygon GetBoostPolygon() const {return boost_polygon_;} + std::vector GetPolygon() const {return polygon_;} void SetVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} private: diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index f22bf545..b5954cf5 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -54,40 +54,40 @@ class PointHelper bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, const Eigen::Vector2d & base_point, const double backward_length, - Eigen::Vector2d & output_point); + Eigen::Vector2d & output_point) const; void getNearestPoint( const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); + pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) const; void getLateralNearestPoint( const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation); + pcl::PointXYZ * lateral_nearest_point, double * deviation) const; autoware_planning_msgs::msg::TrajectoryPoint insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path); + autoware_planning_msgs::msg::Trajectory & output_path) const; StopPoint searchInsertPoint( const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec); + const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) const; StopPoint createTargetPoint( const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path); + const autoware_planning_msgs::msg::Trajectory & base_path) const; SlowDownPoint createSlowDownStartPoint( const int idx, const double margin, const double slow_down_target_vel, const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path, - const double current_velocity_x); + const double current_velocity_x) const; autoware_planning_msgs::msg::TrajectoryPoint insertSlowDownStartPoint( const SlowDownPoint & slow_down_start_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path); + autoware_planning_msgs::msg::Trajectory & output_path) const; autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( - double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); + double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const; private: std::shared_ptr vehicle_info_; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp index 0d88ae3d..fb9595d8 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -54,7 +54,7 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo } geometry_msgs::msg::Pose getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose) + const geometry_msgs::msg::Pose & base_pose) const { geometry_msgs::msg::Pose center_pose; const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); @@ -67,7 +67,7 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo return center_pose; } - double getSearchRadius() + double getSearchRadius() const { if(enable_slow_down_) { return slow_down_search_radius_; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 176d1efd..6b0ace42 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -33,8 +33,6 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" -#include "obstacle_stop_planner/point_helper.hpp" -#include "obstacle_stop_planner/one_step_polygon.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" @@ -165,6 +163,7 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointXYZ lateral_nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; + PointHelper point_helper; point_helper.SetVehicleInfo(vehicle_info_); @@ -203,38 +202,10 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - if (!is_slow_down && vehicle_info_.enable_slow_down_) { - for (size_t j = 0; j < obstacle_candidate_pointcloud_ptr->size(); ++j) { - Point point( - obstacle_candidate_pointcloud_ptr->at(j).x, obstacle_candidate_pointcloud_ptr->at(j).y); - if ( - bg::distance(prev_center_point, point) < vehicle_info_.slow_down_search_radius_ || - bg::distance(next_center_point, point) < vehicle_info_.slow_down_search_radius_) - { - if (bg::within(point, move_slow_down_range_polygon.GetBoostPolygon())) { - slow_down_pointcloud_ptr->push_back(obstacle_candidate_pointcloud_ptr->at(j)); - candidate_slow_down = true; - } - } - } - } else { - slow_down_pointcloud_ptr = obstacle_candidate_pointcloud_ptr; - } - for (size_t j = 0; j < slow_down_pointcloud_ptr->size(); ++j) { - Point point(slow_down_pointcloud_ptr->at(j).x, slow_down_pointcloud_ptr->at(j).y); - if ( - bg::distance(prev_center_point, point) < vehicle_info_.stop_search_radius_ || - bg::distance(next_center_point, point) < vehicle_info_.stop_search_radius_) - { - if (bg::within(point, move_vehicle_polygon.GetBoostPolygon())) { - collision_pointcloud_ptr->push_back(slow_down_pointcloud_ptr->at(j)); - is_collision = true; - debug_ptr_->pushPolygon( - move_vehicle_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, - PolygonType::Collision); - } - } - } + getSlowDownPointcloud(is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point,next_center_point, vehicle_info_.slow_down_search_radius_, move_slow_down_range_polygon.GetBoostPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); + + getCollisionPointcloud(slow_down_pointcloud_ptr, prev_center_point, next_center_point, vehicle_info_.stop_search_radius_, move_vehicle_polygon, trajectory.points.at(i), collision_pointcloud_ptr, is_collision); + if (candidate_slow_down && !is_collision && !is_slow_down) { is_slow_down = true; decimate_trajectory_slow_down_index = i; @@ -277,76 +248,125 @@ void ObstacleStopPlannerNode::pathCallback( * insert stop point */ if (need_to_stop) { - for (size_t i = decimate_trajectory_index_map.at(decimate_trajectory_collision_index) + - trajectory_trim_index; - i < base_path.points.size(); ++i) + insertStopPoint(decimate_trajectory_index_map.at(decimate_trajectory_collision_index) + trajectory_trim_index, base_path, nearest_collision_point, point_helper, output_msg, stop_reason_diag); + } + + /* + * insert slow_down point + */ + if (is_slow_down) { + insertSlowDownPoint(decimate_trajectory_index_map.at(decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, point_helper, calcSlowDownTargetVel(lateral_deviation), vehicle_info_.slow_down_margin_, output_msg); + } + path_pub_->publish(output_msg); + stop_reason_diag_pub_->publish(stop_reason_diag); + debug_ptr_->publish(); +} + +// collision +void ObstacleStopPlannerNode::getCollisionPointcloud(const pcl::PointCloud::Ptr slow_down_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const OneStepPolygon& one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool& is_collision) +{ + for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { + Point point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); + if ( + bg::distance(prev_center_point, point) < search_radius || + bg::distance(next_center_point, point) < search_radius) { - Eigen::Vector2d trajectory_vec; - { - const double yaw = - getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); - trajectory_vec << std::cos(yaw), std::sin(yaw); + if (bg::within(point, one_step_polygon.GetBoostPolygon())) { + collision_pointcloud->push_back(slow_down_pointcloud->at(j)); + is_collision = true; + debug_ptr_->pushPolygon( + one_step_polygon.GetPolygon(), trajectory_point.pose.position.z, + PolygonType::Collision); } - Eigen::Vector2d collision_point_vec; - collision_point_vec << nearest_collision_point.x - base_path.points.at(i).pose.position.x, - nearest_collision_point.y - base_path.points.at(i).pose.position.y; + } + } +} +// slow down +void ObstacleStopPlannerNode::getSlowDownPointcloud(const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const Polygon& boost_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down) +{ + if (!is_slow_down && enable_slow_down) { + for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { + Point point( + obstacle_candidate_pointcloud->at(j).x, obstacle_candidate_pointcloud->at(j).y); if ( - trajectory_vec.dot(collision_point_vec) < 0.0 || - (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) + bg::distance(prev_center_point, point) < search_radius || + bg::distance(next_center_point, point) < search_radius) { - const auto stop_point = - point_helper.searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); - if (stop_point.index <= output_msg.points.size()) { - const auto trajectory_point = point_helper.insertStopPoint(stop_point, base_path, output_msg); - stop_reason_diag = makeStopReasonDiag("obstacle", trajectory_point.pose); - debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); + if (bg::within(point, boost_polygon)) { + slow_down_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); + candidate_slow_down = true; } - break; } } + } else { + slow_down_pointcloud = obstacle_candidate_pointcloud; } +} - /* - * insert slow_down point - */ - if (is_slow_down) { - for (size_t i = decimate_trajectory_index_map.at(decimate_trajectory_slow_down_index); - i < base_path.points.size(); ++i) +void ObstacleStopPlannerNode::insertSlowDownPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_slow_down_point, const PointHelper& point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg) +{ + for (size_t i = search_start_index; i < base_path.points.size(); ++i) + { + Eigen::Vector2d trajectory_vec; { - Eigen::Vector2d trajectory_vec; - { - const double yaw = - getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); - trajectory_vec << std::cos(yaw), std::sin(yaw); + const double yaw = + getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); + trajectory_vec << std::cos(yaw), std::sin(yaw); + } + Eigen::Vector2d slow_down_point_vec; + slow_down_point_vec << nearest_slow_down_point.x - base_path.points.at(i).pose.position.x, + nearest_slow_down_point.y - base_path.points.at(i).pose.position.y; + + if ( + trajectory_vec.dot(slow_down_point_vec) < 0.0 || + (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) + { + const auto slow_down_start_point = point_helper.createSlowDownStartPoint( + i, slow_down_margin, slow_down_target_vel, trajectory_vec, slow_down_point_vec, + base_path, current_velocity_ptr_->twist.linear.x); + + if (slow_down_start_point.index <= output_msg.points.size()) { + const auto slowdown_trajectory_point = point_helper.insertSlowDownStartPoint(slow_down_start_point, base_path, output_msg); + debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); + insertSlowDownVelocity( + slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, + output_msg); } - Eigen::Vector2d slow_down_point_vec; - slow_down_point_vec << nearest_slow_down_point.x - base_path.points.at(i).pose.position.x, - nearest_slow_down_point.y - base_path.points.at(i).pose.position.y; + break; + } + } +} - if ( - trajectory_vec.dot(slow_down_point_vec) < 0.0 || - (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) - { - const double slow_down_target_vel = calcSlowDownTargetVel(lateral_deviation); - const auto slow_down_start_point = point_helper.createSlowDownStartPoint( - i, vehicle_info_.slow_down_margin_, slow_down_target_vel, trajectory_vec, slow_down_point_vec, - base_path, current_velocity_ptr_->twist.linear.x); - - if (slow_down_start_point.index <= output_msg.points.size()) { - const auto slowdown_trajectory_point = point_helper.insertSlowDownStartPoint(slow_down_start_point, base_path, output_msg); - debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); - insertSlowDownVelocity( - slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, - output_msg); - } - break; +// stop +void ObstacleStopPlannerNode::insertStopPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_collision_point, const PointHelper& point_helper, autoware_planning_msgs::msg::Trajectory & output_msg, diagnostic_msgs::msg::DiagnosticStatus& stop_reason_diag) +{ + for (size_t i = search_start_index; i < base_path.points.size(); ++i) + { + Eigen::Vector2d trajectory_vec; + { + const double yaw = + getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); + trajectory_vec << std::cos(yaw), std::sin(yaw); + } + Eigen::Vector2d collision_point_vec; + collision_point_vec << nearest_collision_point.x - base_path.points.at(i).pose.position.x, + nearest_collision_point.y - base_path.points.at(i).pose.position.y; + + if ( + trajectory_vec.dot(collision_point_vec) < 0.0 || + (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) + { + const auto stop_point = + point_helper.searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); + if (stop_point.index <= output_msg.points.size()) { + const auto trajectory_point = point_helper.insertStopPoint(stop_point, base_path, output_msg); + stop_reason_diag = makeStopReasonDiag("obstacle", trajectory_point.pose); + debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); } + break; } } - path_pub_->publish(output_msg); - stop_reason_diag_pub_->publish(stop_reason_diag); - debug_ptr_->publish(); } void ObstacleStopPlannerNode::externalExpandStopRangeCallback( diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 4d60d805..b96c2b89 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -19,7 +19,7 @@ namespace motion_planning { bool PointHelper::getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) + const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) const { Eigen::Vector2d line_vec = line_point2 - line_point1; Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); @@ -29,7 +29,7 @@ bool PointHelper::getBackwardPointFromBasePoint( void PointHelper::getNearestPoint( const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) + pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) const { double min_norm = 0.0; bool is_init = false; @@ -53,7 +53,7 @@ void PointHelper::getNearestPoint( void PointHelper::getLateralNearestPoint( const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation) + pcl::PointXYZ * lateral_nearest_point, double * deviation) const { double min_norm = std::numeric_limits::max(); const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); @@ -75,7 +75,7 @@ void PointHelper::getLateralNearestPoint( autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) + autoware_planning_msgs::msg::Trajectory & output_path) const { autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = base_path.points.at(std::max(static_cast(stop_point.index) - 1, 0)); @@ -91,7 +91,7 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( StopPoint PointHelper::searchInsertPoint( const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) + const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) const { const auto max_dist_stop_point = createTargetPoint(idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, base_path); @@ -118,7 +118,7 @@ StopPoint PointHelper::searchInsertPoint( StopPoint PointHelper::createTargetPoint( const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) + const autoware_planning_msgs::msg::Trajectory & base_path) const { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(collision_point_vec); @@ -152,7 +152,7 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( const int idx, const double margin, const double slow_down_target_vel, const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path, - const double current_velocity_x) + const double current_velocity_x) const { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); @@ -190,7 +190,7 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertSlowDownStartPoint( const SlowDownPoint & slow_down_start_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) + autoware_planning_msgs::msg::Trajectory & output_path) const { autoware_planning_msgs::msg::TrajectoryPoint slow_down_start_trajectory_point = base_path.points.at(std::max(static_cast(slow_down_start_point.index) - 1, 0)); @@ -210,7 +210,7 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertSlowDownStartPoi } autoware_planning_msgs::msg::TrajectoryPoint PointHelper::getExtendTrajectoryPoint( - const double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) + const double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const { tf2::Transform map2goal; tf2::fromMsg(goal_point.pose, map2goal); From e00f3929af1fad59d0da45241cf54dc5cc9ebe08 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 2 Mar 2021 14:19:51 +0900 Subject: [PATCH 11/62] update design document --- .../design/class_diagram.pu | 38 ++++++++++- obstacle_stop_planner_refine/design/design.md | 60 +----------------- .../design/img/class_diagram_orig.png | Bin 0 -> 14639 bytes .../design/img/class_diagram_refine-1.png | Bin 0 -> 42496 bytes 4 files changed, 39 insertions(+), 59 deletions(-) create mode 100644 obstacle_stop_planner_refine/design/img/class_diagram_orig.png create mode 100644 obstacle_stop_planner_refine/design/img/class_diagram_refine-1.png diff --git a/obstacle_stop_planner_refine/design/class_diagram.pu b/obstacle_stop_planner_refine/design/class_diagram.pu index 63634f7d..03a0183b 100644 --- a/obstacle_stop_planner_refine/design/class_diagram.pu +++ b/obstacle_stop_planner_refine/design/class_diagram.pu @@ -1,7 +1,43 @@ @startuml -Node <|-- ObstacleStopPlannerNode +namespace rclcpp { + class Node +} + +namespace motion_planning { +rclcpp.Node <|-- ObstacleStopPlannerNode +ObstacleStopPlannerNode *-- AdaptiveCruiseController +ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode +} + +@enduml + +@startuml + +namespace rclcpp { + class Node + class Logger +} + +namespace motion_planning { +rclcpp.Node <|-- ObstacleStopPlannerNode ObstacleStopPlannerNode *-- AdaptiveCruiseController ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode +ObstacleStopPlannerNode *-- ObstaclePointCloud +ObstacleStopPlannerNode *-- VehicleInfo +ObstaclePointCloud *-- VehicleInfo +ObstaclePointCloud *-- rclcpp.Logger +ObstacleStopPlannerNode *-- Trajectory +AdaptiveCruiseController *-- rclcpp.Node +ObstacleStopPlannerDebugNode *-- rclcpp.Node +OneStepPolygon *-- VehicleInfo +PointHelper *-- VehicleInfo + + +vehicle_info_util.VehicleInfo <|-- VehicleInfo +} +namespace vehicle_info_util { +class VehicleInfo +} @enduml \ No newline at end of file diff --git a/obstacle_stop_planner_refine/design/design.md b/obstacle_stop_planner_refine/design/design.md index 0c9699ee..1b41ce64 100644 --- a/obstacle_stop_planner_refine/design/design.md +++ b/obstacle_stop_planner_refine/design/design.md @@ -3,65 +3,9 @@ class ObstacleStopPlannerNode Key input: pathCallback(input/trajectory) Class diagram -```plantuml -@startuml -Node <|-- ObstacleStopPlannerNode -ObstacleStopPlannerNode *-- AdaptiveCruiseController -ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode - -@enduml -``` +![img](img/class_diagram_orig.png) Proposal class diagram -```plantuml -@startuml - -Node <|-- ObstacleStopPlannerNode -ObstacleStopPlannerNode *-- ObstacleStopPlanner -ObstacleStopPlanner *-- AdaptiveCruiseController -ObstacleStopPlanner *-- ObstacleStopPlannerDebugNode - -@enduml -``` - -Callback - -void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); - -void pathCallback(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg); - -``` -path_pub_->publish(output_msg); -stop_reason_diag_pub_->publish(stop_reason_diag); -debug_ptr_->publish(); -``` - -基本はこのメソッドで処理を行っている。長いが、構造はシンプル。 - -void dynamicObjectCallback( -const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg); -void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg); -void externalExpandStopRangeCallback( -const autoware_debug_msgs::msg::Float32Stamped::ConstSharedPtr input_msg); - -Publisher - -rclcpp::Publisher::SharedPtr path_pub*; -rclcpp::Publisher::SharedPtr stop_reason_diag_pub*; - - ---- - -class AdaptiveCruiseController - -publicメソッドは1つのみ。insertAdaptiveCruiseVelocity()というメソッドでTrajectoryを出力していることから、速度を調整しているっぽい。 -Publisherはdebug用のメッセージのみ。 - - ---- - -class ObstacleStopPlannerDebugNode -図形を保持してPublishしている？ -PublishするタイミングはpathCallback。 +![img](img/class_diagram_refine-1.png) diff --git a/obstacle_stop_planner_refine/design/img/class_diagram_orig.png b/obstacle_stop_planner_refine/design/img/class_diagram_orig.png new file mode 100644 index 0000000000000000000000000000000000000000..689bc42e6f6ee2a40e7db4938b8e3a34655c43a6 GIT binary patch literal 14639 zcmcJ$Wmwf~xF@^_NeStOrF3_9Nl14K(j`cDhm@3bcb7C0f(R(xU4nE;_xu;Q&))l- zdFP#(>kJ=6*Lvda_&vAp73E)|AQ2#eKp+%pDRE^G2pSm#f>K9-20k(GmSzBc(Kt$I zI=*>p=VonU>IiyiVr%lw(9y)0%*c(*+|kjFmx;;F+R)a~$;O)T&0Cvi%sfOO5R8R| zs;1*#pM#))V_eh9ljdv*SkQwHG**e}PbrJV#nJN^wk5a=6h+OY-svRki7o8T>Qu^M z)Ono`W?(3zvZ+e=ho5eIIqM(1Kggg;Al%8RT2=eJreBgoNK(BdBpbuy z!cpYVjH2Z2SbbI79bJ0q;-|hCe|_%xJ}5;Qo!ZIZ8&P>;#4BXEhQHs{WLz#Sl5N95HvC<8u)&arVK!WoECNQh$ut_F zjMHPCp^6h@L8&99=W<)11;1mL5~+`MLDr$+!IBVqJ_zj=sF%eCxZVI9gR4s!R&3m&o|tUtY-| zYBCnWGRLW=5ngG3#E{5~)XkTRk_m{qQdj&=_H%(hUhs+c84S0MD>PBRC)AS{d+mqt z+)@f1&4^^9$ibn6 z_%)`)c_D*H)CDc)8h)fJU=#8EAOo`eqLeJ{*=WP=^9KA_t*iO_soZnQ37@FCK4Yx~ zQc5w-H>&(kFk-$AizXk52}R8eh4mz5%6Lv9(k&iLY37ZY{^o$gEgYS~=-6OWP)_r@ zQ7ac6lg%O%(~OHMq@jLAV$}okK#rS*`e0a@W0m$wJo{@l^R<}`{?7X*or>~XqATw+ z{cc$(4-XzwIzx=6X>X2bd+McTwPw$GR@c{k6}k}IyR0}ioYg&%hH zkRN|7mVv|ezY9ZQe8F=&%A`NIg|-|(2@(g6Va$;L%X=xmyw-C6 zv3HH=dn^q0)$?=<3dP3}lc)_&J*#NTtQcH$yeXLm`5pjtwS?jXk>TX)5#7mTm_O+_ zhpZQm5hda?kZW;YUlgl`0{OrMft<6|5E70Y27l3O)sWk&5)Iuoyj{}2|9*9Gaq;ZM z%YFl9p>{HZy`dI(mmmG#)3|)}t#z%9jEtO}IS;Tue;k=#JpQ>>wnz0A$r8QD`%2PVEK9N+9lXG3;C+)0S2Aog z`kkDuJgsYEe|S-yvliJIbo2D_@{_Z(lFwMpY-DeAbW&$~73svKxSFe9hpT3H+yCrI zwpbspbG$P*`L)Q_@PrTK4Tl7^+bIQBWrPWQbz4RgMKV zW@aWK(LL*Ar4=_Tls7kOBD=G*bNOkrriPWR+>(}X)*=Kgv*pF#+hLXLON|z;oG>Zn z@mSt{{d{>GvFMDlpOBo)bKcZcZC^cBpa3Xgt<}E)iNB@k^;rdS4mPMA`caFi%orq+ zhw6TSF7CK_N}F0=T7xJs@rrPeRes0X$h#^0J7m(B$CKeABwk1vl1gnzDy8< zv3mvX{88{z!>5_IrL|N6*zNPMyI3@SkzjgHC3E~e3g`k2V&>Fpp?AW|xJ(=5g$5Xe zphHrK!gvx7;zWt;ZtsgH6P|c5-R)kEnN`kDan7h}^9A2vAc7d70Ac&r`e8wLzoL0x z&&RNg36a|-LE5{eFP}?}vE@e8!fe?`ZTTF~?9CNl$^zehgpd#~zTton@`WCX3u3W; z^JZ}U7upm)jtIoQpN0Im=7o9b75?E&*hR0SEwjX1?b}Kygqb ziMZyukSvHB5n_DS`)}?--sK)7-nsH{U$A*@WTSV`LFn)OCHRZ!qy9p05V&^`10)Yo z066uUK2M{;&=8^K6RdCCpJm8f6E>Q7{=4*0%j6LLYMPjg-BDoUk+J1c*(&4~wNJ*I zb>9`Vt5PA)6mzTw68YoIXhC;KL*!`eU~qV%?b>QDYPk|Ph2&zQ=ILITUy&adagw-g z4Cbqg{V+YC<&P$U`;RS|O743HXx9ARPHW#Bv=X<2C^^Pu1lOeRj8awbP{98pZs9~` z%X?)p{CJU;?U;l8H(25Khj^(?R515HSpZg@!^WQsn0fvt_t~c4jx|-lO;wq$+V~Q@*vU zbB{~VFfp0M3Ui@%cX#XQ>0Lg#%$59rF^{nAVQ5}ZpkJD4kRBJ8TVFIQhB-=#1jP^c zs8h_*onH}FEuUzEo2hpUw1%)&(_I!<`<+rwuwJ2?})n+`L( z$;imya%saSB^6@vey(O7Gt=iVcV_dZCsOX!tA6g7FSjs$!tU7TtQ>~4Dxobd2S{?z^E$;7b`@NStpz6}ENqOFp^H4O^*VmVq+x#quY;is?a{iZC+I2nZR}7~V zaZSGVK6?cQ!gC-c)V8dZqK>BbTT4qBDXFafL58mdPe<1SkCc~TCYbZm&@<;D<%)-J#nLM`k zc%8%jbmR1j0$L%f^#!wf67k=HW^+I@yi=jf^+3oeEw%dAkMDHqWV#P7DJjuxt*ME< z9!lf*R8bK%F#-5W50FM%q6fT0-6y?mbMLMq!ZORF3uQ*@J1v>e z($T#^VzE3an*Th1cIKShpa*=odMZp-Wj7*fyS2Z+SpBBw>&1K3qD~>L?X9huW~ZHL z`!`#ZKfl6LH3~WT+)rPBG93zw4A-l&6A=-C`Xu%*nr{VRs~x7~DKWO6^KN;Oc0vJ{ zzxfjaz^w#cg(=(~XS?6mKEC-Dl`VVWXRts`^4lWT=rG+99^m`~)MEgt-I~~RN%nuU zbif)U{+}Ghag5G;4%-*uPpkp{c_#!5dIk%GU{TI<{Ctg@4?LjfPabU?IYa-*rv%E5 zYSRwF7q@A{JOl+)>rUG4@b7{DP}63%}ILNjn{GYYa;bUFnn z?a%32QkAw{o#@79h#)FRW&sq1Gg>-V#SHro3nx7mO45+{NX-EMt`uNj_OC#B;0KVL z0ZA1Aoh0_qU>i`xI&us$T}?nHXw~(#B34f|ctQ`v zKyUx(KB>KYom9Eze(hG~s?-MQ4It+s(4$$*@_Jn2B3sj1$?UYp+K1d(rgmzYldVoi z#QdeB&Y&iyf-3M`C(IQcr#LDukd%lZqHS1jYp!S$y8qc)kl}qL>r~An+GvviQiP=4 zmHPRHJ$0ScZpo5igOc!pXPq`c;v)F7&8M%FZ)R8;`#*0gnRh8|Jtr(I@qmGG0=}#X zm5=FA12%#nmRI_2iEfINvGK%Mp-3J_&Eu=x1U(cPgpHMw`SY$1R>y=b7tbg9ax6dK zRisdWU?EOAA9pS2Hhruy!x50lED#K&wwCWW`15SCAoGVZMqyvaDL*I-KlixNECDnO zae$TBc{9hBtk&vK1jb|vkmJKT>3!tiy&|sM&z|YB%7QlktixTaqgZ)T7`hy*1ucvV z;f`yqZP~y}(MS5Za-(4ah#4AE#jLl9ao)FwESt*0gYV7|9Gq0h>vHMn4YH5=lhpl>tIc_doC zCYCWvNrgP{k=qN@2m+!uy-$$^*Ks9TZwS8x9S8)(%>L=)^Raz?jBZU8d|!e9_H+JEaIF)E>t9TT-JU8@(=ks=ffgr1l3 zU~2r^H_DV@b25t?=iOpYo<2}T_t-b#hhr4~N8^+Y4F{p?19nhWyAez%QQuag_t86f z9}p>f@*m2ThKd!oH?!gwJ4IL?*lg4Q8wGbOJ}Zw}?kMPLeE-kp>Ez+22I@f0HrFz!Cw}u$O3hT|;~kw~xA$xl#FaoWMFf%j*M3k5$L5&V(%Vj&;+saI5r>9CJKBiyU z`r}Miek78%H{H*eWsW>wz$u#Q+SU$FZp-w!%ws|)Vn|`77drCiyJRj z_}re=-k!Ouf>}`0y*$S;X8A4dEDXn@*ZOEoL1<#371AkY7h9X-?g|6$ExqiaJEu2ul$ja>IFYLF~)hkO&bsD_&JU#h~ zi9e}G{KZ;N6d_^mjN0P&Vyms2k?;MjX12)AkO0=fzoLY|7H(R@Uuwl*P*BO(|0P0n zI2~!?1F_KL*qE5K?n2%|Sl#4u1|_zC(qp?R1~}$xk_{};i@Z>;sxBcKBaO|nLFOfm zRF*m%s9&#tsT4FS*{dFGZ|As6NCY)Yw_sb9e-W4|e09S{ovV->3fw+s1H14l6h-*K z&bbTGcP-e~uK;D9*=gR^x{a<$uW6~NnPgW&tQ)#tq*YX#o{mnXq2>xGzTdzZOj=r4 zNNat}u^qMT*0!=!ILmXF7)6>LB2y@N|2^25mp5bx3eUUp!qCc%AhGa#dTIs5s4R9udQQn^)?Y z=dZG4qb{;Jg~c2Dcj^uvAErTV4?4|4SL%4He=)HBBax4f?_jYtHJ<%YQ`D;`_ZQ%# zj=oXpl{kbQxDVl?2iu>oEDen zEo}`7c(^Z3bWOKS0m&Y8UpTv9@?|kx$m8y0hVHyhjyexr`vp>%kBn8%PtpsPfHcu) z&S=rBEb+JCpkV?f#TykFV?o$h6@+H`d=#SoQeK0Rz|~o0$K05xXZv7B;e+a0pTNFCvG0y+L{i~}V5Wz79xOSBsk-eR|j4gKJFTA+WvEzHG z?5G2AU-vwDLJ_g<`9$8K(h%$?2L244ax!C&~`~>2skw5b@>7WU8Y(veEU)E@<@r*F7=H#mD6p*c{x)b0g%p?(RCi{nU7A z%b(~}tZ%L?2Fb^3O!ZLrhFJ1svyaCLpT`$Cw%Z+ayS@FGe7TKz8BKL5M(%6N4Tbjl zflc3i8o7V{0@{udKqUK%YJU!4@=(6I81D{_gGHsW7wG(YdhwllvyywHM z^vlZ2*Uz$}P(|kSfMESvi7A*oPYA2C^gr(VS0|kuWlI(o9^TU2{2TfA%SaN6#%JT_ zox{6|N=hu=ofn;*oj~W*yqD$PU5+$~#kx=w^9~1LhRa0l`~E^pYD$VZm8`6+24$u3 zSu}A#S4%@}?dQ`;ogWEIn7gQAb9%Qo+i7V;brKUKDbaU|2$e>UQnizLJJ?vyw_^{t zQRZVJpc3k`=oUigFeoU^PV~^cPF5OZ#>9NV{Xi-tZ}rAW8_Xf6kS3ovJ-0NyICp*F zP+n70<958!{jGa2gYEhxd8|JU_m{)@{z9GAR1`7S*$m52+sSX)c>m zG_T@9w(Ti1E2i^oS2C9@q-?j0$Kls7IlX?(#o6B6mp#d!&+k77UYY-n@9PRidlNox z0<^rd0_+3?Znkr6k!Q>-EG$38C3}+tMP2{ce4vW^=#E5U3CRWxDur$WHBBz!A4EVA z!@}HrIb1c9(~F>V3`qro2#9`!fk$KZxXHT!y4d~weLk1{&&?}p`~9>LO%5BQ=_m^W z{O%{4kz9+e45u8X3PNca@$ss9Dht!&jW%RaZx6ZgR5Te(alGiwn#3 zZ|ol~yVFW~dILB-os!ZX7pPR_uV&kQgj~jm5e^3T!3m5yw9}E2BB)mS+A10vFO$!g zvjeuHnw@j)Q7$UAj+ei{v`P7>sHiM?90=4&u#n99!N8B7MYACW9n#`paAS2MBNU!6 zO=Z})PIHV)0EHB+3c_AcJ?J!_qljTV(IA>OV5;QBo0m1bi8kF6tU<}dtVtget2(_2}_^Z5XRS@d#na8QtwA6`wC>sU}YGPDC&G&uj= zCZpd0vrW)J)rjzPo>I2eA+Gl|^cGITr!az3>Rvyrs0nGI%_wl=g1PRRfrdfPwsj1j zqUi=sx2rh-hlmZ5|IfrR+gbqpoLh4Nm1_T{QRrLF1lFt zDhb2f?T@7{)8=H&GFe4{RiX3#mUz)J(%1{ma(D2(9!sCh5bzks$<0PQi`MzuZ2ITL zlJ4WBJ&!#C z7tRK`Vry4z`+&gTqQgjzZgI;l6Q{xBA5~lxbsHr z>?Qp8+9oEw45Hw_LU$)k`8AQhP_g&M-O#4dh)-a#5R?`83Z3&Gs7$lahYEi|P86^nHQ8zabd@Us<1@PD3{Z1E$ zD|vb3tlNSi-0mmlGBQZzAHGIEw98yM82f)1T2Llu@zPw_LbJM1XP0LQUz^x z8>{07E`NqOYzz)~+1gs`?gGgE_a8r!k9uQX?2h5pYwGI0O02GxZ*F%3aHILfxw-4> z+9iby*5tf(08dJaX&CiFroCnqmm@GOR0v7n;T=f(-$Fqd4L z&v`cqPbdaS?V^WC66P5OJJlL>1Z&Hqe!jm>3<}2yCL)M5*_kY7XKe1vx_sIsK^>^^ zC!ulYJGG;U?9SJ*NuHij(dEP~o`Y|E_f;=2|G5SRZmF1ZFhk%6Noh^u(Zo`eDV|s1 zEYLV|{~e>x;yJJM3YIl5^e6#HV%fY4(G}(~xjHIX+|<)m1^+8eM&6@AmHi_(zIK%P z>C+E|j2DC%>P$}#sUi@(Wq#hP!qE7KV20}d`N&NM{(p*5ov)nbXCYG$D?c0o!z$iEN05aO$_JW2mb(f>yU)c^YW_1BDTXnq*M@6eY*4yh@A zKh#{)_U{*e2gL7Il9XP_d?U{GJawHwQ#Y}?)l)WKYIE0}-|$-yRF`-$di7GnPt+Cd zQRxsk>c{Sg=H0DPm(TTP1?R-wg=5|1jN1Car;@ss+kv=qb8+pBOG6Q;VCPI%F;rK~+3R!MFD~QiWGz_5L!T1*KzA=cBL=w@*Ws21*#Mwr~jf#ibq28`w&LYk>DUap18V;6AOOMTohz>?qS= z$CCH@E9a0sfXZE)St?i5x*kg+iep1u1)5t%%;mm4qcqcQMdYQUfaL`S``Oc{Ztu2W zVkN6lK=2QdqJu}!YVUGqH)Toxbm57UnUAmd5$@&bfPkwr%JX1IsSZLtQu5lEQcx>g za=1pbn1)>tqQ5V@Bo(YE-|aXc_UnD9kbN;f>yKRR^>24lG6^MecGc_HEcKmS9J?)R zKQLXDxaj+PM;Q`pRE7V*mvvrs;r$ZQ_REj+Z;v+< zNy?Ol$8XE~w_DXljy^r_V0&=O*SH#U?(Htkl$SDW`YR*vZj}y}GAi7TEW?4fa+}8` z$XX~cL-}L-%d9ie!TWdjA2kwPtDl`~bl^V_gCN9b+u%j_iMOFz)*2*caey4b*hH!_ zJ&g1bboK^PnhJAoAyXqWa@;QeoYmQLEgZwL{&q)GSU(HD#0($vkUo5?M&`#961#@5 zR>Cw>mKLmU!^^gZw2u@*d?O#IKqs-~-@a{OtNS5nbv96G5t!U?Q0H^2dfJ0_I6_o~ z{J?T&NAk7a61Undj{efkyET5iB*`W3J)-Hz2hkfO=zX+V;#W%#2w$?ZYpUB|`eezu z=t|8AI?R7mAFmgb$x$IpJBQ=(vkcjg$dti0(v9)Kmhr%$9bwnUHP0CYZo7~PGZ-yH z6MU$NC2G&$g0VRxXq%Q$KY~^sJP$-SEZ4P^S*+*3Fs?|1SUa%f3UV*kr&JL{va!a`ba6>vG8GFh8{xgUvuHDOp->ML9H zE|mk`tl1xq2n-fT)W4z0kbN2ZJ<2aP+OY{A9!m2uWT01VjJD9(pAxvGxL@77czfLh z!RuXQ5(<+%$k+dfRP95XeaTh77k{ENavn+^dLE|SMI)NjLkr?72*_{CY{18*;0!K9 zw&iKFmsB(Vq1xGZR3ZX0zXv7o0ylI4Om|ziOq`|nZ`a^_euCWpxLr!ZgGn!w@d~RQ zdCfU$(%BPGZ(p!H@Gk$5TVU}-2bUeT^jsNQ?>i{tg@VK_9+;8>2&5 z9AB|G*?%@Uj*l+lhSIeOeXhzzp@n){1!T_EQ@~KZ%W6|?$h6(h6$6tgUE959guVw4Hgjik0MKpCpLj;Jbw1nfcTa@M{-7?Q zQBuVOpUInBHjXJkCb?-%OQ)wQ6K|KSa0z`sJnU`Dt>a>@xwuntW!!UuL>Is>0QbNq z^fh=qdsW@z08OYO-KQ9l>GOr(_F$XO_hLOe#7eL)6vRIRKlEDdUa6a&5;h#^&b}OG zc(K3~#R#$1Ljj0vI+=D`jyHD-OW2MG1@8j(aWR=V?_~A$#JB`CPBUI{j8tMWBF|HM@P30pWWA#GyWs{f8fi(&H z2gd4eTY_+LJVPEp4oX0FTgped?)-^xysFMmZ}6tVehl0;Hl1KTP-z7VmnnVcsoTD5~kd*-0H687;Rg zBaY=iSQ_NPH0V*<-;XN*Jmd6{n&obcMB?xbFF?p4ZjK>>h0re!-dqhIWIQg!aRXo> z3_{8Q5NOY%(C4|-^AG7|15RP)h? zhlT&Tu^eQ3k&)f5y5VedPA0{J9Jo6NU=9{z1;EhR$WRm|B0U40$pb88W&yzOOcid` z_4NsJA-ue>bIFC7Zteh%y)#t_OqmsIW`lKGWOg903lPBSYef4l&5P6y>=5i#K;suv zNFmMC>+9>g!lNGv^6M)qDsKF5lyYAp+QOc;GCMLd8np3Sozc@KR_dzHO(Z3o>(RTc zsj8~JO5z3!3AMMj-U3(+R&GuRN}yHlWbn{mjBwCf-1R8n8E9%s9V?s@sa7_seM!mt zCH`HD%y=nYjRqLju(hmkzJ2TD zbUOF8T`uJJ)#*;om||yLX>G>DP~~8s+dD7H7E`vnn!<=jO$^{qIhzRx>TpA5P~^KG zU@xai&teG3Anhe^x_AFFy#Mqb7hz9QlD#M8mj4wBK*ngN8wS!FRhp^h%659drXcsxdw5qFA-5bBw zon2g(0JLrbrIIywSCZyKQK1QB%15w@jj6o2v^2Mu&vkPd7%gnNl{Om?gx5+{WSLbk z3<{&v-tC&K%Da5~H!Mgfk+Oe;{cDNZeGILTih@FIN|g&hgy-rb@x|ot9$`HZjY;d6 z7uM{(N|TRZUTw88dl;EWkre=rpsRM;nI~)}L`?*gfiH#wyKB8HhVFk|!UvQAc+H#^ z1qa6|@HC0pj5koCw8}cGCV|-pj*(jq>q0vEQD;U-;*8>?BpE^UjLntSlF1Sl0Lc89 zMxPKb(0}3`*q!1?;`@c9F!a_L5 zal`R;ILD}RGtp~et!xQa+5lx;HMQ>fOH;a3SA!mi2Mkn8ncgMs@c1w`Eo+4$*sUk3iJf4?tqU=k9_gl#V)sqtv60A1ayeB}Y8|24tC2Jb8`t^Yi0#x71yM*VrE# z^k&Y6xgt?D`@*$7@xTj6E;EuS1iyB?y}r8I-1l2E1h7)}p)#6Y0>=qJjwUjSh1?OL3|czG>1 zFgyKyxB*$-)6QSP(cYdV|I|#9Uo0^M5+J+_r4YGI-Nj?8+8`4BrI$Wm@ z3}@1AC#IMO0b_hQ*I%9><6>c9F}JW@Sn^+6b^?PPDMB>QR+D~T0Pp5ArSw`eTC)rp zd@fPyqNu>I1|2YIDmfFW3_V-@2L3%fcFEzjmZ8~2&d4D?c&Z?SapAAH0~j4#r2Syp zmjki>WEIVeH398hsWE{E;{(crgM%?|A-C_wu{d|AIzSjIOREd7 zjPoJ)iF>@l2fqY6KD7X%^rVpK;V1eOqUN}tmBe%+Q(1iMxpDEo$WKpIQ`omfHHz5T z+ep-v;2P@eMmnQES;jX;IIE3_zmtL#*;S&OZLabF2pmT&>+~$0ohc&(dBFy1BL>Cg zhq#1hGyZ}bC{Oq%IgYJ2gS7@x*IpA04QHTFv8kLW&>2STd8BnKry9JUsL=)8xj@ky z++vdRoPOb{JU$2T`ebcwSyoz^JIC5wf}Xl3eir|j8?y24LGpISKA@LKfi1Gbk7a3R zl_MLs$v9xuj=%#MFcB{$C%3NfDP4rH?+awxF;e#OOLjk~<`ECRPV8-wi zLy$#hfc;?10NGR~N*z!cqg!M?n`UqGdorhRay^#qHlJ6kcfUbH+*7{S91$1H6<7tU zB+@H|11apkVnc(X-i)kH2ZO4xXQUG3mXWZa8{vC$x~oy3UendY&;#`;HLM-lzgrog z`}y*jwj?VHmI;`M%i_iC7O1p0SAnte# zi8sO`i29p3`u|-W6EFtmVMnh`G=~nVe@@s3hyj>PIbYVS1=<9VwFGCZ$wNQ3-0gtH zCULfI`y-x=UWs4HSRoms54M6k7oD2=SNNq}!_!WTZrSdehl1P;3z{h-H;JZ($wddw z@cnA!bB{k9Fbjc}&Hn$Ixc?oJfC;|+FV*Ju)?%RPFl+K3kMc+yE-rut zZhjBk9540orLYK#L4Qb#33>vxKJ70Be4yK!E=RXXZ{6THILyg{rNLq|!V4k{qzFs= z)1x}m!2iEVmdsBhQ!Yt)FhJu4JTU7A0(t%Oe=2RF$t5Nl9r~bW1k~NOyO4cf+0Tz0bMs z^V~n+em|(sTFYl@>umeSr!C1A`_e`W^xUgTx2}1FM7#2R;$S5YhpE zQQLh`vD3G-ayBzGvV##Zv@o>MwKFsz(Q_vGXlG}|$-rP`rfXqmZ*E4fZ)yINk%JHh z27cB=LB;MrpTod{*El5>#?4sFyuAe-uNqXODdiQ=AQw zp`la3`I4*w{(9}UYr?{H_%a9y>L*f^iG97_*=rgr)=hMxN_VRqT&JEU!K>0_xlXx{ zG`MkucxmurYpj z9ATe+6jlkYaK@BmU|Pm=CLhHzQZVBr7vrnc5lP)RwvtPzDsKwbkHDqE~+P1Jxcg$J7>SxnE6yBdiDgBBJPbT&iGJm|g zdXx4}t&0l%IYhi!oP)y454t@2jXrH0G&=_k8s`g60cxMm1F_=%&n~Ah7R}aB ztV#|MpM5RA?Q#SM#xY$(&PrHgAzv|Z~aMrf5+=*Qv0jpy}t zgM=pE#T_8jO>MKO|1MP2_mT=U3}&XyQYr%6bbZ<)G4^NATfiyU>? zG0VH>6~<;%G0tBXieDxWL@SNWheo(<_<^3#kNzQ zw10Fhhh7~`wB%o$Q1?h$EX_ysCPw$nPMqxM{G3RfTVI^R8q!aJq`htpJ3~CQ`(zR# z-RzC@Eg*sJFw50}f~ef)vAiw@yTF+T+ABfq7WiTP3&)HyzR%RiAJONsJABcR25A%L znkQdfVyM4g;Wc}kU<3o>1ta!eK*33CckWS~!q^<&;p$mm0=L+F+xyaXs-PfBIc#hx zrajAJqg)9F+CIehPaij^$YRQpypStReJX(YHQ28)snAT9z;KGwEMeZkh2eq4HnzvA zCu**yjK#JoWrO=>mg8d9rGJ)dqpHfS9RmQ#Ur#u{wk{a|p9dKQmij;c7NtUf{^#E$ zX_(=^k?i;5Yb5z6~!&qy2U!>X_~U-4mw&sH9RvBy&o+ORk^v_G_o3^3`uFc(s1d;&C^h(liBfep$R**P6D@q;F3s0neQemP@%L}MqCSHqNEq;& zB2$cYzhCK*XNE9rWD2h%|MUH*H~3!v5j1B~N>^^_bHMYj_+0Btei+Ki$_Fz|2lx0F z#-TkLm2Rraj<}lDm6b88;ck&J^y=cg{{6nvY4K09GbLhTqUp6u-G*1izGX{#oYxrR zpdxB|g?uMx;Ikac_|o~LtgP%YDk`4-R#=MrIsVll@|F(`S)D;O@Hs`5h8TFP}`XG#TSX-+^_+@ zhx#r9PU~d^e61e_D2hKSD;?T8pRQ&=G8J+a47}V7+Mt_L%r@!q4ohT@xi~q^*k&5t zZ#p}0Xq7d-<34_i@>Zxqt-)>za$H(k%4GLO(`jIyM{`b7I-BM7>wr~DPkq;qOpR;8 zENtH+4GK3EDYk;zWf+yhju%6xgBX$w4E`}AC?*2bi$I${FFQNn2^V4LVA#trV=$A9 zj;~qb5P_7eEXwVb8rzM|4#S>Uzy5$1MjP*0%x!I1&W4Q2BEs~4g{JY^q*JCDiz0P) zMCU8{hc+l&zbc-PI5zG}{^0(kY4+nUVfn}coGVeb_-m*0UDf`v8SQzYyetWG!&Ugd zcL<95_awSkj(k1r>gLjjbC-Ip;|<~(7eY|$!X+KV9Hd!Ch993@x$V|4$s*#hBr@BJ zZ>pH82Ku9%r`*IF1f>0@X1(23f$x|@IbGvFKY2ghBxycPSebw2xT zeFT#H!PYZG_OXQS;Lp3KZzY2~)EvD0lUn(40@q_2tXDcBbOTxi{3)=+HmS}bNNj9w zV%sr1*Z#bX>gRz;ACj6hqO(~iy22~{{B9!WDojai7j3ZTp9~G@y^bUPzR*-OYEC|#<0P}(}JQW41x z{(9Q_``Br5Q)5r(MOdwWhN6B1T#zl!D~bF)9IUd5K;@!fG+wjqZA$%(=HjE>Ngc*q+(t z+=w5=#@k&v(f|8UCrPrgm!%+u3;$lPt9adkMtp$p9}g>eE)shIznT!hljuYY4bqCL zf}5!KVNvC|d72{I$jYLm|9vv$uNj@(`O)AY;p4ww_twe(*jt(+DcT^TGFU4VX7Z+`ZT0DCQR7hyB_KV?4u~hmJkaxDT;1kc0|Hh2jc#F{PCrwD;$tgm4(;cE3skbYYLAuKEmi6-f17;E2k1Q@}8 zZ5=c}@F_ydfHv(=lU>>*q zCRpso*qm3$Wg2GYPZT7xIuBPLtvqK?)lgK#;f#rTE#H31o%{7Ee(xJyax(!r@yfFx zt+uzu?5|?zNhqx_Ld(X9%z{$8hs9{!L1O~bE1w8F!%hVQwR6{tyuAG9+~qo_ zaYJB_vQDOKk;$@tK6#owwq>tgWzpB&{hiVJ=l0wDo$Z~R+dyo8k%Js+`E2g0eGWfC zfemdfMuAS76qQ>9ve&2j@1ORNks$u>F@o{al+shK#n;w8+=hgNDCighmM}Iph8~cy zY|ws*vVz}^%xu25)`A@Lp4onSw7Xk8e@snL@q<4FF8OFq;82Jx6OK{fyS?wPC+bi7 zxm+5O3k)p=sEs!Ye&U*BBhW~b25)1&vH;1jF19RoxAUrP*r zD&VvwyTf&L2TL}ch07)fn;XV=AJ_kezUJ9PYJ^D_p`b0B`Lx~UAnEl=i_fF3*&JPF zA?9uG-A7rHNk8sF_ADG9q&}aWoqc(!8O1?=(t?Qb{5cMUV(WeM*52M+iEisE^|#m3 z->dJh&X9b(yDyx$>-Z*lKAi*3^Tws-RDn0ea-q?^Y@_%0(e^MjnN`i?-hwDMWyWS2 z_e(3MXU%12uc|XK+gR!~{3@Adq^*!(E4xyD^c%m3fhrs*zlXs6XT-;y(kk@^*2c!h zmu`)FuhzndxY+d9zdj`sktOaTx83M7EGAvZiij+>^h_$yb#OROYYlj{F9t|3Kk{a% zHy#OqXe$H+7%#i_K$6)9)3Nyd`x0dH@m*B~jjXDwSf*faNr`pL?<}tUW)*5up5(2z zFXM#JzTp?CmI-6OM>^k~6IwH#OHou)+P33#b#c*}y?vz__agO;Ms-PP>Clpu%9|MW zd8d_V^J3YnoAU$p8k`jIXB;--7fXfYYr#b)oZPfE1UP|K$34R>OQ4t7(IV~bVp z*Apze4VbsdLP`hchTrnd0o!jtu+IXUS^$wPy-yCHQ6~do=;K^Ozy3|P+ ziw|t3PSiP5TntT~cni*`EWZEf#PS6`a$i{W>!%=>A^jhKBKvH0@ePqa?Be(p69Tle z+yFg|W2rdYhBIlD5kD1Pac^&LXS_Ip*}i+$+4hsE>E_Q-n9Pa_`>}1sZb zrDQ8;)#?R)>odwAK5tW@S}=U?M({I`cxcnDv2gJ*_tDykIj*)dAd(JIkW8rUC0oc7 zjp_2DqCkoF{HQR#UjGK-ugGv4EYQt>@1Dn9uk9!|g!-li!c8qLQ#j2{6%`d1>0HOW zt9I?}>|`^TKQp*zB+Hzx%F7H7(5Y2f5XEmYBX$W z+*{1OL?(b-oi^$^>2g0v!PX;CmRE5@8tlp`qsVA#n5Joqdo&l=GSvFL~W8F24EnU-PnkThzabLSzvYM z7mw<;q{t9o(Leov+kB$sRDN;s05THvmkge^nv!XS8m+WaxUMi0-vr>*-(JhmtCai| z9V`tEDUwp2x^c`UK3bPdX1+R`(9R})tEi>bco^Xa$RI9HZBA)vL^rq9B8>efg0oUU z6X$P%1P4g%^>Poxrd!p_-F?vo@Q(<4&+#R)fLuwJf`JV(zYV^3DZRI)9mHg`!fO*_ z6NOr3-_3?bP2}0lCQ~7~Ic~oMlcYBiEE_p^K5>c}cs0g`SPJ1Ck*KwSGbR7$Og*h4 zJ5rpum$M?wo%bpxIvsxd)>O*V*g&smFvzfgNu%sdP714v*=ROyDDj=2| zUfqsVDXyh#HNB@&5EDZ=L)Zr?7f2?G@)~!?xj##O$U<`ttW+T(`2gtV|C*V7ALT@d z^N(V}ZPKmaRvte3@T%`~9!38l0cAXlY%Wml_+ZH%LQ*(n(V@vc6d0Yue^IO4o}Cju z=f;C3j7NfUX+f(6X50Cu8-f2Y4i^S@d`0sAat@F||L+v!h@M~2As#=}azFc!UHo#p zW?kh#W+IyL6|z?G!BG<0m-+m#bx!KP_)rFhjzBGtOaAvAcwaguOzgjn>ewX>a*AHR zH=3K%CpD2zuy+4M!ZT(THs%htjM!HApzbvRX+>)P#0PZF#_CBMT__pgb+6Omb+?smq#g?6N z>1T_41((#uCVY7lMS0?}pSh>P+3}`V3N*tAjpzz_!(6$=DA92{urOPQe;NTMc0d(h zjzZpJyJWuTN3X=8j#Af?1C^&8k-`djVga`gia9xr*J!xJho^-Z4PWPP%GK!Jc8&BQ z** z=q^cnf#q=Iuz%WYVB*ZiYt0cO+f*(omqO@4trXTKhdj}GM9bi?`3G+A?8A|8IiYEBPj7s?i?Ysrrii?2o(=2Fqi$3!Qn4U<;8?O)Ii(1IL*TRkB9FVy2v-;1RsqT3j1C!WWDX<4qS zyDr{NZ(1aZY5s0EWrdI1-27)|LZX>VnC$Q+!R`g$x0xJ^?H{)8c3-z*1h4w8yuUCM zy_*%{Os~R0@neD!6_n@dbN)b+e_YUt@Ep^ia zmMEv%-52b;SpNPeTD28acqNo3B87ul8-MWXzIEK|fPjE@*q4>s+SK!@KFhv}9d}hk3T+Bo> zu|>nQ&wVy!a2(r+K^;Pd9Pac{`Y8v){D5o!s3-hN<6@?|C6eV9#nzIJC~ZEQ6*Qfp z0UD}IuKA6in)GP=i?WH_#R>h#Q9q=zP=o^>Ge)FWsxOQq{X1dxKK~I3y~y96?R49P z!rRQKw=@!N#f9WT7Uc7w^ui=ORa`z{y4@CoJl1ShXDv;2bi_Sy6GW^Dn#`qI){8aV z&Hl)6ZBM^{8;lBVMhl`_zt1n{MQ=P>oZG)oz|7To&haF2WxhBVr}gowGGYqxMaZI| zs={GX<4Cv*<(zuU>keWC1U(gd|K^P4XxtS3N8#Lm0w1G-=tR!=;;o?Wj7LlTwcEn( z_5Dsl5jTD2zT1_&^Wt)rUpD+vR??hz2bS^$;!PfaW*r{`y((%UdMW$FBqo*IX|Yd9^ABoJI(923H| z7Llfr0~r`h1vHR=0#yDxmqh?xoXk`knzwsk7eB#9@Poka0x@ObluyJZU`@w<~(?NwoC9v9W8=h&#nM`^-DXIQnbLNew)&r7_L-KfUs zsg)5s6&3Z7bnUV7i^AK|oR`k86DM}cUYgh1>0hMtl}W~^Z?QVgZLUyuOhlgLyF4Rn zc)+ed+)jq9Ey(0`O=h-K3DtWtS&SLAP$)N}{RzCKkt%aFrZ?(I>o`*G*ru#aZaAY~aOV7jFFJpH^bsePt7{<+X8_#8g|s-(F$mK*1T;GLUT(MSrF0e?AQo4> z+Q8jbYFR)awzF?vO16B7reSurVKdhDqR*!#b!)6qjWgq zbD2fGd`QxYt>4}P7~$v0HFXPZp+9bClY7C%PRk7ZgG$kN*$n5)&i9Bq# zKIH9xW&>?qOJ{HKG>OFuWs=z?3x;m%MUVJI;4WsRcrx89+xd?_p-JE3a*q9$V%k1R z_ZdxGKX0Y;^A31~ld~5ai-`IB%%a66Y2~q>xk#+Dd%m@nkA-CP^*+{Gh_g2Vyk+ON zyW000c{GQtwA*m`KEeFe;zlCXG+Tep;eGnVK1V^hxjDO3@z}hzpulxFIQ#IW5c6LH ztCf`B9xL-^5+guKLPc$B-KAP+L$>|0&n7my8to-r+LyG3PhO)iQH!!c>azP6xwC#8 zY;QF+n`%_7gGgvntIadYQqO;%Z8KJ`wijvZ{;a=-jcg>?O% z^w+pG&!0wlBkHD#avw2F@ZzHi^5V*jKymcdQm`^B$Db zzH1h1&X_qB&p=X39aUzltDXME6muok4ha<|lP%b8Gaz?K`orpv*I0JSSJ~}?#&zw; z;1KWr?f$i5wrVPww_1SKgrJ6qEx}2O@+B|YE50rdTJc>I*ARR6*=$99&Q zTL^X8+d}$c!qgMb&4QK8eNXhDgm5dmBXfa+@=hw~7$B!6PmC=kuB|Z4R%XXK^_b@K zoNGDn+x!3*4+s^pYOa=@@PkdjmaSh%Vk2F3a`59nppc}CqoI*GN#C~4+N{F=^wdKO zsxp`}GpBD-RvrIApluti|2GEJy}yD4U~$uzB5U?z5w;p&h(~k2t)%U1fFGF!U@N zslU}&00@!Pj%pLt$fkVWvACu{pce8Fp}KP@s1D4N@E-w{AM5z~cFbLRxPm|~_kKaY zp@~+p=;Jn-I|nx^>N{$}WZJNcrh{Dq5tk-+NzOOQY)t|W#lmNMpHMNwKUX7%SVmGh z$~W!;$1L-Ybr0WEH=gz3C(4QFfcRFZj@rh(zM>_3=K~ z?|d652HW`={TB(aeX=}(gr>e)UbQk){@$!N-y^#D7PCOnsILtEtg zc{8LC;u(Syl0IA&b~;<1iKQ$mpiy!FL*(`-o)G3sMJV0gFO;|6^KIZR?)q{@8XZ~` zEUnO8=3%INS5DXyG_|SBR%e*Xq;C?i*4V?&BZ*M}dH`Q+m@&^C=DHbPn~L~@;dzmg zR=Cb@vl=VpHMw#h+C|55N^UUlUOXak8DJ%7v@U-z;t{^~2tAFUq2TD)quZm*=7@ks zI=CxxvB(c9%>L>Xn|S{saxjeiQ$@Aoh062f@dW^#p&{{OFKo;Twrqp9&naMdvGQ+9 zhR9++teoY7gW4!_F7_$bFS|LfVhl z-7U!vSMQm3`~0=J6+2%hc!&*pDNr@^NQ8#de!uR7{Z_s)q__>XQoOqFTq-^@Eq_C8{z7F6{c*h3t(B*-9^$7`GtQP7RsQRL*XZ;+j80a47e-6=iAjD z{gh<{Ro4SP<{jvZzA=s;v|$pK)KL#B(ChJ6&L3H~)jOa~^m3C8yk&Fl;^{II3X?^i zIpS9t8e2HH2P*Z;vI8L;5C<^-4CVMe_UNyZgU~TQ6Q1up>DT0Wek?%-!1Hd!+Y$L|@lb)~+m{OD z2l~^0n%~c@A2pynazLYW%cxzMmm78~X%GVw7}~l+HWw6cKYj2rHZ%`%uwRNdr!_3W z2Q8=LE+p0>X=2ho2VA090`7sdiA_1rirl1wOiM#+!LzjJaUCEz4(Me$&3HTfndQ!v z5?D4a?l_zs>x&Yi0c!{r&hvw&Z-$or++hf7&663PRT_Usi*Iq%(E3J8pj@arV@49+ zAcmbH_(yf1Eop3JW=#J|q9*wE*!A>lqr!&H?`h6#49L%Hyxjyg;G*rfokU;ezw!L5 zQP~a%XIqKiBF|nSyH~2tp8CnzU0Zwa9+d_0rj=#Lk*}E?7Rn&RPAF&DntgnGzXuJz z4lVe^^vr~iO~VJZ`Ge5~2tJ(?27m@nYfPaz|LDqQU7ZzZ6cM^C6!>#qa?$fwhbDIy zVG^&$KcZ8cJB_0}QOTUI+5CF{U1O@juv?jeQg!s9`?**KS5A3pD?cA~RFgwX+Z!6j zbJOth8lItiSODC<<3}1_mKfIZ9(cxYCQ!%xl~%-fUIwB!=Z|dPs~Bmz6di!1+LCDR zcX$aZyT-^IcaRV~=>71=T%IKNt;`G=HkPRzmAz``n7L1%Z2#6-FwMim{abNgs5cMs464@E*J^@UaW z?^SiZL_vG{@X+T06Uv#67T@BsCQL2+W3>}#6O{7ZGe+EgQC-q{{pl1wL;5ELybJ7a zL^yMWZ3RKTtK-GZ`|va~#4e$jn?7kGmA1n?IYP2!qDeF_M_)8)fF6 z7ECz~ZesC5TLL?f;Cd*Kc;?7m1oh^12sOg|+^%#!KUeFzSObf>_&nj>^onT=AHg}| z&+?j%&Wn&DlZt6wj$y#utG^;5N$T12&6!;aB=2Gvc6&Oi_f`+EZ~_+`97>@@znF!P zJeZ^{Cy2Oe3ZBONyuOZ@2T^oZvEX)mNCGxD;juxlO*le-0$?nRA%)|=lYoU->WKFuN-3nrLc;jKfrS7p2o!k)&Z%OJY0gpJ9%YZ1h9$7cyEk~kx z4U#5$We4NZ$by-w&@<*90cKL|X1&6dt`nr;mPQ&8&V1>*s)_E4UobGKTN?8ZodqCp zS#-yuVQAT0=FXbp%~AyXiTaz07aStWx!^dP#!Pn4xF)Q6XH_@yg0ylTnf(t>k3O5 zOOeT96gM?ttOe-2| z>(OEsd@sUTl0%rhgm^5ornDZodJ;b?%xQ#LZ_D!bY?2$~$|8sicX)ES9{D4ZHgSG1 zUYS$7E@^#Emhs1~9htV<@CTTImoq4TByyNYSETkr$snvJVKs;cYG#i$0Ez|97=&)_ ziX@H~daI|`_mCo&qV{IOhyu1se6WY*E8n{njE*jOzs;<(ZFb^bu~T?&q)sgVWcQ-# z+)H+>qa&joTi~HjQqAT`Uv2n6^v8hBVx||8IYYD7g5Ci^%`l1Ew$k}ykuP_*Puw*t zs~dtvS6`ZrO63h%Iev^B`^vdjz)8n~3&GqHkKHC*C2;?gcKeHaiY)ug*_2EB5dq9c zwd{*M`V;ML#nr^iO?>y7{uZh~s}DM^O&!c{M6TWPpC+LNqL|P>!eKiua|QHSF$hH_ zB`9MLw-;nj&{_HlS9ib>8wZUHLcJ8mjjSKaT2Db5bP+1C!M2G`YwpyC$p2oG?c$61 zFD_}SPU_>D=RTwJo_idBa^tA>l?3_Xx-yj^gP$zfq(}fe!R*XG952Ms@I`!Z(?e>@ zlGO)3GHxQ(^{EdSv((j(0UWk*`mzBUKC2UdgA>TCo%JBOIiRc^oVlKU{n~&^JECg`9}1cm@9E>%Yek zsI3F}m+AH4txceKRbLs1P{7qdHhG97ycF(n>-(aQEPG^?FeG+AD7aKlzs zs#A%aqu>sIJG@nTt$q#NGziwf?Jg!v2&{Rip+tXo!N9x{aY6pnPvD^hZRT4ed4ZMh zDv+_CN4O>5hZHi>Lx^TwQ8{%ufvU*apsoY5D@v`}*UXT*jWzUgK0ZMPr(z4N- zF{g=G>f9e9<2=G&6DVfV4?_==reU7^maRZ7uB~_PaedSs!x~M};|RpS)UjocR4ou> zYKz)aMSPylzh>S!gsb6fNcR9{0pewd(^fK75_?^;30t_5`wdDvFf9uL`XU5m9mkGD zt%VyNPz$Brr;}gL-6J)Gh2VfLix78EW{$ABxs{hT`8Dy;>Jggngpu_?&In*9GowbV zwIejVk>xMG7J=weULW&*>2NlY;X%Q|FcfBpYJ}r?(vWr%+1;nVxClxZ-y`dlozMt| zUm}p5fiKTIsg;*ZsYUI7ucr|-;y~GtZ#;n*RBal9Jw&zmy2=&MW{jQ)3k&w;Bvq6Q z2bTa`sBNb|YSkG2;UK4+t+RVTG@A`&?Qyl8mQNAf!6rf}>H-lN-w1D%8c<>vl+%9Z zn~~c8?r8c+3=2y_=j?V5BBA8_46hV$rnOUtcPqnV8#h)yTCXCsuC-|u1}gF5$cB4x zQD-nt44Jtsra$u|oP%Q-2!7V|F6}b5%{O4gd-DAV5n0o$!-a|k;ISDH%`YE$p{bK_ z5XyuEH;V8`(i)zh4!EDvL1PX7_xPDq0{Ki~R@8SK^}NFz^_0rzIvGW{N1uINSIa&0 zLWG}a_XAUuWe_fQ$2L^MQoHV)x=}fRRNr{vDJ;=Hfjt#$=vTpR5T%M+{ zUO2>Fg<_)~aYHt|UFOQw3RNnfr|a9f0;L}afPGiTXobSQ?WB9{(?oz#ujX6ZhuU4$ z@YY8mLE8D@AjB~aWek%7;s&*j=0n#q32|fC%5Rxof_=^0i$$8B$+TO%;9*?L+p9{E zy{!MH62GFC`ih0<13WS~W;~OE8*Q3|J1<}&r$9*#gs@%>Oxogsrm`6u zcdf;)HJaI<9HW4af)pUGM$bmlu!&S=++0ZB7r9*e08n6{);BT8dr(y@OH+u#TW^Kd zxZeZy5~wxZS-cqpxLAV20{k@7UBCNQQ625=nMB%9Tn&FBKm%Vpy#4By_J;L!+9tBhwodVZ>`7{#NZ)v!99}cFGL}LaBeFVkV8N= z(=qx4o=m^lr`WhI>W%<{9`*3MxC~f`z-*m9kfG}&qCEL>EpSKFnMu0PXh)FGMlfpr^SzsO%aKPzIyYRSaot}NVjb2d~ZgCw@9VtmED`Sv%}Mo9Q3y9 z1@WMr3Toi$db8e_FxCE(ArzZyxwVS->(6@~R#VWEED`fYE-#8EI`~O=*pfcPzy{(U z(W>IT*@1Hgg=nOyLn6s8QJBER%cOh>G*l@jLe^{Q+RH zuCCU((}tZEH5Ibb=o+AtUanJaMnoyEc;K741^c#!(^qsT(o(HH58D6;-XOeWa%%7) z3z^BnQkP9}v)S(E3}u{N^P_{}CQFB5q}#FeRqz@(czTU$ew_WC9WvsLz7!F6u-?AD zzEP2cK4IQad@f3`)OwdbQ)WqR92}Z*mCJT(JrTFd@c_9vm0C0EtI5&P_wL+|`-E53 zpmVwwhJc#UW>i!dT^})>SYnsqfOKpq^Z6=!z7;rEJewEU7t{I*ch)&yfS29l4C+AJfU0M@6dY9t;*w zt}d4uM%{8qaN+Cl7M9p$_h|8y^yRYu>t&868w(6tLLCL?Fl5k>h6BRN8y-0Xrgb~*o?k*Iyo)18pPdF zv}VhsWC_cW68N<6!FGs(S>3!U~$;u5kGn!j@n>2u;(Ds1h-on6<@6U zt*tlYkpn;mm+)yG5K6ijY=FmjKUTWfq&Z7!4@R|XjU3p2*#ksX-uEc2F4;Y|C$A9C zv7SWst+?ioxT#0nR%LdBw9x4b8F`woeMc~BC^1gw#@j^*H69$8;gwq>z+wQ=B4I=n zo@EjVr}t|Yb<4k4CLT7Lcnbp!5Ru+V;U*T^ zeL3tivElha??nvR+_Z(@1TBiEuwyOV6(_dj8t;&Q*Puj(&iEpcBxdN`gERjex!6kCj0%-fGvH<%(xQ z^2ZEFmWBFqvo+jLheX&Cq-+EP8#2O^H||*WU$@T9&DmWY=8J4W)Wt%8E9i#evG&&@ zc_~cjpMadRolLVmYihT)>doPyO%TonCk0vjoUMNHZpn{_AD@s1R!+ilA>SIYZ4sLJ z$M6$JK2?A%0JZdT>BLv0)Z~p^N(J_})%?6w0>*YL>M$r+0bWtk*#sP>2EvE1Vg~Q* z6h3|0sXf~PouAAuy_>r=Qx9%iDRhN&qu|^qByT_12>h@f`DHSa9fHqg1TMR5K1yT> z_EROyW|T14D0w_OK2ERR_{LU)QcNO-p2L1yVN-?8Pw)u9zH~CP^D;CD>SkhMvdL|0 zYYW;vH`FQ4hfT1xIj%rK4)| ztA~>}p^S=8TOepi@*h7>46RezxKLeji{RUSL2?)FaZpjiT(2g|Elr^57)u4vvCe!` zm7;hTNmHpXCjM~*5E%e9vKgzcLs*tvIs^?zbwDMl5(udKsEdrI>$n>@>;Tg9TqB>J zFP=u;C7`t~KVRfY8=VIrh=?IM<|o1Qe%$mHk#ZSp*(MhmqJhXG}aCTh9+JQZtx3n7GOzA@Z$OxY7vry2`~aL$=8tBM5^NlBv7 z>vD|Zp7-JMMPG)om4669e#ZqU zgh&ukg6m|kgmTj!+FA8?zRP5}ILhnZR*{8V4x%P?Y|51tbG}L#*3Z=W^Qy&!8gaml zj4oVw>fB*{E&`7WJKZj4w6h@tf?f4;)5u+LTfisH(YeW0BUm{TO5xYaQmJMuL;aHL zJ2S{RXF)c0!PMxomrQVC;3WByT(W5T9;oIZwF0N-Lm`{o7DzU9nt=@TYw^xyD{yqzzUIe<+7k=A07zTMU)e4a-x{J- z-hi`I(Qf(E2vyNU!H_#3aRBFduB&Do zi0j|&6JpVM)Amp;QABsVCoje9OQE4agT0p^)TrWpuE=1F4e`EodgviBf`j3D=rabRH>a@ixQP>IvoK4GF@a@7SXA+gi?EuXXsT^hY94G^Jlmuk_7le+Qa=y0q7kz1q5!XF!)m?VT@N zSh!CdRLQYmWV;d6zrYF~8u=yv^={BW^Y|^Y0hFd<>){k_Heo6*?W&v?O0mc z;l;U0q2IkAS#I&Op$F(s8dhj-EG43wXp{)slB6(`qIN=6c^Xyiy+y?BRSx;^0-wV{ zwkWmipA}!hG>6|9O!P%S zbR{V0dMM}hcHk6aiUm7#fTjZwI*$i`l5!%8W+pg|$B13i>`G)F(w%QrzaJIOnCUtpNzQ9u}U9JC#Ha^>a?E**<4M%T`j zVykt47MwlY!w0;;C$PD8*u|W|6K*g%AMIzjfs7Sb6Fw_xu>e{-crv@`3Fyrw6bC>v z9HdC_%{%$;mg0`B0H5wy&@=`#Nf!qn06H%Zcv_NKUd+LlLyeymR4CH4Z~LJmy@N*Ko@4UGLaJ2PSf-GW@wwre&$OhRkxrZ%f2_UcT-0 zrNNV&c9j;|_8UcaeKxFv^?hwo-a5fUH}kkVqv2c6vySifyKtggQRxreqivH!5`xmj zmeLLTK%3DxLUZ${B>B+yi2B-29BYp^%#!E>AMxlGiTtS|w@yw5RO<8iJveL7(P~t5 zI5>}#XrZmy+%M@*{!u6sDpmG02TKmoM-p!AODFbs!}?){WE#SArPc}F=-Ecj6rShQ z=8#kEc$GSRu}_*`mOwHRJ>NwL%UBMZiy;A^9gEs+T78;_T%<^4!-tc|D!3x}(G&Y9 zUuxKjrXRV#+hSOhojzJ_MPUufQh#J~Ls9gK@INtZxiVy z<>M=+Voqr$K>b?Rl@ff<8eTFUS%L_QOfw!s_{s527ii)GJbz)6@H25{crS23htmE0 z%W=8WlOTH@@0b$J*b+p6Q!H6(mhJMJOeL^KSS2Rmj41hizI0S;s0 zB?*i``*fuKUMNM=iP6L%+|~CJB<@whkI14Fbt)H%ZaY-hS8cV){1THz5|f`vTu^eN zO2@vFb(2=}bHXMdp?oWumYnk!3tcnJv=H zf6sy!U!t4wwS@fA1&;b(OZU~588yLMa?~XB)5hHXDC%1rRSjzYLppX$wtl!CBZKo0 ziOcD)8HbOtY|BuZjF+iyKeuThHa~ByXp_4{Oj8xiLtcoS(%TEo#4H|oxKzEHqY$sa zW}+^YDDsM$*qN~@G2D$gzGb2Ll_P8L23*2(`CyevywJFey}%jl!RrP#A|%jqqMc~r zv#f|vU@>HQvjfp4XsoXn=jB3HT0`cm#_6wIjfiPehkNnWE9#heFq4;R60=8i$bDFl z$J!F>*uY}exM%b(RU<$X8@R^GB$n>=j6;n3pWA<~tBrqO{xk73U7A(aZL11RPr=x5 zNte!OsoC*ut|1v68eA@jx~!#F&a!&a5}$1Qd)d#7%~8E=1wOKQB=e9V^dqLBdSM%X zdJ4DkdGHPCnpNd09Cw*}`|FYS_xa#@e-*R}lGc%&7%SJ_-yZ;nTRtzzc znxx~X2l$k`<2CC5lw&`9 z_dPoLt?n|yanFyU#?Wr#Vlm0(`Cudl*oud~j0QH0{5;>+cCIU*%yzq419KUL+DumF z=O+>K^7C(bsU9>(5q9P#(BHbyi zAl=>FEz;c}jevA_Bi$g~pdc;Xxuv_i@7#Xh`Q3IdtnbhI#OGU&%LGRm7YFDou4?TkT+8F@0Q zb-I?~vg9gOF&(P`MAtIS){uUpHQvbDH!tn&M#cH}z4yf{xK5%eAD&v-=;LZh*J3K^ z2Lz3G&n=ODqKn@12Yg;dSve|wWOi2Kk&kjEM14fh3+jCuAR2(oU(wD z()b#AkMpXA@ExK|GCg=CTf&$zhYulrAN-O>`e;^`$Y5P7315VA0 zOv!sWk-VfTvr|*ubdHUm%V?5^mE`zTv^}Wt@psd5sEPgwf5luyF1o3-x6nM8PfY>_ zY*8XgZCmW{UW2up{%a@Q>#NnsZRw#$TxiQ`;WCf=Wy}4=e3k*k?Y0b+?{0+X$|dCA zK2u7u`b0giS?j~d_85^htTjgU@5Bz;8!Hr{eR8mznYc&)ONVNyP$0_t?to-^o*x-} zwPwnsL^=#u8gfjJ+ZyA2FxTQTyC(Z3K`UBfqr+kioO%9)>wSHn7q09k-a*Pk%^m|2 z3Pw3bmEW6TqZ;zPGunyUx3Q$!91VI+$mML={Tqi&G~%e*Io;O3oa_WI<-zwAJSri% zLrwsP#n0jPQ1Sb@zdf+!ENy+)=HxXAIhG=nvB7t&6nqnf_)N4rWYni~78NoXOv!X# z=|VFck~HiNzC4|HP(wX`SB)+>a)H@aX*cEN>3N?mh82DP_{SF>DHO3NCJb|Oe!f}1 z^SNT#YbqJ-$2+&CC3siNBeBJ$Em&s@{fhl=89Y=X4(XSH*+?J)u9D5FK^tc+z5X`N zCTiAZs-$+H)y&Gzq!PLDZ&<*gm833;&!}MLi*7mn;r?@_;V#N9sP?l?t={vDXl(p$ zYjRpQkNJ8|H)-di_uwp78nQS!I+NVZqqm`tG({uva!JoSy^;}UK}KJ~0ql=Nws&{o zfi_wCxL{z_i*C-qWIO{O8y+0yvue#y+ox6u)OLcwCoT#}ni~tR+yo!jH;aO_qJK`l z=v+IsQWKrA!Q@aa)vAxcMPUvMO2xd1KD@-kA-3RhS<=41zq+7-dnw9$_OyBPJ%$67UgmaFCok?b;JRRNil5FDIjEr>2e*Z=g#{}=V#1v z7JeBXI}&a~`o>+2@c!kV^^CS(F~Gnb;1!nf@J#mq+wSHl2l%2(wYpZpb<`8OS4!?V z&)Kz*46OQOi9y?Nmhd<(V?4@r1GMewG8HQC2mexdm+mrRV)W`dBoRjf&`XZoT;8Gds*O{a0~5ZLnM^t2L()45Y5??qu| zJ7K3snczIarmrA8J1To6!v}4@;FVWD)i>Wc7$6^&Q5ghysal{#d>dq6Jq~VNSe>#L*~uO(iQI8R;-G~M5oiz-RfVojkaIlk4Ls+*-{qucvQ#SXV5`h9KV8q`fenq-WI^D#CwpEtzThjYe~H zcl|E=rNf8oy)Fi&ATFrP zw6puAh=@q;6zCKe^CDD;9ckC=BX{fzs{Z7G(R2}ReF$J^aw7tXsXG#6hP~ulYpW-i zE})bz3+*tMk$DLnZBSEJSFYJ=4rW+PXh!FYKyUt!SC!F6wV0y35S2G@*5P6Q&1FZ> zfxm$58J=SXfdnx_<5pT<|MA`ebZnoU>v>o)N;@6xOPCX%k!@?qCb9o~CH%awoKlY` z;9B$R6x#p1>e7CV%0U*}XFVfM5$tSk|7VcM`ueDYk)LpVQUv>)Gt-6^`}G*n^~rS| zE!iF$?HOXxJBJ|S@vTS$SFiR_I53sL{c_Po$l?Et5RTb)Zr&=T>hobq$~RIWJ{X&P zoZ0Os3HOZh_A$impL=c6H#5C6hRFH$u!p${jtIVQ!P-|LoK}PdRfn^C^LcMA4$rBf zbwRmMg=eTehGNt9{wR zfAO{l$&jp06yOmQhpjWtCT?n5vuP(C^GPpND?~AHQJ3diG5$Jj4m^aM{ws6;N|Zgg zh|F3Oe=_YiTjw_lIbsU06@xv_ zwQE6828k0sek=7WSC|-h>$G~`@nOo`!}ilt?$oIJs23;9)*xp^sBtE~#ih;rcz4QT zF%<;|&oNR{QwN>%LX&wYFOIqgY4-+k+`cjG)cEl4`F;XX^h~C=M73hB)*Xw-yf8w$ za!728(`Zi=7gD~uFq}nHAe$?DjuhcM zkTjbtx0g^a=k}e!+?%NchW1;(#j|p<*8$}LBQN7AO{=qx_H_~Upl&=Z++V@SgQdfo zChUJ02tKm>+yeK34FOuI@I#z;>~L50v( zUta}T7NjFdU_cl=zV^`bxb6ac2Mh%C*gNi#&ES?;cOXO*f?u${ylq~}eDCm_V|}DT zAgf0-&Fa_nt>3h3+x153+U5%h!yh>I)Q9o}ji1v0cW)4**FEv3H_Ss*TE$~)B#Q&} zHwl&O$-HkHI{S)$oO;RUAAl(MTCRd|H=>a^G7qY%x^5KAe}nj#t9q+3iw47Wo`@Gr zkunWUrh?CIo-<^*aqEi{1(IF1)1SP8LA^SX$-rOi`%_Yw9 z@b$*NcX%zhb%B`=k>as{f{;!mqI{wrA*?@IueOgFbgS5zue=KHd_9_4XxA2n44H+n$Q_09 z5Ed=(?wS)jrk|6lvYC+SE92;ee@{hR{TKVGJww7`;P$+6+)_V@{_v5{dmc%NAWhGq zjhSmU77Pla3|`9fINypqtIa<04Bmnrf|D&p3a|P(LUhfViEd||)g0>ZA6&uTA|Y;# zFH88k`K>g)gY}DMS!%R;-}m~zz?7g*iNi0Ju*IE+@?}!Vu^1-plQ?XxaUqFuhnHGg zDN#l|55HE?F8_D3P~)GZJO@I6a+xsSdAyXE_uM?!Qa*}wR3;;>KIa4yQ5G(Ytpt>*poHnH4krRuRy za6!a#4Khp2P$Mv%BxQhz^Y&uiX7`LUPqGn|=bVs%eE*q$`8`VLZ9+#bNORb^zlH-~ zjh-V_#=^p4%Ob|V+w}*U>zjAK?uk+1{EU-nj^YvCBtmT9UjIYdah14QT}Ub8^MNEH z0;Z9H2^MVgaEBZbgg_-D9cL2DMZ0K2WNdT7xc8XZrOx0aSToCL-F@1jE8jFVN^If( z<+l|mcgm9E&hvP<-h_fwR?oRQt9g7ZqBI;@pq8ms&z@J4J_9UeCN-gaUj})mmipC& zZ%o!~SmwKj5&aD`eqs-J{EG~<7w!Um^3RZud3rm2Dz!rSXOJX2jFW%=$BH|x`<6kG zrj(7cv^l{iMKdamCyKuk{OG~Fk4v|l`^;z568*77ppX@5vkj5#?~2){UpwKx9MYkJ zs)!K*m?n9@6k2~?r=lOIwY~`d{~>Gg)Q-FNAp8QkK=fz1y{;=@%*dKD+F(1EIF4dW zT;v0HT=st_Yew`tMerp$EKEefq@@O2BxJK&DE(D2k5Bh-pz5azOd7jZC{*G!MxYL? zrU*o{aFIqXOq#e+1*H3^n?h@6{;1>J^_{5@#Wa;lv;xnDY1jA&T?oRTd;@l4{kxl_qbn#R_G!V4d2 z>M_tOSyw4me1+#NNe`0$W)f6kzl&JXAEVm|m?lJ_lhzpq0Lqw6TI&w&H>#NLz=tiO z1jU*Bp-y^#aiLu4E<%m}ev?1_i)<>#yu0T4s&96t5A<|r`2wUArL3>(zQx6U=ZOWm z7+i663yB$DmLg)xaX6;TK8hFO z1LAObU+%O-pVQMb2lu28AM0+S^+dexiCDE8+i$IZj4ZQdkFge?+CFg1yxhI*-zDvf zEEOB(x#ZT9YAQvai}@Pgjej24Bs8uzxt&X;1 z|FxiGi)nl&yh;9BR8&9c1dyC_h;!u-^p+EkD%&xfM>AxYr&_N}A4))k3fvDWFec6;Du?V_&LyU`e%F^wT(D=SLJ!(t!QFdwYhO9VrnB9V#zk~ z^h2uQ7bTzGPX20P;-_Cn>chZhM&I&p@?;6}t%=DN|5#%piN9{HOhPE5H5*pgSbuTP zLn2UD9BU;O)o7U+7yID`R?nI@soK+&m}+(?-;#@ly=pSdlIzK{sIuDLIx?SsPyNqe zYsPH6o58z&U07FSgdz{Bn(|y8vE)RuvrGS{v&FJ?1OgjXo{A{clZ|9{;I$2JNBCu{ z(q(3ym2C}6aKK?B5^Gt2vxm{3sG&egkAOY)$lTpC_v66_cI5H(zpx_>9_qEutM5!9 zt;!4d4MMf;mS+`bBhQuig@C(vF z{SE@QvNm zl+Z}dFhcXYIQrXrcV@lq6HjLs8Nk1rjxJ$fXjm1#HXF5!dHO#MN)W=N`DOflQZG(K zx`x$fs~>~lpeX+f`=m?1Pn|1$%=g&Kvlnnz(lso~lJz%^<+Me7<%(^Z@WTIF?;Smv zBjc4VTRX=csl`oc<9lbQ`75|@eIJ~l}4fK3~-1GjGnCamaWZN z#QDS+k2IGWLt66EiS$1)P;6VL#tv|W!dZI363G$~ihe`FyQEEZB-|sI7{#^@5fG|W z(k;1se;Xa<{t^>Ir5SZ4<#10kxIlTiM)lvg5$vPlny2|iTZAr2txrzi&=s#`|MNdR zb0woPJKM`+>R-zGa%aeuTD4#Jr&pC3xDO2ex+&862$(8KQz=YN0zX4m#WoFG9)h4M zHGabX>h$;0F-}IC3p=}YX>#@maC_zcGspBXCYzd38tbnQCH@+JU+z%+vRpi@^@;t< z|LQuqC?@RLwlJ^hGsY|YUoTG_JW&m?jQx8{VMa9xvh-L|!0l6^^45+XNuUFVSHv@7 zlsRsQ(qlho^926C-j2K6p``id^=cPWV!r>?2l)2QtRu8H_1t@hZ;K)DbrD+u_F?@9lRGHr zPP<#{X>r=SOwpvIJi%DJrM2_Y+nS{e5?zd>q8>%I_`UPOFm*fMty8XUtXg@3grdpD zn(*4NAilZMKRPdytrrh2H|tvlCUu4b@L-kCl~4QD#2a+Xf%(Gay}x`L`L>JSi}UO5nq7|2B1{m>CLA zjtbnbr?07a^t^wV2tlq*he-|(xp(&Wnbm5C+=`ZOz00qct3rOUo?L1R4p0v{xOE$( zRCx3LzQHFwK4%OVR?k|t&Lm~x>&m9y#wR#UxxmXr$++1xKPVPN?i=Z_4~kYq)OZes2S92fA4$p z2z+*r+DPWs{ob1>vPKDU*C%Kvs>0+e=?nvxbzyYo@9rum8@?~u9;@Co*zfoZlC9~T zq|jaOT5n?Jv`@d5xcv@OG!#8zA99iQP%8OobpR1%jcl3F->^<26@ON>t85;^A(pae zkk}W+UhQi=+o`J+L~=c=H@A8v)t=g_tQ%8bf3jG2XsO>mEsozHBE~6ka1j!wg@>#( z;K&{SUE>nIEV{Y1hQw89J*H*$EuAf52**Pq3t`pQ&Hcwg88V2PlIGAw)w2|eY<53s z2ny{afkOXjIk`YUQDEFwFRP5ZWhv3`f4m#cj%`4GoYI=!1!C{l1@c^R{_+45Rr-RDlK-(F> z9o&-#~n`NC44k{skE3V*&Y#$4;|U-`kemloPO$qX3&yD z(L^>-BqUXIY95B_!o@1z#`}e6JSoCxn9A+(_)QJ3JRMxGM}WUF;qpdqLxV=K!Q|N& z3Qm*H5Epexr-)n{v19FM>>}@jz*ic*OBlIF7vD@i@el<_u=n^dGJ(-s@Zu6fufn_% z55t|+scUoRe=cc`5-u($=`=D@alMbA$?;OP=}_x|d5eAB!WC3Og})2iPpvmsvIWa; zZn)xX$l>{UR*j>`#%fANMM0^*DE^*7>Mdqa7v6r>H2eb6Tq&?Gu>=JNExO^$M&%9u zPd5q3`34&2ikZ8oN67R7Uk71po5R;}lbjS3)i~tMo{9cym9^7Fn|@ zC|?vkd~S2-9sWBjuPJor9RdUu=j_h9d2Ffi(s8csr;F>v&n`z-3_pBEoGY);@y#f9 zjhr49KbZcDIl}2L9yX7M)}v)J+b{mCzT$V5mJLA`H!dvflqw3{l_A=_k0w79IU#vS z{@LzPup0CH^>s9JF|vzQbs5fyV^haMJ!8JXY+Yfevr=lj(lIeDdG1NXw=|6Hi3pkk z%Auaze^wl`SX*zJ8sADSExTge8vF(CQr&Cz3Zp;+Ae(FSJx)mAWkh-oJpVf$+3 zcnhURW3oF~<#?aI2~Nrpg7fIo8)@+z79&(zW4M^<-Q~Wlnt~y#yLI^TZ<@|H`;eCV zf+ACwL`R$W+yBwgpo9WB}!8=yf@N@S>Q&eM~JnLLUAs`He`6hqA z)O%UCCA;jNT}XkUHG9aZDx;@vvcP{GcdP*;|B90Dl~aJh`o@pyW*s%Q)X29vvJ+d3 z6{ExIlQ&-VZw}Y@-apI9Eq;_)e(%BI=dYa6Pfpd<%5ARP%Awqg=S+VKNh2^B) z$8Q^u@-SsgkgTwpjDk)&Ip9`$9&I%7wN_M{B>u?0efRRKcRQi$cqzd}4BP066)U-_ zKdDfO5f94A_EGcx1uRi{!jre0(N|kNZjZyeGKS>WzZj_86nq4QjWLI7QX|~lZ;Muk zksc({kE0199>;jK;prP0)RN8;`foYE34ZZcevnL!bt_Eu{2!Z+ldR0uQK^A@|D1f0 z0zEV`qsghg`2y2P-8%xFL(Wz-KI>;f&G_3FM)@!tA>cr-IG6gqe>IRh=PZa*3gxtH z=6-TO3;#b^IxBN}2|zlvDIJ-v?t{2$+p)uyW| z$&2a3aB}N~=F(B=jrpsDR)0*ftvE>@ceCX!`>&fHlI7;jXG*`Bd|XeUMRzY6W|0wL zG_6K-`~D2(Z2&#KyXOy?0MY_AoCn^mV{-QyCYZHk!|_Ue?=x>VNDdc1)+7!p-DtX! z)z!W8tvlHgAsg>jm%CHsZ3`}08?K<2xcGXrAqVvs{9^OsLm)u}049OW1ZRoU22xP( z1~1yP=|62wk2Rh8R-2Xa*+w_dLTQhaSYWtEN>w7}6Hj6{xdN+{kcS@P4au=nkyS%Z zP9d3Li^@MCjy~CWOz`E?n!OFjKdo<_4GUpUbScLxZRwVFUX(5Ew$(1W+2=ig8{2Pu zs?<7!uJnU2qOv|9PMjd7@`F3IAr89YGoRGhl&iD1O7EI#dhFJ&j(ib?|2i-@WTq}J zuo+4+YIgRa#25~%ga`|zY|$JTWaDAJcpjf~j!m4K#RD&9WD95&Kel`5BXqAkVxRpU z+X7qK#~$B`3ljo|LmSh#Q)zzFg1f5am*IOm0rQrSG~ZxKeh?|_K_ha#i+9?F6KT7Gv+`=InQX~m;tcQ{?kpwNWp$7A zq1zl>GMwDDMl7~N7z}DAmc>tJdIh!k+QrR{i^WWNZZ~j-QEp7wm4~V>E)5T%K)V3;QK3DvAa>I1S@4arZr4*363NDjP3BOPwlT zUnA#`Q$pBP4%QTojcIMed(^bF3M*WFMw(BCl<$>fT;2@PW53z>F*|86-C$-1uMsgZ z?+b#>+W6Wi0g49>5&tOZY9Bz|N;Ky^DkE|S_StMOy+mWOJ-rh`1_&)Pn<*8w_&Tx# zCHZ|GYm+<&xZF6N-iBeoI96{ZjDSBO(YIGrV@++;D+l9Dhqfct9_sqo^U|7X7n_9k zI`?hMtEn(TA=5^DP9?f$vtJa*b1yD01%_)ik=)I6ckBZH!4Tf&KVk295_ryN|;!|M^h z9-DdwJ?KqKjx1i)%)fZQhAmf_#YVdY2UR1cO%ZL!0hq>bN0qY1oZSLD^EkttyPYXx%pU z^$cVKu>$90fyc`MLh?8447hOeGGCo{xn+%XuL@ zOxrN!ctaO8xm0(_aKlSQ53%apr^~XL*26x}3ozt^rPr_sMgFy&OSGwjdwJKth{D@P zHaVNpH0M@r|G+l6mX4t}s9Gc|%HHb-8sI&MBFg`A{IWH0Up`)fT>atZ4OcqBkjG;x zywAy+Ip;?3uaeTAY8MT_hVaz{h`=beX-(COW}4ryCZl019-~(plL7V5k9|%{UnfM_ z-;$N?+P%8QIk?Y!g&)!N$g+3umG?m9)Ghb&LAr`)4j(Kvz;X9fla8;9ZF$)jNDf`Z znTdv_BQX_;VqHCq&&k`D!JF%Hr{wZSF`p|}#QqlSlivgOm3_ba7A7o$593;{ISsH; zL+oqE3WoytB6FDJwLZt+0D@T`@wE-~X>(osl?8|p)Bz}JcvQ5PI``Zo+Ki?N1uMij z`MW;`#)mNj5A!2Eb+(IjNsGYNBKIinZP-u&kGtDZA`=lP=U85;w{EMwG*<7QmhVNF%DcO>hbWgakH(z`G2D|_s;pLrVBE^q5< zGe3)Ry5=C5-$wBMcFH?p#aGe&Sw1)-?2K@$s-0_P-~MYno1}a)Tu-4HqIdEDC3$X$ zLO2V1eHW9CNc+N}r1LHK1eOnCw?|f|-etoqEinRl?hAu=ZjK!Bfog>3ZltHR2|$X9 zH07o!`vcq+Dm5g25Xw25xva#~;V>FGwQMyaSK}n_=>}&E;EPKI$V*_WNFECeeyX>0 zp=q{GdulbG*OUeLNjid);Hm1Y{Bm;scD*?joGn8!^U89;EV#nQbIT}+)4uzELUo)4A-3YcF7Rl%jP8a% zE^Vs1q$h*=bW_&^i;(G={6eq)={4_kq(!*lJBffepsLi7dxNjHpdngwisrx5qW?Ml z9Ey7jly^{Z9;alP#_xhI&RtIrDhhB&4Q6+162xM>dw8VL(G|NQ5QY2vFShJ=x>YSY z{k05{%HfrtGks}qAMn~yy+_HrQz3DWiZZVWj8GWAj%`EkUZK6!0U(=#3qL9LtyTeT zOKH#Xz2to`?mdX5lMTF2zBOz=8D$a5xYk5U_SRK5xpSzf9>V;ERR<%jm?hVPznnCO zeZElb{3eZkjRppc)|Vv9mZ357srgxm8bYvxiqW`tH>{i*!?-jK(AZO7;AGH zmrb$FmQHtz=c54NKtO3FQf!!}jwh89k7nCWo(y>07<_4$3n3PI$@FK7u@LRK^q3Fh z32=zuU;!mbpSR4NW_mU|J^zINm^f2ra_Z0P<}^cYS5s=I)~SBb)^3X3+-{R@y$2>5FG&GFKCbdXv~kwQAH(D88C+`k1Bc8; zviVJ$@hU1|g|alNx~+ISceCQ7d_51Od2@mK#D7BXW)Wb>St*CCA zHqzSVs!8)6M7O4G<&Sg{fcC84Iy&ok>uN9lQm#JNk=uW+C)}~jdKid47 z^mzBv+Ggz>_R>gZ0~osDmNPK=ZQadVGF>LL#chODXIURoVdD%v?7IOsD3I!_ISo_^ zXrxbY=d>E$b8fAi7P<*!Qu1*`BN!U_#whqig4RbNk1AK4JNmO9eS=%YZMjt)fC`Y= zY_6PsTZ5AGBHc~5^ok|SHQp0-ulDVl=!no(3`%$e>OW#PZR12G+GC$M8x_4(YnRIe zBJoh+dcP*#>Htu$id4>{Pdn$Y43tiVaJ&8QiLHX7b+(p$C_R9>)C_aqy~bQa7-OAF z&vN2q#F1HQ9CS9z*#;Xox{l#`IwU_1ZVRK?yZho%Y1Qb@P16$j`5>RrHxVbieYF?& z%ULZc`5VdxCma$?3~}Zso$@2&oX5;g-IH?fhIs)upOVzUTp{-MDRwF5i*--<#2s-l z0pFI72~ot=N>wtX<>pJ%?$TjYF5&0o3!m=?+9*UkN-vogBZ9Ur4|elk&7w#Z20auj zC?ilO9DNo$M%%x2s5q^2-W74`25e`HN?K`@Dl(Et{I^A=Ef=h6lvPrAAF=;kVxM0H zu5B!XX(AP!qcdJm*Kj#&)PZm76G~XHWr{I_Yn4+1JF>U-7CBT0!0zc%>#qG!b?N}&F+tmKd|&E?SCsDeQm3h zSOxG+NeTV5_Et2CuXvVCfa17}{&u4fXmJvB3nbfBgzzpAH+$AtE{Z8Z zGTmXu1w028ipGMSp#%I^GoA0Pb&Oc%(nt=HR1V~-`TR~w2DIOaC9nWuOl_HmBXh+Pwu}%K47H#AUm-qRr}5^BY@Xb zhw(b`949eIYp!g;umbGEwZ0N2>;M5@%_;_mC@)jPBfve1ZkmT7n3!Y0awi%j^&y~gm1QD<7lum>m zdJVYJ*98LhaTD)lR%PP&5_Tt$Y+CG;=PG&{z znVW4H;#v~JD;eakE^ZyA%O}tKdvIgQacv#MmdOvc4I>v{+bt$aR8H6#*M8wD>V4PC z9m|y`;8C$w68dQ^*uf%fftgl6$~t!bM{WotH`PiK-6jH}o_I=3)^o0an~0k z>KBHs){uB%&s7&|x7-0G^3;fRK1GC5B~|8SLzSdUE)DO+(@DKB!r5b$rPGl7N-kH4 zC=$~Zdyp0C&7RnhhDcL}b0`0bZ`Ru8>+1gUGfCAIjf zqR~@hu_w5Nx6OM<^QC)v#E9sMKNzi0#h2zUt?P*ti*Jqmw)tvMAzQSO0Ldu7x&MMv z&wynhXs#OyGMKU#V|4Sop;2-Rt2Eu31WRk+z2%Y7FHsI{52d_{+NE<^SQ-v*s^FV0 z7W@0xgk|SBoqz6SFi8CIGBSg!ZIDHx9u@>_4RjP{Ql*Of!!KRN_UZ=4Y8UE=Q}|T<8Kx(yD|

H!G*3qju{ln|p?jjnuE?JYI_KOnis9}+l zFD{49?$K1Rq4Xpoyo`*6ECEK`+Yz>wW|6A^!F=V#vL1Z~-mUjlGU>}3()sR+wHwpB z`~iSO2fA(CTT1&pmabOO`s6RL&u6OT0r4i>sjFDpWqAM0*_|x7Qa51yeXsTknsO%I z-C=}D#lc|}WWxA<_DV#3EeyXTpRt_C9ev%QIUUt`UT$o}A?0^VeL<9c?_GVU&6^j# z{uoEE=ir>#h1wz3e5HTUZ8B{2^q#%g3R|5fSgoWNtrIMDu|LLjO{$`7v>fk6$li;d z6+>lVUL)`NMQcP%8$9SYM0*KoURl}s^Riq$vi|Y_qRJ0G=x%p z0Bpgg_mOR{_1o0vuoN%j`cTKJJ$imlfHvjSMroKI^KwJfxDluw-a2QCJ`&CK=tdJ5 zDt~BJlnW3pvdMtn7??g+(qm1nBLEA&#-9N_KSi(P@Rt#LqfqzqVdT|NA(Myac<;K5 zv58?j6$CPFc578&wSxW%5xM{dlKAg~Z`uI5G{j-4?>Y%y(pzIoG(x>@;1reNYo1r} zKi48hpgvi;c5U4rZnfv;lmyHm`=_I7ZC18=bsF^fuD;U0WW8HX(DrA>i-5pMszjq5 z8Y>ij@kg5Fet(e6a3lV+U=t%QUu*x7UgLvhDflKOY`X+yE_Uv;cEPFcoN0<8??t>v z?}3RQ3HE*06_)o*=Sq8V-K#eef;$Z;iJO zy{0+aoWh38>{bShgiBJR6Wtp`8YGB94i=`FG5tP?ikQ@e z7XgK)s_E~?H+%72I9b;s>1LOc6lH-DwX`enoSg0K%NBr*hnAy@qxufvke`>40>i)+ z{xVV`egRxWXC>qHzQ97vKo3-g!1U+;+BBRXLFJz+f>YK3;XKPPFhhuV=;3%!3FplR)--FxwPiX zz`kG7+{+q2qsj#m&z|J{Ety8+Oj##RQ_ZL&5ITSRM@oZd`f$es8jKyb1To@kN^{FX z9oFrI6?*696|NtU()N7`Y?^mz3M(1M+gDKc8%roplY2_{{+YEUL38}!1&RfQ;y~;L zARNkXTD!JPD&$|~QRXg0UjaWxRc$+3!SG6_1x%u1nnp4B^#uBk)8Gp4AB$RB)yt}l z+Prk@NV_Fpcz8jbOH8WhLpC8#=Y@syv7~UD#(b1@o`oLs0y7FwwjXk-cBpSjEZ$yp z>kc=vbOXbZyW~8!dKNosh)SR*z}Gm?cde}WLWDB+N##{uc>ZlOxfi67?3k)2Q(N}o z{7-NwcsPu!+?HeM_?x!kS-@0=o9x1<3y8C=)v}Bo9$5eIR;6)fho$7h2pmeyfQ`^<8tOPpkR#op^ji>3XOb6a3ZHs;l{XslhN(MO4n zYjmFB5|UwD3h2>=zApx{-FT&<(#HhR_QK8t>__Hx%R+(;-RL+qyU|#!%xk^j2Qeja zpa+hwpDDj_`WRf^;}=+C4D}lYZzka+X)!{*voC>i3IgKdlr_a7ZTbEy8eAgJPl=~j zh)uJEpDJi=5y2YrJ;i!vRNBy4PneYcgwb?@JwPcK8{!3P@Tm&Uh`7!^HsXl@M!t9q z9}~hLW<0>=taD=NM8~iwukLI$G%{iDCduLS8Ru&tnF*lhe-hPr5-Bild%kTJoaY00 z{L|V!cEz4{wytk0lhaF$!(D_#NnLPf*}8`jSK2F>;rYS=;;jnQE==q-{j zmXPh?)LAB)#`T{4c%v6-#kAflTDvIjHGI)}{otNI1ssR+cwn4-kA*4sTe8=Geb$AK z2Gdq3H)NA^d@uHc0c!Hj=?aNbGVYPcP!a3GlD^9C`$$Pi@#yZm9NZjcx}QXfz#}ZC z*MWB<^1FKV6g>E?a%^Tq*Gi7f!8yKecIu6>m{5CoE2n`2L#B{#tFn39$FsX07Vqoq z#MbPQy$eym^C5GkT&r;WcQu2I(lv1jSBA3@Dudkvh*1l2=gP7(OPm460kVvMQ!7nF8 zYGUBOb9Z3Z(So}8vOQ004FKxNTWspuLY2A_F$x*qKpq_1N<3#ryh&hqMxL8-;FO_j zl(cs42~=Sq>gjz*szKq_dI{c~pCmUWf}$ViOaw?ny0exK>rQM#jUKX2GUqWAQlJ$k zQJnQ93QF723KFP5Zn;S*IrRSX5YYYHn1hxi-i_Jm>rLD3VJer+!Ae&s3$DycfntRb zyT`Iaf?A74TVQPqKTxzF3*7d`@yk?BqAmi%%~k*I-CJg@_1UKzFfb$;62bz?>=zeN zo15qPIRixiOuhuQYDm~(G&?jWW)mM%3|Ywo5qU_?uNXGL`ui4&rdLs7W7(S(D|~hr z!E3;LBn4aedLX!4EOW=nYm;wr^Wh-!lF#-rgU9_-rKiE@(qb$4GqXGr7%$ zOP?kDsDqFwul_v%2cuph*UDihw|Jn&JPfxE=@3TOOVeU+qeCrBRc88)@m%@0*NWaV z%U25Fqcp5|Mu?&SPw~o#0C3mc5 zuK@0?UT8lvPo-@EnU}B>-uiApS1d*XsrL2?7VwkGv-OBpx-{ZP2^NCY1m?{Xj>MsKZ>5rTAb<}sK3ejDw$ZAE)njs2 zsU+wICPUf}`#n#5?=Gj=9ynub5kyV?lp5`LTM-!2GiooZ-x-EK0G{8Xx$3tV%ahvg z_wN$vc{w$$UOsp=dxGmCk#0BXlbW(%kQ;9}L&VeWOlP8}e#wgUNstDUsMVzz2>-D` zHb2QWH{jAQ8xY;}Q=J%3ilOTap@eRsEa}g)gWj1$0#DwDqrjSE$_=Nv!I-=>#ftQ; zDK<^qYkpATq#Tj;t~yx#J-*kciN@3Z$N^g&Rj6{nGoN0y0+y7;FtDzYNR>hub1~ZVUOl8b($7S+m^4)^G)j zEMr06kBru)pnq4^;s5D)Q1o%1V5+mf5XgA|^fkcl+#mDB+yyd(xz{1llB|dPVlaD0 zZ&@Aibct(eHyB7UX}P1`%EX&c(g8y$SGroFYg zn)%fWWMpLS%R7gKeV6??r@;jaMazrcoX8~U{9gtG;JPqraE(UIig_)g?jo5LW@0A8 zYHQnYHqY@tzW`1#%Da7@z*2-8RRk(2xHIIPh^+#R0$`5;X`Mw&ul5FJrS$TnzW5uU z%yuxYL~FrPjOMFIgKf(%Uw-MgzYx8-J5JU%IJ`=P_Ej>`9F2?OeZm#1wLEgC_XT(z{MYR{<}< z)1(iKSD3C*Um`l@3}s>#iEwM1IvIC+oeqhdrHS)buAs8d7HzMBbm>NGzr*xygs|kL zxfurLBt3qF`g8dj;8cxthuQ;r4iaW87cP^ivKm~=gMn*wt@aq^7@);(^L0%S4^VyC z;H_OgrHtQTc=@tuhL#heF8Bt$qS1 zAu%y~!iEaI><$CN6lKmyS%VMK&DP<^m-G9Aha^(z_L8j$yCGP+gl;x?AYE?Z5P(9R zg~E^*QSl8_Kj$Aa(t+Y)z7n=dHhPqQKwe@MV0SO$4x5gDPpAwU$ni7A;1z-NmWY$6 z{U6t);DJ;BiBxwU=1E5fW4#cNm|qw%PtF^)$vs-Lhb|+^l9MtUpXsu(rH@_g8kjD~jSPWkS9g zgE*KV^?(vaY3c%yhw=Sv&jDRiSS;9@@-%UevmvXx%n6*qtc5#`CeOo}in2Lt!-}`k zAQu2=6ex07Jn&SaQLF@{&y{{dgn;z~k~x_ga{68ox;b_{YR5-ntDsb^zBbY*ANYcM z&ym>8NSMsr@HiWg#H<*|*>yQ#hB}IJQzpGxOXq^yj1GCahaJMpIQyM149yf`nP-1@ zDHGVPOnw^QC8vqn&gI(5&-}{IcQGaJi71)Z?_9+NdpA@ocwk@ zv#Y3+H~R*;P!oyWZVZuphY2w}gQl0V!)^dw2829{4_>DZxoL4(seb41uqyCmRWO5F zT73Up1kiHsiS&nr+elLgNH%zsFvC zBtWaz5gQ8F;|t;G2$$7c7VxiR3apAlcq_WEJTfg3soD?hQn}U0orOx)J2+Q^_%0Lu z*0ltSmoU?^@*%iq-_`aiMhTl5mV`CMC$C)0p$mku1p!hEEUrIU`ZuU@OwB~jxcUU= z@`lvvw^#87*W;|i8kY{Aw(d`z+fJh$IJN`cv(^kawmsfI)mh9577g=E+IRS^+ z2#Q;RbqwltGv%|qfw$`}j@ndJ)A9p3j!@5Xd_tKF@?6qm1$*N@>ZRm6Uv@F}IsiW$ zInD($2Qn|5p(F?9ts4lBxikfXI4R}Xji`dY$iPyWRYbh?kE+$eBHCECIO@AUe+)iY zwuX1t5kMQAf!Y))?(5u@D+2~5_!b=Wi z(G|FZYAmS)5;P?Ep#Wu3Qr)}E?}t1Jib9_s6$1QVQ9ntO_Hy#h-AzhP$Jok?bong3 zsnw63w`kW8ikJCB(>Q?HUuY?q3V&b)nn7kz_tRwTGZCn_p8d#K8m2j8yvTp8jrO1| z`as6B9YYWSDkiKIekX=XNKLF5g<}NxV8qLzu9+hfyOF0)=0pCZ5KmKp+tudEga!#rT?eH zh|im~>L|218y}J(I44_|hw+GA?f>Yidpai8peVg3^2UH?n5Eu-3XPPA*+dP`{vnqh z`rx<1f481WKa!0}D#zQvCdKCDiy<_QoI&G=XKhA15@PstG@~_wZK$(05^`;T^)?tq2ss3d$%P zZRhdd&Ha@FH<~$}T5ioc-}e7>bLHVsx7}ZF^$5ut*@w)KwJq@6 zLnLSO;~y&=0{5gAmJ4{==TZufYhP=_`0!=DPuaeU z42UrLmL@WTj-yjzf!CAC4?`hZI~&^*y>7Uc*0{!8B?W0lt^c5JC}Z|bPS?{utp&2P zI1eL8qM1age4?t?wFL`sB}#tOV(!B@VUO6K&?fK&11thTs22=iJw6=Vd8U7N?)91h z_oC;GM0vayKV@EajpC`>CqCuBncib;iCgI1x0~VF$EHDF@*Kx7cu@wFT~rn`f~(#YlB*p#WV}m_ zWmI0*B%Q34s)uEJD)1f8T}3AP89eayN|&0n6e}vN(&XmZN#PrEj~w0C75*7qK=gH- zPRv+oI`$3!$Iwo-9U;2(MuyCM2#Nl)oE&Cf*VhpeWgn!Bv}b5W;FPT&J_PK2SBh+U zvO3|BqPkY}=!&b?`6UZ@p_;9ZJMRM#K4Rp(de+`|2Y&h*Vz@Lnx%op=;e{{J6$0LH z6-<@XxhN+^l1u8 z)|P{{D@Z210e3Xu)=xLZ;&N<5PNFAWL||@iR6RAy>+8Y!sDG}k;JfFo{DPk)22xah zY5}=z6j=o0@)hBDPQpJ<^qYLd# zD>|4^q1!6B$IE-~Pv0L^(83kyTQifZG?r7)3$huXJTv2!R%R2r#@OGPsZJP$GASr1m^v6@MAX6GawZ+rVj4tu zehl@>jgF4C_icdsC=P81oB1f!@@n%2coRsK;Xzkyn}^LC`&{Mk%Wvv6T&O%S*FSjS zzGcTyP!^@TloH(P5k^p5Cp2~Wb*!X42K%*{0nHdY>FnQ_UlrM~U^pbhg-ew^UdGqq za#fiF51QwsL}WeT%R zI1dCuq$kKJtYr~2N_pz?D5w17di6KkOqOxP;Ir%WQJM@$Cl5Jpky0qRVL3GuR~*M6 zVlap#8mwA)D%6s$y&U5TUM`Mx;-Sd-iXnCL6>>yVJ>}`w@&nN`GQHztrYcU3j;iNy zk&9CW!%o_c+k`$H=QL2YdiPrRngUnIXdCD4*9)N4oq+_pbh$Xp2%>Ii;Nnzf-{jko z0cY!9Ki#1&@BQwu(vm{tk7w~bNau^nQ+J(17W*_@zGRx|K40yiH*Q0Qe#@-;HlcQ-1!b7ngO z(Rl))tLR6~n;{9*ljDjU@B9EedXupPcfIeSHU8@i&zwy#EmxmN=-~k(`xvRayBiq6 zC!)BNz$adprFJuvYw`I>$IHirR_AuyvC+X=8eoFQIu!K!B%QGSJrfI-BDtlz;PQX` z%%pTqEov)}OX|+Nt0TkmYKB93deWyaD!mosAXet@nyBuXayZ-bkFx%kq%{sV7 zzW7`q3>(~b{_4{Y^Zr)rZl4AT*bZ;4OY@*Z8BhGNZu3(7qxEE60euOClQ-$?Sa*S; zr>alo;E4YiNsce5B6Kk~1Nv@iU1Kmt&KOp4>+YihD&$29RUojUh`YoY> zJi-MZ@PRv^!O|(kZ>Q$(US~@)h6=yEFDgGT zq^H*`Hg$E{ijGa}5wZYL* zf#Y}|JRj4S;4*e97;kI+cXEvJ@~ke5)8sgFNN* z(4=ixo8P$l3oh?On1CIMGcKs3ofwxFGC~nIo4{`I!LyE-jG-T+&#KS#n>gIeH4u?9 zwfEZ=Tg`;6Oj|RDJ4xp)d@qrw5Lu;Ybgze8r`oFbsZqzv__n|i#e+W0pI<+oGddS_ zg1VLr5^`VOou4jDcjFYR5)q6TT@+IZ%8@R`JK+5}H7y_>Z#dW9KX7~7J!DMKLL*{O z>FAi4^nNX^sedCSe?pjvN|N+B^03EETH-lE*xb}q9btVnGz?D&y0j$B5xO!}C~k#@ zsKTaB23`KHwzy#NidaRL2YOMSo$4+KY{>e~b=>p1vAx>-NDu)!*XV=^9ta{drP)BY zyFTwklI_*mu5qn16)N-SqUYKEDpbAlP0G6Rl)kh@amP=5N^{PloW+%OS^|pubSjFm z+v;xY))a?5u8(a#R7+b|m$oPUbynjx8Bbk|=o!A{97zsbPbEY?`|l z44p|jptjDf>7@Z3(PA7SqGD<0`IkAY50XKXsCQrF^KiAnZA3V-O#iV88)d3uSleHw zIv13CUu_U18>->$ktN*+2T|EV>MxygApb7Fs;@2d0MG08)R@7l#BTrI&_(U9t}kVu zsy>kzofD0LAKLewxqC|v%N|!FVh3`W5b~GUHW}ky)UQk-s%8U;M^Y0R-jOc!&136W z+S`m+WifQdxN2nYR(JhzHcoHy$A>0N^7;`Julhi=w?3v5`%Tge`!jBAFsok^+*9O; zld4a=KMb3n>=Pyk=EJr)!{ZfMu?siEn?5(VyQvZy-q5*9f|1f6W}Ps>O;eLZ4Q7A} zBVOpsXIQ>!S0C76AT^LcUj#$(Y0__v=LP>E8&s^DR1`VhPtA-qq5;yP6BAkVu-b8u z+)FE()2l2Nn_(2P2fKBL27_#EW2f<_S6M7c5}^jT)O3~;}Q6w zqU|xl3u{Y;F(7D#S9rQ-KsI@?bwKxn=HC20AZB%`lwdZNnA zni75>bX03ICwW+5c7kRk0|!4c{~wijJM~ec?`^C`eu7_hMAJ1Yq`P7$-bKm6Up=Ge zUw$d?(9aQSe62{lBxu<7jsNu>V5$Ho%4viFbIfrg0H|=flYwOM5hDIqW1vDyDYGpFpDSE%ndDd|bl z%~n%IQCr1I!S^lR=nu(FAVKpP?az;`@22I!I{i~}YdRm^*( zYLhgjYL*5iLgo8tx>W0~fB!-Qa-KC}VhMY= zM5~=Qf?E & vel_que); - double lowpass_filter(const double current_value, const double prev_value, const double gain); + static double lowpass_filter( + const double current_value, const double prev_value, + const double gain); double calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, const geometry_msgs::msg::Pose & self_pose, const Point2d & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time); - double calcTrajYaw( + static double calcTrajYaw( const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx); std::tuple estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, @@ -194,18 +196,20 @@ class AdaptiveCruiseController const rclcpp::Time & nearest_collision_point_time, const double old_velocity); double estimateRoughPointVelocity(const double current_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); - double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel); - double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel); - double calcTargetVelocity_P(const double target_dist, const double current_dist); - double calcTargetVelocity_I(const double target_dist, const double current_dist); + double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel) const; + double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel) const; + double calcTargetVelocity_P(const double target_dist, const double current_dist) const; + static double calcTargetVelocity_I(const double target_dist, const double current_dist); double calcTargetVelocity_D(const double target_dist, const double current_dist); double calcTargetVelocityByPID( const double current_vel, const double current_dist, const double obj_vel); autoware_planning_msgs::msg::Trajectory insertMaxVelocityToPath( - const geometry_msgs::msg::Pose & self_pose, const double current_vel, const double target_vel, + const geometry_msgs::msg::Pose & /*self_pose*/, + const double current_vel, + const double target_vel, const double dist_to_collision_point, - const autoware_planning_msgs::msg::Trajectory & output_trajectory); + const autoware_planning_msgs::msg::Trajectory & input_trajectory) const; void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); /* Debug */ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 6e0ecefc..91f6d4ef 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -91,8 +91,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node const double slow_down_target_vel, const double slow_down_vel, const autoware_planning_msgs::msg::Trajectory & input_path); - double calcSlowDownTargetVel(const double lateral_deviation); - std::tuple::Ptr> getSlowDownPointcloud( + double calcSlowDownTargetVel(const double lateral_deviation) const; + static std::tuple::Ptr> getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point2d & prev_center_point, @@ -120,7 +120,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node const Point2d & nearest_slow_down_point, const double slow_down_target_vel, const double slow_down_margin, - const autoware_planning_msgs::msg::Trajectory & output_msg); + const autoware_planning_msgs::msg::Trajectory & input_msg); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 07d62a25..3adda2bc 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -44,7 +44,7 @@ class ObstaclePointCloud const Param & param); private: - pcl::PointCloud::Ptr searchPointcloudNearTrajectory( + static pcl::PointCloud::Ptr searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, const Param & param); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index 43ead397..67e5e53b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -65,29 +65,30 @@ class PointHelper explicit PointHelper(const Param & param) : param_(param) {} - Point2d getBackwardPointFromBasePoint( + static Point2d getBackwardPointFromBasePoint( const Point2d & line_point1, const Point2d & line_point2, - const Point2d & base_point, const double backward_length) const; - PointStamped getNearestPoint( + const Point2d & base_point, const double backward_length); + static PointStamped getNearestPoint( const pcl::PointCloud & pointcloud, - const geometry_msgs::msg::Pose & base_pose) const; - PointDeviation getLateralNearestPoint( + const geometry_msgs::msg::Pose & base_pose); + static PointDeviation getLateralNearestPoint( const pcl::PointCloud & pointcloud, - const geometry_msgs::msg::Pose & base_pose) const; + const geometry_msgs::msg::Pose & base_pose); - std::tuple + static std::tuple insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - const autoware_planning_msgs::msg::Trajectory & input_path) const; + const autoware_planning_msgs::msg::Trajectory & input_path); StopPoint searchInsertPoint( - const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, + const size_t idx, const autoware_planning_msgs::msg::Trajectory & base_path, const Point2d & trajectory_vec, const Point2d & collision_point_vec) const; - StopPoint createTargetPoint( - const int idx, const double margin, const Point2d & trajectory_vec, + static StopPoint createTargetPoint( + const size_t idx, const double margin, const Point2d & trajectory_vec, const Point2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) const; + const autoware_planning_msgs::msg::Trajectory & base_path); SlowDownPoint createSlowDownStartPoint( const int idx, const double margin, const double slow_down_target_vel, @@ -95,14 +96,15 @@ class PointHelper const autoware_planning_msgs::msg::Trajectory & base_path, const double current_velocity_x) const; - std::tuple + static std::tuple insertSlowDownStartPoint( const SlowDownPoint & slow_down_start_point, const autoware_planning_msgs::msg::Trajectory & base_path, - const autoware_planning_msgs::msg::Trajectory & input_path) const; + const autoware_planning_msgs::msg::Trajectory & input_path); - autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( - double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const; + static autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( + double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); private: const Param param_; diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 813e5fee..002f28c6 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -38,10 +38,11 @@ AdaptiveCruiseController::AdaptiveCruiseController( vehicle_width_(vehicle_width), vehicle_length_(vehicle_length), wheel_base_(wheel_base), - front_overhang_(front_overhang) + front_overhang_(front_overhang), + prev_collision_point_(0.0, 0.0) { // get parameter - std::string acc_ns = "adaptive_cruise_control."; + const std::string acc_ns = "adaptive_cruise_control."; /* config */ param_.min_behavior_stop_margin = @@ -107,7 +108,7 @@ std::tuple AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const Point2d & nearest_collision_point, - const rclcpp::Time nearest_collision_point_time, + const rclcpp::Time & nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, const autoware_planning_msgs::msg::Trajectory & input_trajectory) @@ -187,8 +188,8 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const geometry_msgs::msg::Pose & self_pose, const Point2d & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time) { - double distance; - if (trajectory.points.size() == 0) { + double distance = 0.0; + if (trajectory.points.empty()) { RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "input path is too short(size=0)"); @@ -258,8 +259,8 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromObje { /* get object velocity, and current yaw */ bool get_obj = false; - double obj_vel; - double obj_yaw; + double obj_vel = 0.0; + double obj_yaw = 0.0; for (const auto & obj : object_ptr->objects) { const auto obj_poly = getPolygon( obj.state.pose_covariance.pose, obj.shape.dimensions, 0.0, @@ -276,9 +277,8 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromObje const auto velocity = obj_vel * std::cos(obj_yaw - traj_yaw); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = velocity; return std::forward_as_tuple(true, velocity); - } else { - return std::forward_as_tuple(false, old_velocity); } + return std::forward_as_tuple(false, old_velocity); } std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( @@ -370,7 +370,7 @@ double AdaptiveCruiseController::calcUpperVelocity( } double AdaptiveCruiseController::calcThreshDistToForwardObstacle( - const double current_vel, const double obj_vel) + const double current_vel, const double obj_vel) const { const double current_vel_min = std::max(1.0, std::fabs(current_vel)); const double obj_vel_min = std::max(0.0, obj_vel); @@ -387,7 +387,7 @@ double AdaptiveCruiseController::calcThreshDistToForwardObstacle( } double AdaptiveCruiseController::calcBaseDistToForwardObstacle( - const double current_vel, const double obj_vel) + const double current_vel, const double obj_vel) const { const double obj_vel_min = std::max(0.0, obj_vel); const double minimum_distance = param_.min_dist_standard; @@ -402,10 +402,10 @@ double AdaptiveCruiseController::calcBaseDistToForwardObstacle( } double AdaptiveCruiseController::calcTargetVelocity_P( - const double target_dist, const double current_dist) + const double target_dist, const double current_dist) const { const double diff_dist = current_dist - target_dist; - double add_vel_p; + double add_vel_p = 0.0; if (diff_dist >= 0) { add_vel_p = diff_dist * param_.p_coeff_pos; } else { @@ -415,7 +415,7 @@ double AdaptiveCruiseController::calcTargetVelocity_P( } double AdaptiveCruiseController::calcTargetVelocity_I( - const double target_dist, const double current_dist) + const double /*target_dist*/, const double /*current_dist*/) { // not implemented return 0.0; @@ -424,7 +424,9 @@ double AdaptiveCruiseController::calcTargetVelocity_I( double AdaptiveCruiseController::calcTargetVelocity_D( const double target_dist, const double current_dist) { - if (node_->now().seconds() - prev_target_vehicle_time_ >= param_.d_coeff_valid_time) { + if (node_->now().seconds() - prev_target_vehicle_time_ >= + AdaptiveCruiseController::Param::d_coeff_valid_time) + { // invalid time(prev is too old) return 0.0; } @@ -432,7 +434,7 @@ double AdaptiveCruiseController::calcTargetVelocity_D( double diff_vel = (target_dist - prev_target_vehicle_dist_) / (node_->now().seconds() - prev_target_vehicle_time_); - if (std::fabs(diff_vel) >= param_.d_coeff_valid_diff_vel) { + if (std::fabs(diff_vel) >= AdaptiveCruiseController::Param::d_coeff_valid_diff_vel) { // invalid(discontinuous) diff_vel return 0.0; } @@ -440,7 +442,10 @@ double AdaptiveCruiseController::calcTargetVelocity_D( double add_vel_d = 0; add_vel_d = diff_vel; - add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm); + add_vel_d = boost::algorithm::clamp( + add_vel_d, + -AdaptiveCruiseController::Param::d_max_vel_norm, + AdaptiveCruiseController::Param::d_max_vel_norm); // add buffer prev_target_vehicle_dist_ = current_dist; @@ -471,9 +476,9 @@ double AdaptiveCruiseController::calcTargetVelocityByPID( autoware_planning_msgs::msg::Trajectory AdaptiveCruiseController::insertMaxVelocityToPath( - const geometry_msgs::msg::Pose & self_pose, const double current_vel, const double target_vel, + const geometry_msgs::msg::Pose & /*self_pose*/, const double current_vel, const double target_vel, const double dist_to_collision_point, - const autoware_planning_msgs::msg::Trajectory & input_trajectory) + const autoware_planning_msgs::msg::Trajectory & input_trajectory) const { // plus distance from self to next nearest point auto output_trajectory = input_trajectory; @@ -494,7 +499,7 @@ AdaptiveCruiseController::insertMaxVelocityToPath( autoware_utils::fromMsg(prev_p.pose.position).to_2d()); total_dist += p_dist; if (current_p.twist.linear.x > target_vel && total_dist >= 0) { - double next_pre_vel; + double next_pre_vel = 0.0; if (std::fabs(clipped_acc) < 1e-05) { next_pre_vel = pre_vel; } else { @@ -533,8 +538,9 @@ void AdaptiveCruiseController::registerQueToVelocity( delete_idxs.push_back(i); } } - for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { - est_vel_que_.erase(est_vel_que_.begin() + delete_idxs.at(delete_idx)); + // for (size_t delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { + for (const auto & delete_idx : boost::adaptors::reverse(delete_idxs)) { + est_vel_que_.erase(est_vel_que_.begin() + delete_idx); } // append new que @@ -547,7 +553,7 @@ void AdaptiveCruiseController::registerQueToVelocity( double AdaptiveCruiseController::getMedianVel( const std::vector & vel_que) { - if (vel_que.size() == 0) { + if (vel_que.empty()) { RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); return 0.0; } @@ -557,7 +563,7 @@ double AdaptiveCruiseController::getMedianVel( raw_vel_que.emplace_back(vel.twist.linear.x); } - double med_vel; + double med_vel = 0.0; if (raw_vel_que.size() % 2 == 0) { size_t med1 = (raw_vel_que.size()) / 2 - 1; size_t med2 = (raw_vel_que.size()) / 2; diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index a290e817..86c226aa 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -154,22 +154,22 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; - for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { - for (size_t j = 0; j < vehicle_polygons_.at(i).outer().size(); ++j) { + for (auto & vehicle_polygon : vehicle_polygons_) { + for (size_t j = 0; j < vehicle_polygon.outer().size(); ++j) { { geometry_msgs::msg::Point point = autoware_utils::toMsg( - vehicle_polygons_.at(i).outer().at( + vehicle_polygon.outer().at( j)); marker.points.push_back(point); } - if (j + 1 == vehicle_polygons_.at(i).outer().size()) { + if (j + 1 == vehicle_polygon.outer().size()) { geometry_msgs::msg::Point point = autoware_utils::toMsg( - vehicle_polygons_.at(i).outer().at( + vehicle_polygon.outer().at( 0)); marker.points.push_back(point); } else { geometry_msgs::msg::Point point = autoware_utils::toMsg( - vehicle_polygons_.at(i).outer().at( + vehicle_polygon.outer().at( j + 1)); marker.points.push_back(point); } @@ -201,24 +201,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; - for (size_t i = 0; i < collision_polygons_.size(); ++i) { - for (size_t j = 0; j < collision_polygons_.at(i).outer().size(); ++j) { + for (auto & collision_polygon : collision_polygons_) { + for (size_t j = 0; j < collision_polygon.outer().size(); ++j) { { geometry_msgs::msg::Point point = autoware_utils::toMsg( - collision_polygons_.at( - i).outer().at(j)); + collision_polygon.outer().at(j)); marker.points.push_back(point); } - if (j + 1 == collision_polygons_.at(i).outer().size()) { + if (j + 1 == collision_polygon.outer().size()) { geometry_msgs::msg::Point point = autoware_utils::toMsg( - collision_polygons_.at( - i).outer().at(0)); + collision_polygon.outer().at(0)); marker.points.push_back(point); } else { geometry_msgs::msg::Point point = autoware_utils::toMsg( - collision_polygons_.at( - i).outer().at(j + 1)); + collision_polygon.outer().at(j + 1)); marker.points.push_back(point); } } @@ -249,24 +246,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; - for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { - for (size_t j = 0; j < slow_down_range_polygons_.at(i).outer().size(); ++j) { + for (auto & slow_down_range_polygon : slow_down_range_polygons_) { + for (size_t j = 0; j < slow_down_range_polygon.outer().size(); ++j) { { geometry_msgs::msg::Point point = autoware_utils::toMsg( - slow_down_range_polygons_.at( - i).outer().at(j)); + slow_down_range_polygon.outer().at(j)); marker.points.push_back(point); } - if (j + 1 == slow_down_range_polygons_.at(i).outer().size()) { + if (j + 1 == slow_down_range_polygon.outer().size()) { geometry_msgs::msg::Point point = autoware_utils::toMsg( - slow_down_range_polygons_.at( - i).outer().at(0)); + slow_down_range_polygon.outer().at(0)); marker.points.push_back(point); } else { geometry_msgs::msg::Point point = autoware_utils::toMsg( - slow_down_range_polygons_.at( - i).outer().at(j + 1)); + slow_down_range_polygon.outer().at(j + 1)); marker.points.push_back(point); } } @@ -297,24 +291,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; - for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { - for (size_t j = 0; j < slow_down_polygons_.at(i).outer().size(); ++j) { + for (auto & slow_down_polygon : slow_down_polygons_) { + for (size_t j = 0; j < slow_down_polygon.outer().size(); ++j) { { geometry_msgs::msg::Point point = autoware_utils::toMsg( - slow_down_polygons_.at( - i).outer().at(j)); + slow_down_polygon.outer().at(j)); marker.points.push_back(point); } - if (j + 1 == slow_down_polygons_.at(i).outer().size()) { + if (j + 1 == slow_down_polygon.outer().size()) { geometry_msgs::msg::Point point = autoware_utils::toMsg( - slow_down_polygons_.at( - i).outer().at(0)); + slow_down_polygon.outer().at(0)); marker.points.push_back(point); } else { geometry_msgs::msg::Point point = autoware_utils::toMsg( - slow_down_polygons_.at( - i).outer().at(j + 1)); + slow_down_polygon.outer().at(j + 1)); marker.points.push_back(point); } } diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index c2de9c4e..51de39af 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -168,7 +168,7 @@ void ObstacleStopPlannerNode::pathCallback( */ auto self_pose = getSelfPose(input_msg->header, tf_buffer_); autoware_planning_msgs::msg::Trajectory trim_trajectory; - size_t trajectory_trim_index; + size_t trajectory_trim_index = 0; std::tie(trim_trajectory, trajectory_trim_index) = trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); @@ -195,13 +195,13 @@ void ObstacleStopPlannerNode::pathCallback( */ // for collision bool is_collision = false; - size_t decimate_trajectory_collision_index; + size_t decimate_trajectory_collision_index = 0; Point3d nearest_collision_point; rclcpp::Time nearest_collision_point_time; // for slow down bool candidate_slow_down = false; bool is_slow_down = false; - size_t decimate_trajectory_slow_down_index; + size_t decimate_trajectory_slow_down_index = 0; Point3d nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; @@ -272,12 +272,12 @@ void ObstacleStopPlannerNode::pathCallback( move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); - const auto nearest_slow_down_pointstamped = point_helper.getNearestPoint( + const auto nearest_slow_down_pointstamped = PointHelper::getNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); nearest_slow_down_point = nearest_slow_down_pointstamped.point; nearest_collision_point_time = nearest_slow_down_pointstamped.time; - const auto lateral_nearest_slow_down_point = point_helper.getLateralNearestPoint( + const auto lateral_nearest_slow_down_point = PointHelper::getLateralNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); lateral_deviation = lateral_nearest_slow_down_point.deviation; debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); @@ -287,7 +287,7 @@ void ObstacleStopPlannerNode::pathCallback( * search nearest collision point by beginning of path */ if (is_collision) { - const auto nearest_collision_pointstamped = point_helper.getNearestPoint( + const auto nearest_collision_pointstamped = PointHelper::getNearestPoint( *collision_pointcloud_ptr, trajectory.points.at(i).pose); nearest_collision_point = nearest_collision_pointstamped.point; nearest_collision_point_time = nearest_collision_pointstamped.time; @@ -437,7 +437,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownP if (slow_down_start_point.index <= output_msg.points.size()) { autoware_planning_msgs::msg::TrajectoryPoint slowdown_trajectory_point; std::tie(slowdown_trajectory_point, output_msg) = - point_helper.insertSlowDownStartPoint( + PointHelper::insertSlowDownStartPoint( slow_down_start_point, base_path, output_msg); debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); output_msg = insertSlowDownVelocity( @@ -477,7 +477,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertStopPoint if (stop_point.index <= output_msg.points.size()) { autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; std::tie(trajectory_point, output_msg) = - point_helper.insertStopPoint(stop_point, base_path, output_msg); + PointHelper::insertStopPoint(stop_point, base_path, output_msg); debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); } break; @@ -528,7 +528,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownV return output_path; } -double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) +double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) const { return param_.min_slow_down_vel + (param_.max_slow_down_vel - param_.min_slow_down_vel) * diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index a51a13a5..5c7378fb 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -41,7 +41,7 @@ void ObstaclePointCloud::updatePointCloud(const sensor_msgs::msg::PointCloud2::C no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); } filter.setInputCloud(no_height_pointcloud_ptr); - filter.setLeafSize(0.05f, 0.05f, 100000.0f); + filter.setLeafSize(0.05F, 0.05F, 100000.0F); filter.filter(*no_height_filtered_pointcloud_ptr); pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); obstacle_ros_pointcloud_ptr_->header = msg->header; diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 11cbb6b7..4d10d0dd 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -21,24 +21,24 @@ namespace obstacle_stop_planner { -const Point3d pointXYZtoPoint3d(pcl::PointXYZ point) +Point3d pointXYZtoPoint3d(pcl::PointXYZ point) { - return Point3d(point.x, point.y, point.z); + return Point3d{point.x, point.y, point.z}; } Point2d PointHelper::getBackwardPointFromBasePoint( const Point2d & line_point1, const Point2d & line_point2, - const Point2d & base_point, const double backward_length) const + const Point2d & base_point, const double backward_length) { const auto line_vec = Eigen::Vector2d(line_point2) - line_point1; const auto backward_vec = backward_length * line_vec.normalized(); const auto output_point = Eigen::Vector2d(base_point) + backward_vec; - return Point2d(output_point.x(), output_point.y()); + return Point2d{output_point.x(), output_point.y()}; } PointStamped PointHelper::getNearestPoint( const pcl::PointCloud & pointcloud, - const geometry_msgs::msg::Pose & base_pose) const + const geometry_msgs::msg::Pose & base_pose) { const double yaw = getYawFromQuaternion(base_pose.orientation); Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; @@ -68,23 +68,23 @@ PointStamped PointHelper::getNearestPoint( PointDeviation PointHelper::getLateralNearestPoint( const pcl::PointCloud & pointcloud, - const geometry_msgs::msg::Pose & base_pose) const + const geometry_msgs::msg::Pose & base_pose) { double min_norm = std::numeric_limits::max(); const double yaw = getYawFromQuaternion(base_pose.orientation); Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; PointDeviation lateral_nearest_point; - for (size_t i = 0; i < pointcloud.size(); ++i) { + for (auto i : pointcloud) { Point2d pointcloud_vec { - pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y}; + i.x - base_pose.position.x, + i.y - base_pose.position.y}; double norm = std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); if (norm < min_norm) { min_norm = norm; - lateral_nearest_point.point = pointcloud.at(i); + lateral_nearest_point.point = i; } } lateral_nearest_point.deviation = min_norm; @@ -94,7 +94,7 @@ PointDeviation PointHelper::getLateralNearestPoint( std::tuple PointHelper::insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - const autoware_planning_msgs::msg::Trajectory & input_path) const + const autoware_planning_msgs::msg::Trajectory & input_path) { auto output_path = input_path; autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = @@ -110,7 +110,7 @@ PointHelper::insertStopPoint( } StopPoint PointHelper::searchInsertPoint( - const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, + const size_t idx, const autoware_planning_msgs::msg::Trajectory & base_path, const Point2d & trajectory_vec, const Point2d & collision_point_vec) const { const auto max_dist_stop_point = @@ -122,8 +122,8 @@ StopPoint PointHelper::searchInsertPoint( // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; - for (int j = max_dist_stop_point.index - 1; j < static_cast(idx); ++j) { - if (base_path.points.at(std::max(j, 0)).twist.linear.x == 0.0) { + for (size_t j = max_dist_stop_point.index - 1; j < idx; ++j) { + if (base_path.points.at(std::max(j, size_t(0))).twist.linear.x == 0.0) { is_inserted_already_stop_point = true; break; } @@ -138,9 +138,9 @@ StopPoint PointHelper::searchInsertPoint( } StopPoint PointHelper::createTargetPoint( - const int idx, const double margin, const Point2d & trajectory_vec, + const size_t idx, const double margin, const Point2d & trajectory_vec, const Point2d & collision_point_vec, - const autoware_planning_msgs::msg::Trajectory & base_path) const + const autoware_planning_msgs::msg::Trajectory & base_path) { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(collision_point_vec); @@ -207,7 +207,7 @@ std::tuple(input_trajectory.points.size()) - 1; ++i) { if (next_length <= trajectory_length_sum + epsilon) { @@ -40,7 +39,7 @@ DecimateTrajectoryMap decimateTrajectory( input_trajectory.points.at(i).pose.position).to_2d(); const auto line_end_point = autoware_utils::fromMsg( input_trajectory.points.at(i + 1).pose.position).to_2d(); - Point2d interpolated_point = point_helper.getBackwardPointFromBasePoint( + Point2d interpolated_point = PointHelper::getBackwardPointFromBasePoint( line_start_point, line_end_point, line_end_point, -1.0 * (trajectory_length_sum - next_length)); From dfe42b1f809d00e99c32bc1cbf13c355a56a3b90 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 10:20:21 +0900 Subject: [PATCH 45/62] Remove OpenCV dependency --- obstacle_stop_planner_refine/CMakeLists.txt | 5 ----- obstacle_stop_planner_refine/package.xml | 1 - 2 files changed, 6 deletions(-) diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 77a6b2f8..ce44abf8 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) find_package(PCL REQUIRED) ament_auto_add_executable(obstacle_stop_planner_node @@ -30,12 +29,10 @@ ament_auto_add_executable(obstacle_stop_planner_node target_include_directories(obstacle_stop_planner_node PUBLIC - ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) target_link_libraries(obstacle_stop_planner_node - ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) @@ -52,11 +49,9 @@ if(BUILD_TESTING) ) target_include_directories(test_obstacle_stop_planner PRIVATE - ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) target_link_libraries(test_obstacle_stop_planner - ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) endif() diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index 72366d5d..07b0e588 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -26,7 +26,6 @@ tf2_eigen tf2_ros visualization_msgs - libopencv-dev ament_cmake_gtest ament_lint_auto From 64e794bb86ec590ea97f7146b0f2d97566eeb57c Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 14:17:11 +0900 Subject: [PATCH 46/62] create ament_cmake_auto_gtest package --- ament_cmake_auto_gtest/CMakeLists.txt | 14 +++++++++++++ .../ament_cmake_auto_gtest-extras.cmake | 20 +++++++++++++++++++ .../cmake/ament_auto_add_gtest.cmake | 0 ament_cmake_auto_gtest/package.xml | 19 ++++++++++++++++++ obstacle_stop_planner_refine/CMakeLists.txt | 3 +-- obstacle_stop_planner_refine/package.xml | 2 +- 6 files changed, 55 insertions(+), 3 deletions(-) create mode 100644 ament_cmake_auto_gtest/CMakeLists.txt create mode 100644 ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake rename obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake => ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake (100%) create mode 100644 ament_cmake_auto_gtest/package.xml diff --git a/ament_cmake_auto_gtest/CMakeLists.txt b/ament_cmake_auto_gtest/CMakeLists.txt new file mode 100644 index 00000000..768e2eb2 --- /dev/null +++ b/ament_cmake_auto_gtest/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5) + +project(ament_cmake_auto_gtest NONE) + +find_package(ament_cmake REQUIRED) + +ament_package( + CONFIG_EXTRAS "ament_cmake_auto_gtest-extras.cmake" +) + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) diff --git a/ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake b/ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake new file mode 100644 index 00000000..cbdf4472 --- /dev/null +++ b/ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake @@ -0,0 +1,20 @@ +# Copyright 2014 Open Source Robotics Foundation, 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. + +# copied from ament_cmake_auto/ament_cmake_auto-extras.cmake + +find_package(ament_cmake_auto QUIET REQUIRED) +find_package(ament_cmake_gtest QUIET REQUIRED) + +include("${ament_cmake_auto_gtest_DIR}/ament_auto_add_gtest.cmake") diff --git a/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake b/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake similarity index 100% rename from obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake rename to ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake diff --git a/ament_cmake_auto_gtest/package.xml b/ament_cmake_auto_gtest/package.xml new file mode 100644 index 00000000..0c8f1f8c --- /dev/null +++ b/ament_cmake_auto_gtest/package.xml @@ -0,0 +1,19 @@ + + + + ament_cmake_auto_gtest + 0.9.2 + The auto-magic functions for ease to use of the ament buildsystem in CMake. + Dirk Thomas + Apache License 2.0 + + ament_cmake + ament_cmake_gtest + + ament_cmake_auto + ament_cmake_gtest + + + ament_cmake + + diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index ce44abf8..d7534df5 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -40,8 +40,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) - include(test/cmake/ament_cmake_auto_gtest-extras.cmake) + find_package(ament_cmake_auto_gtest REQUIRED) file(GLOB_RECURSE test_files test/**/*.cpp) ament_auto_add_gtest(test_obstacle_stop_planner test/test_obstacle_stop_planner.test diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index 07b0e588..43938fae 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -27,7 +27,7 @@ tf2_ros visualization_msgs - ament_cmake_gtest + ament_cmake_auto_gtest ament_lint_auto ament_lint_common From c8b7cc69878aafc83eb521f9b8e9874c299c9251 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 15:00:54 +0900 Subject: [PATCH 47/62] Add readme --- ament_cmake_auto_gtest/README.md | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 ament_cmake_auto_gtest/README.md diff --git a/ament_cmake_auto_gtest/README.md b/ament_cmake_auto_gtest/README.md new file mode 100644 index 00000000..04e7b08d --- /dev/null +++ b/ament_cmake_auto_gtest/README.md @@ -0,0 +1,3 @@ +NOTE: This package is temporary and will be removed after the following pull requests have been merged. + + From eeceeb77a683719d85467571610429c717f83b4c Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 16:22:45 +0900 Subject: [PATCH 48/62] Replace tuple with optional --- .../adaptive_cruise_control.hpp | 13 ++++---- .../src/adaptive_cruise_control.cpp | 33 +++++++++++-------- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index a167228d..23911a0b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -20,9 +20,10 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tf2/utils.h" +#include "boost/optional.hpp" +#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_utils/autoware_utils.hpp" @@ -33,6 +34,7 @@ using autoware_utils::Point2d; using autoware_utils::Point3d; using autoware_utils::Polygon2d; using autoware_utils::Polygon3d; +using boost::optional; class AdaptiveCruiseController { @@ -186,14 +188,13 @@ class AdaptiveCruiseController const rclcpp::Time & nearest_collision_point_time); static double calcTrajYaw( const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx); - std::tuple estimatePointVelocityFromObject( + optional estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const Point2d & nearest_collision_point, - const double old_velocity); - std::tuple estimatePointVelocityFromPcl( + const Point2d & nearest_collision_point); + optional estimatePointVelocityFromPcl( const double traj_yaw, const Point2d & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, const double old_velocity); + const rclcpp::Time & nearest_collision_point_time); double estimateRoughPointVelocity(const double current_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel) const; diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 002f28c6..3c8a1fa9 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -137,13 +137,19 @@ AdaptiveCruiseController::insertAdaptiveCruiseVelocity( * estimate velocity of collision point */ if (param_.use_pcl_to_est_vel) { - std::tie(success_estm_vel, point_velocity) = estimatePointVelocityFromPcl( - traj_yaw, nearest_collision_point, nearest_collision_point_time, point_velocity); + const auto velocity = estimatePointVelocityFromPcl( + traj_yaw, nearest_collision_point, nearest_collision_point_time); + if (velocity) { + point_velocity = velocity.get(); + } } if (param_.use_object_to_est_vel) { - std::tie(success_estm_vel, point_velocity) = estimatePointVelocityFromObject( - object_ptr, traj_yaw, nearest_collision_point, point_velocity); + const auto velocity = estimatePointVelocityFromObject( + object_ptr, traj_yaw, nearest_collision_point); + if (velocity) { + point_velocity = velocity.get(); + } } if (param_.use_rough_est_vel && !success_estm_vel) { @@ -251,11 +257,10 @@ double AdaptiveCruiseController::calcTrajYaw( return tf2::getYaw(trajectory.points.at(collision_point_idx).pose.orientation); } -std::tuple AdaptiveCruiseController::estimatePointVelocityFromObject( +optional AdaptiveCruiseController::estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const Point2d & nearest_collision_point, - const double old_velocity) + const Point2d & nearest_collision_point) { /* get object velocity, and current yaw */ bool get_obj = false; @@ -276,14 +281,14 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromObje if (get_obj) { const auto velocity = obj_vel * std::cos(obj_yaw - traj_yaw); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = velocity; - return std::forward_as_tuple(true, velocity); + return velocity; } - return std::forward_as_tuple(false, old_velocity); + return boost::none; } -std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( +optional AdaptiveCruiseController::estimatePointVelocityFromPcl( const double traj_yaw, const Point2d & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, const double old_velocity) + const rclcpp::Time & nearest_collision_point_time) { /* estimate velocity */ const double p_dt = nearest_collision_point_time.seconds() - prev_collision_point_time_.seconds(); @@ -296,7 +301,7 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_time_ = nearest_collision_point_time; prev_collision_point_ = nearest_collision_point; prev_collision_point_valid_ = true; - return std::forward_as_tuple(false, old_velocity); + return boost::none; } const double p_dist = boost::geometry::distance(nearest_collision_point, prev_collision_point_); const auto p_diff = nearest_collision_point - prev_collision_point_; @@ -309,7 +314,7 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_ = nearest_collision_point; prev_collision_point_valid_ = true; est_vel_que_.clear(); - return std::forward_as_tuple(false, old_velocity); + return boost::none; } // append new velocity and remove old velocity from que @@ -324,7 +329,7 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_ = nearest_collision_point; prev_target_velocity_ = velocity; prev_collision_point_valid_ = true; - return std::forward_as_tuple(true, velocity); + return velocity; } double AdaptiveCruiseController::estimateRoughPointVelocity(const double current_vel) From 06ec3b66c5fef49535fa9b0c88f1a4c3eff1a168 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 16:51:05 +0900 Subject: [PATCH 49/62] add pcl_ros --- obstacle_stop_planner_refine/CMakeLists.txt | 17 ----------------- obstacle_stop_planner_refine/package.xml | 1 + 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index d7534df5..522537f0 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) ament_auto_add_executable(obstacle_stop_planner_node src/debug_marker.cpp @@ -27,15 +26,6 @@ ament_auto_add_executable(obstacle_stop_planner_node src/trajectory.cpp ) -target_include_directories(obstacle_stop_planner_node - PUBLIC - ${PCL_INCLUDE_DIRS} -) - -target_link_libraries(obstacle_stop_planner_node - ${PCL_LIBRARIES} -) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -46,13 +36,6 @@ if(BUILD_TESTING) test/test_obstacle_stop_planner.test ${test_files} ) - target_include_directories(test_obstacle_stop_planner - PRIVATE - ${PCL_INCLUDE_DIRS} - ) - target_link_libraries(test_obstacle_stop_planner - ${PCL_LIBRARIES} - ) endif() ament_auto_package( diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index 43938fae..c1b67637 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -26,6 +26,7 @@ tf2_eigen tf2_ros visualization_msgs + pcl_ros ament_cmake_auto_gtest ament_lint_auto From f83300afa996407824ad9d2e3c71cd9cc0f4f3a7 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 17:44:09 +0900 Subject: [PATCH 50/62] Modify ObstaclePointCloud class --- obstacle_stop_planner_refine/CMakeLists.txt | 1 - .../include/obstacle_stop_planner/node.hpp | 2 +- .../obstacle_point_cloud.hpp | 108 ++++++++++++---- obstacle_stop_planner_refine/src/node.cpp | 12 +- .../src/obstacle_point_cloud.cpp | 119 ------------------ 5 files changed, 95 insertions(+), 147 deletions(-) delete mode 100644 obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 522537f0..ecdb2360 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -21,7 +21,6 @@ ament_auto_add_executable(obstacle_stop_planner_node src/node.cpp src/main.cpp src/adaptive_cruise_control.cpp - src/obstacle_point_cloud.cpp src/point_helper.cpp src/trajectory.cpp ) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 91f6d4ef..d2d2b737 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -64,7 +64,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::shared_ptr debug_ptr_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - ObstaclePointCloud obstacle_pointcloud_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; Param param_; /* diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 3adda2bc..3ec3416d 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -21,6 +21,7 @@ #include "pcl/point_cloud.h" #include "pcl/common/transforms.h" #include "pcl/filters/voxel_grid.h" +#include "tf2_eigen/tf2_eigen.h" #include "tf2_ros/transform_listener.h" #include "rclcpp/logger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" @@ -29,29 +30,94 @@ namespace obstacle_stop_planner { +inline sensor_msgs::msg::PointCloud2::ConstSharedPtr updatePointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + auto obstacle_ros_pointcloud_ptr = std::make_shared(); + pcl::VoxelGrid filter; + pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( + new pcl::PointCloud); + + pcl::fromROSMsg(*msg, *pointcloud_ptr); + + for (const auto & point : pointcloud_ptr->points) { + no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); + } + filter.setInputCloud(no_height_pointcloud_ptr); + filter.setLeafSize(0.05F, 0.05F, 100000.0F); + filter.filter(*no_height_filtered_pointcloud_ptr); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr); + obstacle_ros_pointcloud_ptr->header = msg->header; + return obstacle_ros_pointcloud_ptr; +} + +inline static pcl::PointCloud::Ptr searchPointcloudNearTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const pcl::PointCloud::Ptr input_pointcloud_ptr, + const Param & param) +{ + pcl::PointCloud::Ptr output_pointcloud_ptr( + new pcl::PointCloud); + const double squared_radius = getSearchRadius(param) * getSearchRadius(param); + for (const auto & trajectory_point : trajectory.points) { + const auto center_pose = getVehicleCenterFromBase( + trajectory_point.pose, + param.vehicle_info.vehicle_length, + param.vehicle_info.rear_overhang); + + for (const auto & point : input_pointcloud_ptr->points) { + const double x = center_pose.position.x - point.x; + const double y = center_pose.position.y - point.y; + const double squared_distance = x * x + y * y; + if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} + } + } + return output_pointcloud_ptr; +} -class ObstaclePointCloud +inline pcl::PointCloud::Ptr searchCandidateObstacle( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & obstacle_ros_pointcloud_ptr, + const tf2_ros::Buffer & tf_buffer, + const autoware_planning_msgs::msg::Trajectory & trajectory, + const Param & param, + const rclcpp::Logger & logger) { -public: - explicit ObstaclePointCloud(const rclcpp::Logger & logger); - - void updatePointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - - bool isDataReceived(); - pcl::PointCloud::Ptr searchCandidateObstacle( - const tf2_ros::Buffer & tf_buffer, - const autoware_planning_msgs::msg::Trajectory & trajectory, - const Param & param); - -private: - static pcl::PointCloud::Ptr searchPointcloudNearTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, - const pcl::PointCloud::Ptr input_pointcloud_ptr, - const Param & param); - - sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; - const rclcpp::Logger logger_; -}; + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer.lookupTransform( + trajectory.header.frame_id, obstacle_ros_pointcloud_ptr->header.frame_id, + obstacle_ros_pointcloud_ptr->header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + logger, + "[obstacle_stop_planner] Failed to look up transform from " << + trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr->header.frame_id); + // do not publish path + return nullptr; + } + + Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl::PointCloud::Ptr obstacle_pointcloud_pcl_ptr( + new pcl::PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr, *obstacle_pointcloud_pcl_ptr); + pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( + new pcl::PointCloud); + pcl::transformPointCloud( + *obstacle_pointcloud_pcl_ptr, + *transformed_obstacle_pointcloud_ptr, + affine_matrix); + + // search obstacle candidate pointcloud to reduce calculation cost + auto obstacle_candidate_pointcloud_ptr = searchPointcloudNearTrajectory( + trajectory, transformed_obstacle_pointcloud_ptr, + param); + obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; + return obstacle_candidate_pointcloud_ptr; +} } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 51de39af..30953441 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -47,8 +47,7 @@ namespace obstacle_stop_planner ObstacleStopPlannerNode::ObstacleStopPlannerNode() : Node("obstacle_stop_planner"), tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - obstacle_pointcloud_(this->get_logger()) + tf_listener_(tf_buffer_) { // Vehicle Info auto i = vehicle_info_util::VehicleInfo::create(*this); @@ -135,12 +134,13 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() void ObstacleStopPlannerNode::obstaclePointcloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { - obstacle_pointcloud_.updatePointCloud(input_msg); + obstacle_pointcloud_ptr_ = updatePointCloud(input_msg); } + void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) { - if (!obstacle_pointcloud_.isDataReceived()) { + if (!obstacle_pointcloud_ptr_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for obstacle pointcloud..."); @@ -188,7 +188,9 @@ void ObstacleStopPlannerNode::pathCallback( new pcl::PointCloud); // search obstacle candidate pointcloud to reduce calculation cost - obstacle_pointcloud_.searchCandidateObstacle(tf_buffer_, trajectory, param_); + obstacle_candidate_pointcloud_ptr = searchCandidateObstacle( + obstacle_pointcloud_ptr_, + tf_buffer_, trajectory, param_, this->get_logger()); /* * check collision, slow_down diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp deleted file mode 100644 index 5c7378fb..00000000 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ /dev/null @@ -1,119 +0,0 @@ -// Copyright 2021 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 "obstacle_stop_planner/obstacle_point_cloud.hpp" -#include "obstacle_stop_planner/util.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/utils.h" -#include "tf2_eigen/tf2_eigen.h" - -namespace obstacle_stop_planner -{ -ObstaclePointCloud::ObstaclePointCloud(const rclcpp::Logger & logger) -: logger_(logger) -{ -} - -void ObstaclePointCloud::updatePointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -{ - obstacle_ros_pointcloud_ptr_ = std::make_shared(); - pcl::VoxelGrid filter; - pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( - new pcl::PointCloud); - - pcl::fromROSMsg(*msg, *pointcloud_ptr); - - for (const auto & point : pointcloud_ptr->points) { - no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); - } - filter.setInputCloud(no_height_pointcloud_ptr); - filter.setLeafSize(0.05F, 0.05F, 100000.0F); - filter.filter(*no_height_filtered_pointcloud_ptr); - pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); - obstacle_ros_pointcloud_ptr_->header = msg->header; -} - -bool ObstaclePointCloud::isDataReceived() -{ - return obstacle_ros_pointcloud_ptr_ != nullptr ? true : false; -} - -pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( - const tf2_ros::Buffer & tf_buffer, - const autoware_planning_msgs::msg::Trajectory & trajectory, - const Param & param) -{ - // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer.lookupTransform( - trajectory.header.frame_id, obstacle_ros_pointcloud_ptr_->header.frame_id, - obstacle_ros_pointcloud_ptr_->header.stamp, rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - logger_, - "[obstacle_stop_planner] Failed to look up transform from " << - trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr_->header.frame_id); - // do not publish path - return nullptr; - } - - Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl::PointCloud::Ptr obstacle_pointcloud_pcl_ptr_( - new pcl::PointCloud); - pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_pointcloud_pcl_ptr_); - pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( - new pcl::PointCloud); - pcl::transformPointCloud( - *obstacle_pointcloud_pcl_ptr_, - *transformed_obstacle_pointcloud_ptr, - affine_matrix); - - // search obstacle candidate pointcloud to reduce calculation cost - auto obstacle_candidate_pointcloud_ptr = searchPointcloudNearTrajectory( - trajectory, transformed_obstacle_pointcloud_ptr, - param); - obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; - return obstacle_candidate_pointcloud_ptr; -} - -pcl::PointCloud::Ptr ObstaclePointCloud::searchPointcloudNearTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, - const pcl::PointCloud::Ptr input_pointcloud_ptr, - const Param & param) -{ - pcl::PointCloud::Ptr output_pointcloud_ptr( - new pcl::PointCloud); - const double squared_radius = getSearchRadius(param) * getSearchRadius(param); - for (const auto & trajectory_point : trajectory.points) { - const auto center_pose = getVehicleCenterFromBase( - trajectory_point.pose, - param.vehicle_info.vehicle_length, - param.vehicle_info.rear_overhang); - - for (const auto & point : input_pointcloud_ptr->points) { - const double x = center_pose.position.x - point.x; - const double y = center_pose.position.y - point.y; - const double squared_distance = x * x + y * y; - if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} - } - } - return output_pointcloud_ptr; -} - -} // namespace obstacle_stop_planner From 388a88da78139dcbf128018eb9008fdb328816c6 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 18:37:22 +0900 Subject: [PATCH 51/62] add build_depends --- .github/workflows/build_and_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index fe18847e..a093d1c3 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -28,7 +28,7 @@ jobs: with: package-name: obstacle_stop_planner_refine target-ros2-distro: foxy - vcs-repo-file-url: build_depends.repos + vcs-repo-file-url: build_depends.repos src/autoware/AutowareArchitectureProposal.iv/build_depends.repos colcon-mixin-name: coverage-gcc coverage-pytest # If possible, pin the repository in the workflow to a specific commit to avoid # changes in colcon-mixin-repository from breaking your tests. From 64a1121874ca2b2bbba2bcf10d1303576bfc1cd1 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 18:50:46 +0900 Subject: [PATCH 52/62] fix path --- .github/workflows/build_and_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index a093d1c3..8ad3f5f4 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -28,7 +28,7 @@ jobs: with: package-name: obstacle_stop_planner_refine target-ros2-distro: foxy - vcs-repo-file-url: build_depends.repos src/autoware/AutowareArchitectureProposal.iv/build_depends.repos + vcs-repo-file-url: build_depends.repos ${{ github.workspace }}/src/autoware/AutowareArchitectureProposal.iv/build_depends.repos colcon-mixin-name: coverage-gcc coverage-pytest # If possible, pin the repository in the workflow to a specific commit to avoid # changes in colcon-mixin-repository from breaking your tests. From 4e7e19d06d8308678f6f61a3731f436bbe5842c6 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 20:07:14 +0900 Subject: [PATCH 53/62] fix path --- .github/workflows/build_and_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index 8ad3f5f4..d9fa651f 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -28,7 +28,7 @@ jobs: with: package-name: obstacle_stop_planner_refine target-ros2-distro: foxy - vcs-repo-file-url: build_depends.repos ${{ github.workspace }}/src/autoware/AutowareArchitectureProposal.iv/build_depends.repos + vcs-repo-file-url: build_depends.repos file://${{ github.workspace }}/src/autoware/AutowareArchitectureProposal.iv/build_depends.repos colcon-mixin-name: coverage-gcc coverage-pytest # If possible, pin the repository in the workflow to a specific commit to avoid # changes in colcon-mixin-repository from breaking your tests. From 0b0ff6480bc1b0906a90e0bad2d72a0c30e8fc16 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 22 Mar 2021 20:33:16 +0900 Subject: [PATCH 54/62] append depends --- .github/workflows/build_and_test.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index d9fa651f..2b520c34 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -22,13 +22,17 @@ jobs: with: required-ros-distributions: foxy + - name: Concat build_depends.repos + run: | + curl -sSL https://raw.githubusercontent.com/tier4/AutowareArchitectureProposal.iv/ros2/build_depends.repos | sed '1d' >> build_depends.repos + - name: Run action-ros-ci id: action_ros_ci_step uses: ros-tooling/action-ros-ci@v0.1 with: package-name: obstacle_stop_planner_refine target-ros2-distro: foxy - vcs-repo-file-url: build_depends.repos file://${{ github.workspace }}/src/autoware/AutowareArchitectureProposal.iv/build_depends.repos + vcs-repo-file-url: build_depends.repos colcon-mixin-name: coverage-gcc coverage-pytest # If possible, pin the repository in the workflow to a specific commit to avoid # changes in colcon-mixin-repository from breaking your tests. From f720dd032d03e21cbe076288d519acc846944012 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 24 Mar 2021 18:26:17 +0900 Subject: [PATCH 55/62] Fix rebase error --- .../include/obstacle_stop_planner/node.hpp | 1 - .../obstacle_stop_planner/trajectory.hpp | 3 +++ obstacle_stop_planner_refine/package.xml | 1 - obstacle_stop_planner_refine/src/node.cpp | 6 +---- .../src/point_helper.cpp | 12 +++++----- .../src/trajectory.cpp | 22 +++++++++++++++++++ 6 files changed, 32 insertions(+), 13 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index d2d2b737..6df265e4 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -24,7 +24,6 @@ #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/expand_stop_range.hpp" -#include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "autoware_debug_msgs/msg/float32_stamped.hpp" #include "pcl/point_types.h" diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index 228e7f87..fe293b8e 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -31,6 +31,9 @@ struct DecimateTrajectoryMap std::map index_map; }; +autoware_planning_msgs::msg::Trajectory extendTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const double extend_distance); DecimateTrajectoryMap decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const Param & param); diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index c1b67637..b4780069 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -12,7 +12,6 @@ rclcpp autoware_debug_msgs - autoware_utils autoware_perception_msgs autoware_planning_msgs autoware_utils diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 30953441..b1be39eb 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -22,7 +22,6 @@ #include #include "autoware_utils/geometry/geometry.hpp" -#include "diagnostic_msgs/msg/key_value.hpp" #include "pcl/filters/voxel_grid.h" #include "tf2/utils.h" #include "tf2_eigen/tf2_eigen.h" @@ -108,8 +107,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); - stop_reason_diag_pub_ = - this->create_publisher("~/output/stop_reason", 1); // Subscribers obstacle_pointcloud_sub_ = this->create_subscription( @@ -157,8 +154,7 @@ void ObstacleStopPlannerNode::pathCallback( /* * extend trajectory to consider obstacles after the goal */ - autoware_planning_msgs::msg::Trajectory extended_trajectory; - trajectory_.extendTrajectory(*input_msg, param_, extended_trajectory); + const auto extended_trajectory = extendTrajectory(*input_msg, param_.extend_distance); const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 4d10d0dd..4e93eae9 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -172,8 +172,8 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); - Eigen::Vector2d line_start_point{}; - Eigen::Vector2d line_end_point{}; + Point2d line_start_point{}; + Point2d line_end_point{}; SlowDownPoint slow_down_point{0, Point2d {0.0, 0.0}, 0.0}; for (size_t j = idx; 0 < j; --j) { @@ -188,11 +188,11 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( const double backward_length = length_sum - margin; if (backward_length < 0) { slow_down_point.index = 0; - slow_down_point.point = Eigen::Vector2d( + slow_down_point.point = Point2d( base_path.points.at(0).pose.position.x, base_path.points.at(0).pose.position.y); } else { - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_start_point, backward_length, slow_down_point.point); + slow_down_point.point = getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_start_point, backward_length); } slow_down_point.velocity = std::max( @@ -225,7 +225,7 @@ PointHelper::insertSlowDownStartPoint( output_path.points.insert( output_path.points.begin() + slow_down_start_point.index, slow_down_start_trajectory_point); } - return slow_down_start_trajectory_point; + return std::make_tuple(slow_down_start_trajectory_point, output_path); } autoware_planning_msgs::msg::TrajectoryPoint PointHelper::getExtendTrajectoryPoint( diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 10747cb4..5cdcc213 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -22,6 +22,28 @@ namespace obstacle_stop_planner { +autoware_planning_msgs::msg::Trajectory extendTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const double extend_distance) +{ + auto output_trajectory = input_trajectory; + const auto goal_point = input_trajectory.points.back(); + double interpolation_distance = 0.1; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = + PointHelper::getExtendTrajectoryPoint(extend_sum, goal_point); + output_trajectory.points.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = + PointHelper::getExtendTrajectoryPoint(extend_distance, goal_point); + output_trajectory.points.push_back(extend_trajectory_point); + + return output_trajectory; +} + DecimateTrajectoryMap decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const Param & /*param*/) From 490178f7121c7286a9b5c78b2e0972b1c5331c8e Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 29 Mar 2021 18:53:50 +0900 Subject: [PATCH 56/62] Ported code from the feature/create-node-pkg branch --- obstacle_stop_planner_refine/CMakeLists.txt | 29 +- .../config/vehicle_info.param.yaml | 11 + .../adaptive_cruise_control.hpp | 118 +--- .../include/obstacle_stop_planner/node.hpp | 70 +- .../obstacle_point_cloud.hpp | 14 +- .../obstacle_stop_planner.hpp | 127 ++++ .../one_step_polygon.hpp | 14 +- .../adaptive_cruise_control_parameter.hpp | 124 ++++ .../slow_down_control_parameter.hpp} | 28 +- .../parameter/stop_control_parameter.hpp | 33 + .../obstacle_stop_planner/point_helper.hpp | 19 +- .../obstacle_stop_planner/trajectory.hpp | 4 +- .../visibility_control.hpp | 37 ++ obstacle_stop_planner_refine/package.xml | 3 + .../src/adaptive_cruise_control.cpp | 85 +-- obstacle_stop_planner_refine/src/main.cpp | 26 - obstacle_stop_planner_refine/src/node.cpp | 596 ++++-------------- .../src/obstacle_stop_planner.cpp | 496 +++++++++++++++ .../src/point_helper.cpp | 16 +- .../src/trajectory.cpp | 3 +- .../obstacle_stop_planner_node_launch_test.py | 61 ++ .../src/test_create_vehicle_footprint.cpp | 8 +- .../test/src/test_obstacle_stop_planner.cpp | 115 ++++ .../src/test_obstacle_stop_planner_node.cpp | 183 ++++++ .../test/src/test_point_helper.cpp | 46 ++ .../test/src/test_util.cpp | 6 +- 26 files changed, 1452 insertions(+), 820 deletions(-) create mode 100644 obstacle_stop_planner_refine/config/vehicle_info.param.yaml create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp rename obstacle_stop_planner_refine/include/obstacle_stop_planner/{param.hpp => parameter/slow_down_control_parameter.hpp} (60%) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp delete mode 100644 obstacle_stop_planner_refine/src/main.cpp create mode 100644 obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp create mode 100644 obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py create mode 100644 obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_point_helper.cpp diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index ecdb2360..1654cb48 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -O3) + add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wno-unused-parameter) endif() @@ -16,19 +16,32 @@ ament_auto_find_build_dependencies() find_package(Eigen3 REQUIRED) -ament_auto_add_executable(obstacle_stop_planner_node - src/debug_marker.cpp - src/node.cpp - src/main.cpp - src/adaptive_cruise_control.cpp - src/point_helper.cpp - src/trajectory.cpp +file(GLOB_RECURSE OBSTACLE_STOP_PLANNER_NODES_NODE_SRC + src/* +) +file(GLOB_RECURSE OBSTACLE_STOP_PLANNER_NODES_NODE_HEADERS + include/obstacle_stop_planner/* +) + +# generate component node library +ament_auto_add_library(obstacle_stop_planner_node SHARED + ${OBSTACLE_STOP_PLANNER_NODES_NODE_SRC} + ${OBSTACLE_STOP_PLANNER_NODES_NODE_HEADERS} +) +rclcpp_components_register_node(obstacle_stop_planner_node + PLUGIN "obstacle_stop_planner::ObstacleStopPlannerNode" + EXECUTABLE obstacle_stop_planner_node_exe ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + add_ros_test( + test/obstacle_stop_planner_node_launch_test.py + TIMEOUT "30" + ) + find_package(ament_cmake_auto_gtest REQUIRED) file(GLOB_RECURSE test_files test/**/*.cpp) ament_auto_add_gtest(test_obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/config/vehicle_info.param.yaml b/obstacle_stop_planner_refine/config/vehicle_info.param.yaml new file mode 100644 index 00000000..b1279b50 --- /dev/null +++ b/obstacle_stop_planner_refine/config/vehicle_info.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + wheel_radius: 0.39 + wheel_width: 0.42 + wheel_base: 2.74 # between front wheel center and rear wheel center + wheel_tread: 1.63 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.03 # between rear wheel center and vehicle rear + left_overhang: 0.1 # between left wheel center and vehicle left + right_overhang: 0.1 # between right wheel center and vehicle right + vehicle_height: 2.5 diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 23911a0b..f9637f95 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -27,6 +27,7 @@ #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_utils/autoware_utils.hpp" +#include "obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp" namespace obstacle_stop_planner { @@ -40,8 +41,9 @@ class AdaptiveCruiseController { public: AdaptiveCruiseController( - rclcpp::Node * node, const double vehicle_width, const double vehicle_length, - const double wheel_base, const double front_overhang); + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const AdaptiveCruiseControlParameter & acc_param); std::tuple insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, @@ -59,10 +61,8 @@ class AdaptiveCruiseController /* * Parameter */ - double vehicle_width_; - double vehicle_length_; - double wheel_base_; - double front_overhang_; + VehicleInfo vehicle_info_; + AdaptiveCruiseControlParameter param_; rclcpp::Time prev_collision_point_time_; Point2d prev_collision_point_; @@ -72,111 +72,7 @@ class AdaptiveCruiseController bool prev_collision_point_valid_ = false; std::vector est_vel_que_; double prev_upper_velocity_ = 0.0; - - struct Param - { - double stop_margin; - double min_behavior_stop_margin; - - //!< @brief use tracking objects for estimating object velocity or not - bool use_object_to_est_vel; - - //!< @brief use pcl for estimating object velocity or not - bool use_pcl_to_est_vel; - - //!< @brief consider forward vehicle velocity to self upper velocity or not - bool consider_obj_velocity; - - //!< @brief The distance to extend the polygon length the object in pointcloud-object matching - double object_polygon_length_margin; - - //!< @brief The distance to extend the polygon width the object in pointcloud-object matching - double object_polygon_width_margin; - - //!< @breif Maximum time difference treated as continuous points in speed estimation using a - // point cloud - double valid_est_vel_diff_time; - - //!< @brief Time width of information used for speed estimation in speed estimation using a - // point cloud - double valid_vel_que_time; - - //!< @brief Maximum value of valid speed estimation results in speed estimation using a point - // cloud - double valid_est_vel_max; - - //!< @brief Minimum value of valid speed estimation results in speed estimation using a point - // cloud - double valid_est_vel_min; - - //!< @brief Embed a stop line if the maximum speed calculated by ACC is lower than this speed - double thresh_vel_to_stop; - - /* parameter for acc */ - //!< @brief threshold of forward obstacle velocity to insert stop line (to stop acc) - double obstacle_stop_velocity_thresh; - - //!< @brief supposed minimum acceleration in emergency stop - double emergency_stop_acceleration; - - //!< @brief supposed minimum acceleration of forward vehicle in emergency stop - double obstacle_emergency_stop_acceleration; - - //!< @brief supposed idling time to start emergency stop - double emergency_stop_idling_time; - - //!< @brief minimum distance of emergency stop - double min_dist_stop; - - //!< @brief supposed maximum acceleration in active cruise control - double max_standard_acceleration; - - //!< @brief supposed minimum acceleration(deceleration) in active cruise control - double min_standard_acceleration; - - //!< @brief supposed idling time to react object in active cruise control - double standard_idling_time; - - //!< @brief minimum distance in active cruise control - double min_dist_standard; - - //!< @brief supposed minimum acceleration of forward obstacle - double obstacle_min_standard_acceleration; - - //!< @brief margin to insert upper velocity - double margin_rate_to_change_vel; - - //!< @brief use time-compensation to calculate distance to forward vehicle - bool use_time_compensation_to_dist; - - //!< @brief gain of lowpass filter of upper velocity - double lowpass_gain_; - - //!< @brief when failed to estimate velocity, use rough velocity estimation or not - bool use_rough_est_vel; - - //!< @brief in rough velocity estimation, front car velocity is - //!< estimated as self current velocity * this value - double rough_velocity_rate; - - /* parameter for pid used in acc */ - //!< @brief coefficient P in PID control (used when target dist -current_dist >=0) - double p_coeff_pos; - - //!< @brief coefficient P in PID control (used when target dist -current_dist <0) - double p_coeff_neg; - - //!< @brief coefficient D in PID control (used when delta_dist >=0) - double d_coeff_pos; - - //!< @brief coefficient D in PID control (used when delta_dist <0) - double d_coeff_neg; - - static constexpr double d_coeff_valid_time = 1.0; - static constexpr double d_coeff_valid_diff_vel = 20.0; - static constexpr double d_max_vel_norm = 3.0; - }; - Param param_; + double min_behavior_stop_margin_ = 0.0; double getMedianVel(const std::vector & vel_que); static double lowpass_filter( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 6df265e4..f8706515 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -14,37 +14,24 @@ #ifndef OBSTACLE_STOP_PLANNER__NODE_HPP_ #define OBSTACLE_STOP_PLANNER__NODE_HPP_ -#include #include -#include -#include #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "obstacle_stop_planner/visibility_control.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/expand_stop_range.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "autoware_debug_msgs/msg/float32_stamped.hpp" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_cloud.h" -#include "pcl/common/transforms.h" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tf2/utils.h" -#include "tf2_ros/transform_listener.h" -#include "obstacle_stop_planner/adaptive_cruise_control.hpp" -#include "obstacle_stop_planner/debug_marker.hpp" -#include "obstacle_stop_planner/obstacle_point_cloud.hpp" -#include "obstacle_stop_planner/point_helper.hpp" -#include "obstacle_stop_planner/util.hpp" +#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "obstacle_stop_planner/obstacle_stop_planner.hpp" namespace obstacle_stop_planner { -class ObstacleStopPlannerNode : public rclcpp::Node +class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlannerNode : public rclcpp::Node { public: - ObstacleStopPlannerNode(); + explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & options); private: /* @@ -60,20 +47,22 @@ class ObstacleStopPlannerNode : public rclcpp::Node expand_stop_range_sub_; rclcpp::Publisher::SharedPtr path_pub_; - std::shared_ptr debug_ptr_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; - Param param_; /* * Parameter */ - std::unique_ptr acc_controller_; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_; rclcpp::Time prev_col_point_time_; pcl::PointXYZ prev_col_point_; + bool pointcloud_received_; + bool current_velocity_reveived_; + + std::unique_ptr planner_; + void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); void pathCallback(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg); void dynamicObjectCallback( @@ -81,45 +70,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg); void externalExpandStopRangeCallback( const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); - -private: - geometry_msgs::msg::Pose getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer); - autoware_planning_msgs::msg::Trajectory insertSlowDownVelocity( - const size_t slow_down_start_point_idx, - const double slow_down_target_vel, - const double slow_down_vel, - const autoware_planning_msgs::msg::Trajectory & input_path); - double calcSlowDownTargetVel(const double lateral_deviation) const; - static std::tuple::Ptr> getSlowDownPointcloud( - const bool is_slow_down, const bool enable_slow_down, - const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, - const Point2d & prev_center_point, - const Point2d & next_center_point, - const double search_radius, - const Polygon2d & one_step_polygon, - const pcl::PointCloud::Ptr slow_down_pointcloud, - const bool candidate_slow_down); - std::tuple::Ptr> getCollisionPointcloud( - const pcl::PointCloud::Ptr slow_down_pointcloud, - const Point2d & prev_center_point, - const Point2d & next_center_point, - const double search_radius, const Polygon2d & one_step_polygon, - const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, - const pcl::PointCloud::Ptr collision_pointcloud, - const bool is_collision); - autoware_planning_msgs::msg::Trajectory insertStopPoint( - const size_t search_start_index, - const autoware_planning_msgs::msg::Trajectory & base_path, - const Point2d & nearest_collision_point, - const autoware_planning_msgs::msg::Trajectory & input_msg); - autoware_planning_msgs::msg::Trajectory insertSlowDownPoint( - const size_t search_start_index, - const autoware_planning_msgs::msg::Trajectory & base_path, - const Point2d & nearest_slow_down_point, - const double slow_down_target_vel, - const double slow_down_margin, - const autoware_planning_msgs::msg::Trajectory & input_msg); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 3ec3416d..deadae9c 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -26,7 +26,6 @@ #include "rclcpp/logger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "obstacle_stop_planner/param.hpp" namespace obstacle_stop_planner { @@ -56,16 +55,17 @@ inline sensor_msgs::msg::PointCloud2::ConstSharedPtr updatePointCloud( inline static pcl::PointCloud::Ptr searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, - const Param & param) + const double search_radius, + const VehicleInfo & param) { pcl::PointCloud::Ptr output_pointcloud_ptr( new pcl::PointCloud); - const double squared_radius = getSearchRadius(param) * getSearchRadius(param); + const double squared_radius = search_radius * search_radius; for (const auto & trajectory_point : trajectory.points) { const auto center_pose = getVehicleCenterFromBase( trajectory_point.pose, - param.vehicle_info.vehicle_length, - param.vehicle_info.rear_overhang); + param.vehicle_length, + param.rear_overhang); for (const auto & point : input_pointcloud_ptr->points) { const double x = center_pose.position.x - point.x; @@ -81,7 +81,8 @@ inline pcl::PointCloud::Ptr searchCandidateObstacle( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & obstacle_ros_pointcloud_ptr, const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory, - const Param & param, + const double search_radius, + const VehicleInfo & param, const rclcpp::Logger & logger) { // transform pointcloud @@ -114,6 +115,7 @@ inline pcl::PointCloud::Ptr searchCandidateObstacle( // search obstacle candidate pointcloud to reduce calculation cost auto obstacle_candidate_pointcloud_ptr = searchPointcloudNearTrajectory( trajectory, transformed_obstacle_pointcloud_ptr, + search_radius, param); obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; return obstacle_candidate_pointcloud_ptr; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp new file mode 100644 index 00000000..67aa30a9 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_stop_planner.hpp @@ -0,0 +1,127 @@ +// Copyright 2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef OBSTACLE_STOP_PLANNER__OBSTACLE_STOP_PLANNER_HPP_ +#define OBSTACLE_STOP_PLANNER__OBSTACLE_STOP_PLANNER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "obstacle_stop_planner/visibility_control.hpp" +#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/expand_stop_range.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "autoware_debug_msgs/msg/float32_stamped.hpp" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl/point_cloud.h" +#include "pcl/common/transforms.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_listener.h" +#include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "obstacle_stop_planner/debug_marker.hpp" +#include "obstacle_stop_planner/obstacle_point_cloud.hpp" +#include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/util.hpp" + +namespace obstacle_stop_planner +{ +class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlanner +{ +public: + ObstacleStopPlanner( + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const StopControlParameter & stop_param, + const SlowDownControlParameter & slow_down_param, + const AdaptiveCruiseControlParameter & acc_param); + + void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); + autoware_planning_msgs::msg::Trajectory + pathCallback( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg, + tf2_ros::Buffer & tf_buffer); + void dynamicObjectCallback( + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg); + void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg); + void externalExpandStopRangeCallback( + const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); + +private: + rclcpp::Node * node_; + std::shared_ptr debug_ptr_; + // tf2_ros::Buffer tf_buffer_; + // tf2_ros::TransformListener tf_listener_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; + VehicleInfo vehicle_info_; + StopControlParameter stop_param_; + SlowDownControlParameter slow_down_param_; + AdaptiveCruiseControlParameter acc_param_; + + /* + * Parameter + */ + std::unique_ptr acc_controller_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_; + rclcpp::Time prev_col_point_time_; + pcl::PointXYZ prev_col_point_; + +private: + geometry_msgs::msg::Pose getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer); + autoware_planning_msgs::msg::Trajectory insertSlowDownVelocity( + const size_t slow_down_start_point_idx, + const double slow_down_target_vel, + const double slow_down_vel, + const autoware_planning_msgs::msg::Trajectory & input_path); + double calcSlowDownTargetVel(const double lateral_deviation) const; + static std::tuple::Ptr> getSlowDownPointcloud( + const bool is_slow_down, const bool enable_slow_down, + const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, + const Polygon2d & one_step_polygon, + const pcl::PointCloud::Ptr slow_down_pointcloud, + const bool candidate_slow_down); + std::tuple::Ptr> getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, const Polygon2d & one_step_polygon, + const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, + const pcl::PointCloud::Ptr collision_pointcloud, + const bool is_collision); + autoware_planning_msgs::msg::Trajectory insertStopPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_collision_point, + const autoware_planning_msgs::msg::Trajectory & input_msg); + autoware_planning_msgs::msg::Trajectory insertSlowDownPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_slow_down_point, + const double slow_down_target_vel, + const double slow_down_margin, + const autoware_planning_msgs::msg::Trajectory & input_msg); +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__OBSTACLE_STOP_PLANNER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 928f9dd4..1019f27e 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -38,12 +38,13 @@ inline Polygon2d createOneStepPolygon( boost::geometry::strategy::transform::rotate_transformer< boost::geometry::radian, double, 2, 2> rotate(yaw); - autoware_utils::LinearRing2d transformed_footprint; - boost::geometry::transform(footprint, transformed_footprint, rotate); + autoware_utils::LinearRing2d rotate_footprint; + boost::geometry::transform(footprint, rotate_footprint, rotate); boost::geometry::strategy::transform::translate_transformer translate( base_step_pose.position.x, base_step_pose.position.y); - boost::geometry::transform(transformed_footprint, transformed_footprint, translate); + autoware_utils::LinearRing2d transformed_footprint; + boost::geometry::transform(rotate_footprint, transformed_footprint, translate); one_step_move_vehicle_corner_points.outer() = transformed_footprint; } // next step @@ -51,12 +52,13 @@ inline Polygon2d createOneStepPolygon( double yaw = getYawFromQuaternion(next_step_pose.orientation); boost::geometry::strategy::transform::rotate_transformer< boost::geometry::radian, double, 2, 2> rotate(yaw); - autoware_utils::LinearRing2d transformed_footprint; - boost::geometry::transform(footprint, transformed_footprint, rotate); + autoware_utils::LinearRing2d rotate_footprint; + boost::geometry::transform(footprint, rotate_footprint, rotate); boost::geometry::strategy::transform::translate_transformer translate( base_step_pose.position.x, base_step_pose.position.y); - boost::geometry::transform(transformed_footprint, transformed_footprint, translate); + autoware_utils::LinearRing2d transformed_footprint; + boost::geometry::transform(rotate_footprint, transformed_footprint, translate); for (const auto & item : transformed_footprint) { one_step_move_vehicle_corner_points.outer().emplace_back(item); } diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp new file mode 100644 index 00000000..1c055b3a --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp @@ -0,0 +1,124 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__PARAMETER__ADAPTIVE_CRUISE_CONTROL_PARAMETER_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAMETER__ADAPTIVE_CRUISE_CONTROL_PARAMETER_HPP_ + +#include + +namespace obstacle_stop_planner +{ +struct OBSTACLE_STOP_PLANNER_PUBLIC AdaptiveCruiseControlParameter +{ + //!< @brief use tracking objects for estimating object velocity or not + bool use_object_to_est_vel; + + //!< @brief use pcl for estimating object velocity or not + bool use_pcl_to_est_vel; + + //!< @brief consider forward vehicle velocity to self upper velocity or not + bool consider_obj_velocity; + + //!< @brief The distance to extend the polygon length the object in pointcloud-object matching + double object_polygon_length_margin; + + //!< @brief The distance to extend the polygon width the object in pointcloud-object matching + double object_polygon_width_margin; + + //!< @breif Maximum time difference treated as continuous points in speed estimation using a + // point cloud + double valid_est_vel_diff_time; + + //!< @brief Time width of information used for speed estimation in speed estimation using a + // point cloud + double valid_vel_que_time; + + //!< @brief Maximum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_max; + + //!< @brief Minimum value of valid speed estimation results in speed estimation using a point + // cloud + double valid_est_vel_min; + + //!< @brief Embed a stop line if the maximum speed calculated by ACC is lower than this speed + double thresh_vel_to_stop; + + /* parameter for acc */ + //!< @brief threshold of forward obstacle velocity to insert stop line (to stop acc) + double obstacle_stop_velocity_thresh; + + //!< @brief supposed minimum acceleration in emergency stop + double emergency_stop_acceleration; + + //!< @brief supposed minimum acceleration of forward vehicle in emergency stop + double obstacle_emergency_stop_acceleration; + + //!< @brief supposed idling time to start emergency stop + double emergency_stop_idling_time; + + //!< @brief minimum distance of emergency stop + double min_dist_stop; + + //!< @brief supposed maximum acceleration in active cruise control + double max_standard_acceleration; + + //!< @brief supposed minimum acceleration(deceleration) in active cruise control + double min_standard_acceleration; + + //!< @brief supposed idling time to react object in active cruise control + double standard_idling_time; + + //!< @brief minimum distance in active cruise control + double min_dist_standard; + + //!< @brief supposed minimum acceleration of forward obstacle + double obstacle_min_standard_acceleration; + + //!< @brief margin to insert upper velocity + double margin_rate_to_change_vel; + + //!< @brief use time-compensation to calculate distance to forward vehicle + bool use_time_compensation_to_dist; + + //!< @brief gain of lowpass filter of upper velocity + double lowpass_gain_; + + //!< @brief when failed to estimate velocity, use rough velocity estimation or not + bool use_rough_est_vel; + + //!< @brief in rough velocity estimation, front car velocity is + //!< estimated as self current velocity * this value + double rough_velocity_rate; + + /* parameter for pid used in acc */ + //!< @brief coefficient P in PID control (used when target dist -current_dist >=0) + double p_coeff_pos; + + //!< @brief coefficient P in PID control (used when target dist -current_dist <0) + double p_coeff_neg; + + //!< @brief coefficient D in PID control (used when delta_dist >=0) + double d_coeff_pos; + + //!< @brief coefficient D in PID control (used when delta_dist <0) + double d_coeff_neg; + + const double d_coeff_valid_time = 1.0; + const double d_coeff_valid_diff_vel = 20.0; + const double d_max_vel_norm = 3.0; +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__PARAMETER__ADAPTIVE_CRUISE_CONTROL_PARAMETER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp similarity index 60% rename from obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp rename to obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp index 34af69d3..1f670a53 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/slow_down_control_parameter.hpp @@ -12,41 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_STOP_PLANNER__PARAM_HPP_ -#define OBSTACLE_STOP_PLANNER__PARAM_HPP_ +#ifndef OBSTACLE_STOP_PLANNER__PARAMETER__SLOW_DOWN_CONTROL_PARAMETER_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAMETER__SLOW_DOWN_CONTROL_PARAMETER_HPP_ -#include "autoware_utils/autoware_utils.hpp" +#include namespace obstacle_stop_planner { - -struct Param +struct OBSTACLE_STOP_PLANNER_PUBLIC SlowDownControlParameter { - VehicleInfo vehicle_info; - double stop_margin; - double min_behavior_stop_margin; - double step_length; - double extend_distance; - double expand_stop_range; double slow_down_margin; double expand_slow_down_range; double max_slow_down_vel; double min_slow_down_vel; double max_deceleration; bool enable_slow_down; - double stop_search_radius; double slow_down_search_radius; }; - -inline double getSearchRadius(const Param & param) -{ - if (param.enable_slow_down) { - return param.slow_down_search_radius; - } else { - return param.stop_search_radius; - } -} - } // namespace obstacle_stop_planner -#endif // OBSTACLE_STOP_PLANNER__PARAM_HPP_ +#endif // OBSTACLE_STOP_PLANNER__PARAMETER__SLOW_DOWN_CONTROL_PARAMETER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp new file mode 100644 index 00000000..1c1186df --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/parameter/stop_control_parameter.hpp @@ -0,0 +1,33 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__PARAMETER__STOP_CONTROL_PARAMETER_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAMETER__STOP_CONTROL_PARAMETER_HPP_ + +#include + +namespace obstacle_stop_planner +{ +struct OBSTACLE_STOP_PLANNER_PUBLIC StopControlParameter +{ + double stop_margin; + double min_behavior_stop_margin; + double step_length; + double extend_distance; + double expand_stop_range; + double stop_search_radius; +}; +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__PARAMETER__STOP_CONTROL_PARAMETER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index 67e5e53b..179b06a7 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -24,11 +24,8 @@ #include "geometry_msgs/msg/pose.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "obstacle_stop_planner/util.hpp" -#include "obstacle_stop_planner/param.hpp" - -#define EIGEN_MPL2_ONLY -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" +#include "obstacle_stop_planner/parameter/slow_down_control_parameter.hpp" +#include "obstacle_stop_planner/parameter/stop_control_parameter.hpp" namespace obstacle_stop_planner @@ -62,9 +59,6 @@ struct PointDeviation class PointHelper { public: - explicit PointHelper(const Param & param) - : param_(param) {} - static Point2d getBackwardPointFromBasePoint( const Point2d & line_point1, const Point2d & line_point2, const Point2d & base_point, const double backward_length); @@ -83,7 +77,8 @@ class PointHelper StopPoint searchInsertPoint( const size_t idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Point2d & trajectory_vec, const Point2d & collision_point_vec) const; + const Point2d & trajectory_vec, const Point2d & collision_point_vec, + const StopControlParameter & param) const; static StopPoint createTargetPoint( const size_t idx, const double margin, const Point2d & trajectory_vec, @@ -94,7 +89,8 @@ class PointHelper const int idx, const double margin, const double slow_down_target_vel, const Point2d & trajectory_vec, const Point2d & slow_down_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path, - const double current_velocity_x) const; + const double current_velocity_x, + const SlowDownControlParameter & param) const; static std::tuple @@ -105,9 +101,6 @@ class PointHelper static autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point); - -private: - const Param param_; }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index fe293b8e..fa9336f6 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -19,7 +19,6 @@ #include #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "obstacle_stop_planner/param.hpp" namespace obstacle_stop_planner { @@ -35,8 +34,7 @@ autoware_planning_msgs::msg::Trajectory extendTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double extend_distance); DecimateTrajectoryMap decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & param); + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length); std::tuple trimTrajectoryWithIndexFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const geometry_msgs::msg::Pose & self_pose); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp new file mode 100644 index 00000000..6151990b --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__VISIBILITY_CONTROL_HPP_ +#define OBSTACLE_STOP_PLANNER__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(OBSTACLE_STOP_PLANNER_BUILDING_DLL) || defined(OBSTACLE_STOP_PLANNER_EXPORTS) + #define OBSTACLE_STOP_PLANNER_PUBLIC __declspec(dllexport) + #define OBSTACLE_STOP_PLANNER_LOCAL + #else + #define OBSTACLE_STOP_PLANNER_PUBLIC __declspec(dllimport) + #define OBSTACLE_STOP_PLANNER_LOCAL + #endif +#elif defined(__linux__) + #define OBSTACLE_STOP_PLANNER_PUBLIC __attribute__((visibility("default"))) + #define OBSTACLE_STOP_PLANNER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define OBSTACLE_STOP_PLANNER_PUBLIC __attribute__((visibility("default"))) + #define OBSTACLE_STOP_PLANNER_LOCAL __attribute__((visibility("hidden"))) +#else + #error "Unsupported Build Configuration" +#endif + +#endif // OBSTACLE_STOP_PLANNER__VISIBILITY_CONTROL_HPP_ diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index b4780069..a63b95f2 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -11,6 +11,8 @@ ament_cmake_auto rclcpp + rclcpp_components + autoware_debug_msgs autoware_perception_msgs autoware_planning_msgs @@ -30,6 +32,7 @@ ament_cmake_auto_gtest ament_lint_auto ament_lint_common + ros_testing ament_cmake diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 3c8a1fa9..339667a5 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -32,72 +32,14 @@ namespace obstacle_stop_planner { AdaptiveCruiseController::AdaptiveCruiseController( - rclcpp::Node * node, const double vehicle_width, const double vehicle_length, - const double wheel_base, const double front_overhang) + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const AdaptiveCruiseControlParameter & acc_param) : node_(node), - vehicle_width_(vehicle_width), - vehicle_length_(vehicle_length), - wheel_base_(wheel_base), - front_overhang_(front_overhang), + vehicle_info_(vehicle_info), + param_(acc_param), prev_collision_point_(0.0, 0.0) { - // get parameter - const std::string acc_ns = "adaptive_cruise_control."; - - /* config */ - param_.min_behavior_stop_margin = - node_->get_parameter("stop_planner.min_behavior_stop_margin").as_double() + wheel_base_ + - front_overhang_; - param_.use_object_to_est_vel = - node_->declare_parameter(acc_ns + "use_object_to_estimate_vel", true); - param_.use_pcl_to_est_vel = node_->declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true); - param_.consider_obj_velocity = node_->declare_parameter(acc_ns + "consider_obj_velocity", true); - - /* parameter for acc */ - param_.obstacle_stop_velocity_thresh = - node_->declare_parameter(acc_ns + "obstacle_stop_velocity_thresh", 1.0); - param_.emergency_stop_acceleration = - node_->declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5); - param_.obstacle_emergency_stop_acceleration = - node_->declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0); - param_.emergency_stop_idling_time = - node_->declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5); - param_.min_dist_stop = node_->declare_parameter(acc_ns + "min_dist_stop", 4.0); - param_.max_standard_acceleration = - node_->declare_parameter(acc_ns + "max_standard_acceleration", 0.5); - param_.min_standard_acceleration = - node_->declare_parameter(acc_ns + "min_standard_acceleration", -1.0); - param_.standard_idling_time = node_->declare_parameter(acc_ns + "standard_idling_time", 0.5); - param_.min_dist_standard = node_->declare_parameter(acc_ns + "min_dist_standard", 4.0); - param_.obstacle_min_standard_acceleration = - node_->declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5); - param_.margin_rate_to_change_vel = - node_->declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3); - param_.use_time_compensation_to_dist = - node_->declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true); - param_.lowpass_gain_ = node_->declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6); - - /* parameter for pid in acc */ - param_.p_coeff_pos = node_->declare_parameter(acc_ns + "p_coefficient_positive", 0.1); - param_.p_coeff_neg = node_->declare_parameter(acc_ns + "p_coefficient_negative", 0.3); - param_.d_coeff_pos = node_->declare_parameter(acc_ns + "d_coefficient_positive", 0.0); - param_.d_coeff_neg = node_->declare_parameter(acc_ns + "d_coefficient_negative", 0.1); - - /* parameter for speed estimation of obstacle */ - param_.object_polygon_length_margin = - node_->declare_parameter(acc_ns + "object_polygon_length_margin", 2.0); - param_.object_polygon_width_margin = - node_->declare_parameter(acc_ns + "object_polygon_width_margin", 0.5); - param_.valid_est_vel_diff_time = - node_->declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0); - param_.valid_vel_que_time = node_->declare_parameter(acc_ns + "valid_vel_que_time", 0.5); - param_.valid_est_vel_max = node_->declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0); - param_.valid_est_vel_min = node_->declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0); - param_.thresh_vel_to_stop = node_->declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5); - param_.use_rough_est_vel = - node_->declare_parameter(acc_ns + "use_rough_velocity_estimation", false); - param_.rough_velocity_rate = node_->declare_parameter(acc_ns + "rough_velocity_rate", 0.9); - /* publisher */ pub_debug_ = node_->create_publisher( "~/debug_values", @@ -204,9 +146,10 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( // get self polygon geometry_msgs::msg::Vector3 self_size; - self_size.x = vehicle_length_; - self_size.y = vehicle_width_; - double self_offset = (wheel_base_ + front_overhang_) - vehicle_length_ / 2.0; + self_size.x = vehicle_info_.vehicle_length; + self_size.y = vehicle_info_.vehicle_width; + double self_offset = (vehicle_info_.wheel_base + vehicle_info_.front_overhang) - + vehicle_info_.vehicle_length / 2.0; const auto self_poly = getPolygon(self_pose, self_size, self_offset); // get nearest point @@ -237,7 +180,7 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( autoware_utils::fromMsg(trajectory.points.at(nearest_point_idx - 1).pose.position).to_2d()); // subtract base_link to front - dist_to_point -= param_.min_behavior_stop_margin; + dist_to_point -= min_behavior_stop_margin_; // time compensation if (param_.use_time_compensation_to_dist) { @@ -430,7 +373,7 @@ double AdaptiveCruiseController::calcTargetVelocity_D( const double target_dist, const double current_dist) { if (node_->now().seconds() - prev_target_vehicle_time_ >= - AdaptiveCruiseController::Param::d_coeff_valid_time) + param_.d_coeff_valid_time) { // invalid time(prev is too old) return 0.0; @@ -439,7 +382,7 @@ double AdaptiveCruiseController::calcTargetVelocity_D( double diff_vel = (target_dist - prev_target_vehicle_dist_) / (node_->now().seconds() - prev_target_vehicle_time_); - if (std::fabs(diff_vel) >= AdaptiveCruiseController::Param::d_coeff_valid_diff_vel) { + if (std::fabs(diff_vel) >= param_.d_coeff_valid_diff_vel) { // invalid(discontinuous) diff_vel return 0.0; } @@ -449,8 +392,8 @@ double AdaptiveCruiseController::calcTargetVelocity_D( add_vel_d = diff_vel; add_vel_d = boost::algorithm::clamp( add_vel_d, - -AdaptiveCruiseController::Param::d_max_vel_norm, - AdaptiveCruiseController::Param::d_max_vel_norm); + -param_.d_max_vel_norm, + param_.d_max_vel_norm); // add buffer prev_target_vehicle_dist_ = current_dist; diff --git a/obstacle_stop_planner_refine/src/main.cpp b/obstacle_stop_planner_refine/src/main.cpp deleted file mode 100644 index 21c0d8f6..00000000 --- a/obstacle_stop_planner_refine/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2019 Autoware Foundation -// -// 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 "rclcpp/rclcpp.hpp" -#include "obstacle_stop_planner/node.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index b1be39eb..14cc2e6f 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -12,97 +12,131 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include -#include -#include -#include -#include "autoware_utils/geometry/geometry.hpp" -#include "pcl/filters/voxel_grid.h" -#include "tf2/utils.h" -#include "tf2_eigen/tf2_eigen.h" -#include "boost/assert.hpp" -#include "boost/assign/list_of.hpp" -#include "boost/format.hpp" -#include "boost/geometry.hpp" -#include "boost/geometry/geometries/linestring.hpp" -#include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" -#include "obstacle_stop_planner/util.hpp" -#include "obstacle_stop_planner/one_step_polygon.hpp" -#include "obstacle_stop_planner/trajectory.hpp" #include "vehicle_info_util/vehicle_info.hpp" - -#define EIGEN_MPL2_ONLY -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" +#include "obstacle_stop_planner/parameter/stop_control_parameter.hpp" +#include "obstacle_stop_planner/parameter/slow_down_control_parameter.hpp" +#include "obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp" namespace obstacle_stop_planner { -ObstacleStopPlannerNode::ObstacleStopPlannerNode() -: Node("obstacle_stop_planner"), +ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) +: Node{"obstacle_stop_planner", node_options}, tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_) + tf_listener_(tf_buffer_), + pointcloud_received_(false), + current_velocity_reveived_(false) { // Vehicle Info + VehicleInfo vehicle_info; auto i = vehicle_info_util::VehicleInfo::create(*this); - param_.vehicle_info.wheel_radius = i.wheel_radius_m_; - param_.vehicle_info.wheel_width = i.wheel_width_m_; - param_.vehicle_info.wheel_base = i.wheel_base_m_; - param_.vehicle_info.wheel_tread = i.wheel_tread_m_; - param_.vehicle_info.front_overhang = i.front_overhang_m_; - param_.vehicle_info.rear_overhang = i.rear_overhang_m_; - param_.vehicle_info.left_overhang = i.left_overhang_m_; - param_.vehicle_info.right_overhang = i.right_overhang_m_; - param_.vehicle_info.vehicle_height = i.vehicle_height_m_; - param_.vehicle_info.vehicle_length = i.vehicle_length_m_; - param_.vehicle_info.vehicle_width = i.vehicle_width_m_; - param_.vehicle_info.min_longitudinal_offset = i.min_longitudinal_offset_m_; - param_.vehicle_info.max_longitudinal_offset = i.max_longitudinal_offset_m_; - param_.vehicle_info.min_lateral_offset = i.min_lateral_offset_m_; - param_.vehicle_info.max_lateral_offset = i.max_lateral_offset_m_; - param_.vehicle_info.min_height_offset = i.min_height_offset_m_; - param_.vehicle_info.max_height_offset = i.max_height_offset_m_; + vehicle_info.wheel_radius = i.wheel_radius_m_; + vehicle_info.wheel_width = i.wheel_width_m_; + vehicle_info.wheel_base = i.wheel_base_m_; + vehicle_info.wheel_tread = i.wheel_tread_m_; + vehicle_info.front_overhang = i.front_overhang_m_; + vehicle_info.rear_overhang = i.rear_overhang_m_; + vehicle_info.left_overhang = i.left_overhang_m_; + vehicle_info.right_overhang = i.right_overhang_m_; + vehicle_info.vehicle_height = i.vehicle_height_m_; + vehicle_info.vehicle_length = i.vehicle_length_m_; + vehicle_info.vehicle_width = i.vehicle_width_m_; + vehicle_info.min_longitudinal_offset = i.min_longitudinal_offset_m_; + vehicle_info.max_longitudinal_offset = i.max_longitudinal_offset_m_; + vehicle_info.min_lateral_offset = i.min_lateral_offset_m_; + vehicle_info.max_lateral_offset = i.max_lateral_offset_m_; + vehicle_info.min_height_offset = i.min_height_offset_m_; + vehicle_info.max_height_offset = i.max_height_offset_m_; // Parameters - param_.stop_margin = declare_parameter("stop_planner.stop_margin", 5.0); - param_.min_behavior_stop_margin = declare_parameter("stop_planner.min_behavior_stop_margin", 2.0); - param_.step_length = declare_parameter("stop_planner.step_length", 1.0); - param_.extend_distance = declare_parameter("stop_planner.extend_distance", 0.0); - param_.expand_stop_range = declare_parameter("stop_planner.expand_stop_range", 0.0); - - param_.slow_down_margin = declare_parameter("slow_down_planner.slow_down_margin", 5.0); - param_.expand_slow_down_range = + obstacle_stop_planner::StopControlParameter stop_param; + stop_param.stop_margin = declare_parameter("stop_planner.stop_margin", 5.0); + stop_param.min_behavior_stop_margin = + declare_parameter("stop_planner.min_behavior_stop_margin", 2.0); + stop_param.step_length = declare_parameter("stop_planner.step_length", 1.0); + stop_param.extend_distance = declare_parameter("stop_planner.extend_distance", 0.0); + stop_param.expand_stop_range = declare_parameter("stop_planner.expand_stop_range", 0.0); + + obstacle_stop_planner::SlowDownControlParameter slow_down_param; + slow_down_param.slow_down_margin = declare_parameter("slow_down_planner.slow_down_margin", 5.0); + slow_down_param.expand_slow_down_range = declare_parameter("slow_down_planner.expand_slow_down_range", 1.0); - param_.max_slow_down_vel = declare_parameter("slow_down_planner.max_slow_down_vel", 4.0); - param_.min_slow_down_vel = declare_parameter("slow_down_planner.min_slow_down_vel", 2.0); - param_.max_deceleration = declare_parameter("slow_down_planner.max_deceleration", 2.0); - param_.enable_slow_down = declare_parameter("enable_slow_down", false); - - param_.stop_margin += param_.vehicle_info.wheel_base + param_.vehicle_info.front_overhang; - param_.min_behavior_stop_margin += - param_.vehicle_info.wheel_base + param_.vehicle_info.front_overhang; - param_.slow_down_margin += param_.vehicle_info.wheel_base + param_.vehicle_info.front_overhang; - param_.stop_search_radius = param_.step_length + std::hypot( - param_.vehicle_info.vehicle_width / 2.0 + param_.expand_stop_range, - param_.vehicle_info.vehicle_length / 2.0); - param_.slow_down_search_radius = param_.step_length + std::hypot( - param_.vehicle_info.vehicle_width / 2.0 + param_.expand_slow_down_range, - param_.vehicle_info.vehicle_length / 2.0); - - debug_ptr_ = std::make_shared( + slow_down_param.max_slow_down_vel = declare_parameter("slow_down_planner.max_slow_down_vel", 4.0); + slow_down_param.min_slow_down_vel = declare_parameter("slow_down_planner.min_slow_down_vel", 2.0); + slow_down_param.max_deceleration = declare_parameter("slow_down_planner.max_deceleration", 2.0); + slow_down_param.enable_slow_down = declare_parameter("enable_slow_down", false); + + stop_param.stop_margin += vehicle_info.wheel_base + vehicle_info.front_overhang; + stop_param.min_behavior_stop_margin += + vehicle_info.wheel_base + vehicle_info.front_overhang; + slow_down_param.slow_down_margin += vehicle_info.wheel_base + vehicle_info.front_overhang; + stop_param.stop_search_radius = stop_param.step_length + std::hypot( + vehicle_info.vehicle_width / 2.0 + stop_param.expand_stop_range, + vehicle_info.vehicle_length / 2.0); + slow_down_param.slow_down_search_radius = stop_param.step_length + std::hypot( + vehicle_info.vehicle_width / 2.0 + slow_down_param.expand_slow_down_range, + vehicle_info.vehicle_length / 2.0); + + // Parameters for adaptive_cruise_control + obstacle_stop_planner::AdaptiveCruiseControlParameter acc_param; + std::string acc_ns = "adaptive_cruise_control."; + acc_param.use_object_to_est_vel = + declare_parameter(acc_ns + "use_object_to_estimate_vel", true); + acc_param.use_pcl_to_est_vel = declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true); + acc_param.consider_obj_velocity = declare_parameter(acc_ns + "consider_obj_velocity", true); + acc_param.obstacle_stop_velocity_thresh = + declare_parameter(acc_ns + "obstacle_stop_velocity_thresh", 1.0); + acc_param.emergency_stop_acceleration = + declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5); + acc_param.obstacle_emergency_stop_acceleration = + declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0); + acc_param.emergency_stop_idling_time = + declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5); + acc_param.min_dist_stop = declare_parameter(acc_ns + "min_dist_stop", 4.0); + acc_param.max_standard_acceleration = + declare_parameter(acc_ns + "max_standard_acceleration", 0.5); + acc_param.min_standard_acceleration = + declare_parameter(acc_ns + "min_standard_acceleration", -1.0); + acc_param.standard_idling_time = declare_parameter(acc_ns + "standard_idling_time", 0.5); + acc_param.min_dist_standard = declare_parameter(acc_ns + "min_dist_standard", 4.0); + acc_param.obstacle_min_standard_acceleration = + declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5); + acc_param.margin_rate_to_change_vel = + declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3); + acc_param.use_time_compensation_to_dist = + declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true); + acc_param.lowpass_gain_ = declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6); + + /* parameter for pid in acc */ + acc_param.p_coeff_pos = declare_parameter(acc_ns + "p_coefficient_positive", 0.1); + acc_param.p_coeff_neg = declare_parameter(acc_ns + "p_coefficient_negative", 0.3); + acc_param.d_coeff_pos = declare_parameter(acc_ns + "d_coefficient_positive", 0.0); + acc_param.d_coeff_neg = declare_parameter(acc_ns + "d_coefficient_negative", 0.1); + + /* parameter for speed estimation of obstacle */ + acc_param.object_polygon_length_margin = + declare_parameter(acc_ns + "object_polygon_length_margin", 2.0); + acc_param.object_polygon_width_margin = + declare_parameter(acc_ns + "object_polygon_width_margin", 0.5); + acc_param.valid_est_vel_diff_time = + declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0); + acc_param.valid_vel_que_time = declare_parameter(acc_ns + "valid_vel_que_time", 0.5); + acc_param.valid_est_vel_max = declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0); + acc_param.valid_est_vel_min = declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0); + acc_param.thresh_vel_to_stop = declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5); + acc_param.use_rough_est_vel = + declare_parameter(acc_ns + "use_rough_velocity_estimation", false); + acc_param.rough_velocity_rate = declare_parameter(acc_ns + "rough_velocity_rate", 0.9); + + planner_ = std::make_unique( this, - param_.vehicle_info.wheel_base + - param_.vehicle_info.front_overhang); - - // Initializer - acc_controller_ = std::make_unique( - this, param_.vehicle_info.vehicle_width, param_.vehicle_info.vehicle_length, - param_.vehicle_info.wheel_base, param_.vehicle_info.front_overhang); + vehicle_info, + stop_param, + slow_down_param, + acc_param); // Publishers path_pub_ = @@ -131,437 +165,51 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() void ObstacleStopPlannerNode::obstaclePointcloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { - obstacle_pointcloud_ptr_ = updatePointCloud(input_msg); + planner_->obstaclePointcloudCallback(input_msg); + pointcloud_received_ = true; } void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) { - if (!obstacle_pointcloud_ptr_) { + if (!pointcloud_received_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for obstacle pointcloud..."); return; } - if (!current_velocity_ptr_ && param_.enable_slow_down) { + if (!current_velocity_reveived_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for current velocity..."); return; } - /* - * extend trajectory to consider obstacles after the goal - */ - const auto extended_trajectory = extendTrajectory(*input_msg, param_.extend_distance); - - const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; - autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; - - /* - * trim trajectory from self pose - */ - auto self_pose = getSelfPose(input_msg->header, tf_buffer_); - autoware_planning_msgs::msg::Trajectory trim_trajectory; - size_t trajectory_trim_index = 0; - std::tie(trim_trajectory, trajectory_trim_index) = - trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); - - /* - * decimate trajectory for calculation cost - */ - DecimateTrajectoryMap decimate_trajectory_map = decimateTrajectory( - trim_trajectory, param_.step_length, param_); - - autoware_planning_msgs::msg::Trajectory & trajectory = - decimate_trajectory_map.decimate_trajectory; - - /* - * search candidate obstacle pointcloud - */ - pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( - new pcl::PointCloud); - - // search obstacle candidate pointcloud to reduce calculation cost - obstacle_candidate_pointcloud_ptr = searchCandidateObstacle( - obstacle_pointcloud_ptr_, - tf_buffer_, trajectory, param_, this->get_logger()); - - /* - * check collision, slow_down - */ - // for collision - bool is_collision = false; - size_t decimate_trajectory_collision_index = 0; - Point3d nearest_collision_point; - rclcpp::Time nearest_collision_point_time; - // for slow down - bool candidate_slow_down = false; - bool is_slow_down = false; - size_t decimate_trajectory_slow_down_index = 0; - Point3d nearest_slow_down_point; - pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); - double lateral_deviation = 0.0; - PointHelper point_helper {param_}; - - for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { - /* - * create one step circle center for vehicle - */ - const auto prev_center_pose = getVehicleCenterFromBase( - trajectory.points.at(i).pose, - param_.vehicle_info.vehicle_length, - param_.vehicle_info.rear_overhang); - const auto next_center_pose = getVehicleCenterFromBase( - trajectory.points.at(i + 1).pose, - param_.vehicle_info.vehicle_length, - param_.vehicle_info.rear_overhang); - - Point2d prev_center_point = autoware_utils::fromMsg(prev_center_pose.position).to_2d(); - Point2d next_center_point = autoware_utils::fromMsg(next_center_pose.position).to_2d(); - - /* - * create one step polygon for vehicle - */ - const auto move_vehicle_polygon = createOneStepPolygon( - trajectory.points.at(i).pose, - trajectory.points.at(i + 1).pose, - param_.expand_stop_range, - param_.vehicle_info); - debug_ptr_->pushPolygon( - move_vehicle_polygon, - trajectory.points.at(i).pose.position.z, - PolygonType::Vehicle); - - Polygon2d move_slow_down_range_polygon; - if (param_.enable_slow_down) { - /* - * create one step polygon for slow_down range - */ - move_slow_down_range_polygon = createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - param_.expand_slow_down_range, param_.vehicle_info); - debug_ptr_->pushPolygon( - move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, - PolygonType::SlowDownRange); - } - - // check within polygon - pcl::PointCloud::Ptr collision_pointcloud_ptr( - new pcl::PointCloud); - collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - - std::tie(candidate_slow_down, slow_down_pointcloud_ptr) = getSlowDownPointcloud( - is_slow_down, param_.enable_slow_down, - obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, - param_.slow_down_search_radius, - move_slow_down_range_polygon, slow_down_pointcloud_ptr, candidate_slow_down); - - std::tie(is_collision, collision_pointcloud_ptr) = getCollisionPointcloud( - slow_down_pointcloud_ptr, prev_center_point, next_center_point, - param_.stop_search_radius, move_vehicle_polygon, - trajectory.points.at(i), collision_pointcloud_ptr, is_collision); - - if (candidate_slow_down && !is_collision && !is_slow_down) { - is_slow_down = true; - decimate_trajectory_slow_down_index = i; - debug_ptr_->pushPolygon( - move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, - PolygonType::SlowDown); - - const auto nearest_slow_down_pointstamped = PointHelper::getNearestPoint( - *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); - nearest_slow_down_point = nearest_slow_down_pointstamped.point; - nearest_collision_point_time = nearest_slow_down_pointstamped.time; - - const auto lateral_nearest_slow_down_point = PointHelper::getLateralNearestPoint( - *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); - lateral_deviation = lateral_nearest_slow_down_point.deviation; - debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); - } - - /* - * search nearest collision point by beginning of path - */ - if (is_collision) { - const auto nearest_collision_pointstamped = PointHelper::getNearestPoint( - *collision_pointcloud_ptr, trajectory.points.at(i).pose); - nearest_collision_point = nearest_collision_pointstamped.point; - nearest_collision_point_time = nearest_collision_pointstamped.time; - debug_ptr_->pushObstaclePoint(nearest_collision_point, PointType::Stop); - decimate_trajectory_collision_index = i; - break; - } - } - - /* - * insert max velocity and judge if there is a need to stop - */ - bool need_to_stop = is_collision; - if (is_collision) { - std::tie(need_to_stop, output_msg) = acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory_map.decimate_trajectory, - decimate_trajectory_collision_index, - self_pose, nearest_collision_point.to_2d(), - nearest_collision_point_time, object_ptr_, - current_velocity_ptr_, - output_msg); - } - - /* - * insert stop point - */ - if (need_to_stop) { - output_msg = insertStopPoint( - decimate_trajectory_map.index_map.at(decimate_trajectory_collision_index) + - trajectory_trim_index, - base_path, - nearest_collision_point.to_2d(), - output_msg); - } - - /* - * insert slow_down point - */ - if (is_slow_down) { - output_msg = insertSlowDownPoint( - decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), - base_path, - nearest_slow_down_point.to_2d(), - calcSlowDownTargetVel(lateral_deviation), - param_.slow_down_margin, - output_msg); - } + const auto output_msg = planner_->pathCallback(input_msg, tf_buffer_); path_pub_->publish(output_msg); - debug_ptr_->publish(); -} - -// collision -std::tuple::Ptr> -ObstacleStopPlannerNode::getCollisionPointcloud( - const pcl::PointCloud::Ptr slow_down_pointcloud, - const Point2d & prev_center_point, - const Point2d & next_center_point, - const double search_radius, - const Polygon2d & one_step_polygon, - const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, - const pcl::PointCloud::Ptr collision_pointcloud, - const bool is_collision) -{ - auto output_pointcloud = collision_pointcloud; - auto output_collision = is_collision; - - for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { - Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); - if ( - boost::geometry::distance(prev_center_point, point) < search_radius || - boost::geometry::distance(next_center_point, point) < search_radius) - { - if (boost::geometry::within(point, one_step_polygon)) { - output_pointcloud->push_back(slow_down_pointcloud->at(j)); - output_collision = true; - debug_ptr_->pushPolygon( - one_step_polygon, trajectory_point.pose.position.z, - PolygonType::Collision); - } - } - } - return std::make_tuple(output_collision, output_pointcloud); -} - -// slow down -std::tuple::Ptr> -ObstacleStopPlannerNode::getSlowDownPointcloud( - const bool is_slow_down, - const bool enable_slow_down, - const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, - const Point2d & prev_center_point, - const Point2d & next_center_point, - const double search_radius, - const Polygon2d & one_step_polygon, - const pcl::PointCloud::Ptr slow_down_pointcloud, - const bool candidate_slow_down) -{ - auto output_pointcloud = slow_down_pointcloud; - auto output_candidate = candidate_slow_down; - - if (!is_slow_down && enable_slow_down) { - for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { - Point2d point( - obstacle_candidate_pointcloud->at(j).x, obstacle_candidate_pointcloud->at(j).y); - if ( - boost::geometry::distance(prev_center_point, point) < search_radius || - boost::geometry::distance(next_center_point, point) < search_radius) - { - if (boost::geometry::within(point, one_step_polygon)) { - output_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); - output_candidate = true; - } - } - } - } else { - output_pointcloud = obstacle_candidate_pointcloud; - } - return std::make_tuple(output_candidate, output_pointcloud); -} - -autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownPoint( - const size_t search_start_index, - const autoware_planning_msgs::msg::Trajectory & base_path, - const Point2d & nearest_slow_down_point, - const double slow_down_target_vel, const double slow_down_margin, - const autoware_planning_msgs::msg::Trajectory & input_msg) -{ - auto output_msg = input_msg; - PointHelper point_helper {param_}; - - for (size_t i = search_start_index; i < base_path.points.size(); ++i) { - const double yaw = - getYawFromQuaternion(base_path.points.at(i).pose.orientation); - const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; - const Point2d slow_down_point_vec { - nearest_slow_down_point.x() - base_path.points.at(i).pose.position.x, - nearest_slow_down_point.y() - base_path.points.at(i).pose.position.y}; - - if ( - trajectory_vec.dot(slow_down_point_vec) < 0.0 || - (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) - { - const auto slow_down_start_point = point_helper.createSlowDownStartPoint( - i, slow_down_margin, slow_down_target_vel, trajectory_vec, slow_down_point_vec, - base_path, current_velocity_ptr_->twist.linear.x); - - if (slow_down_start_point.index <= output_msg.points.size()) { - autoware_planning_msgs::msg::TrajectoryPoint slowdown_trajectory_point; - std::tie(slowdown_trajectory_point, output_msg) = - PointHelper::insertSlowDownStartPoint( - slow_down_start_point, base_path, output_msg); - debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); - output_msg = insertSlowDownVelocity( - slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, - output_msg); - } - break; - } - } - return output_msg; -} - -// stop -autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertStopPoint( - const size_t search_start_index, - const autoware_planning_msgs::msg::Trajectory & base_path, - const Point2d & nearest_collision_point, - const autoware_planning_msgs::msg::Trajectory & input_msg) -{ - auto output_msg = input_msg; - PointHelper point_helper {param_}; - - for (size_t i = search_start_index; i < base_path.points.size(); ++i) { - const double yaw = - getYawFromQuaternion(base_path.points.at(i).pose.orientation); - const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; - const Point2d collision_point_vec { - nearest_collision_point.x() - base_path.points.at(i).pose.position.x, - nearest_collision_point.y() - base_path.points.at(i).pose.position.y}; - - if ( - trajectory_vec.dot(collision_point_vec) < 0.0 || - (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) - { - const auto stop_point = - point_helper.searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); - if (stop_point.index <= output_msg.points.size()) { - autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; - std::tie(trajectory_point, output_msg) = - PointHelper::insertStopPoint(stop_point, base_path, output_msg); - debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); - } - break; - } - } - return output_msg; } void ObstacleStopPlannerNode::externalExpandStopRangeCallback( const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg) { - param_.expand_stop_range = input_msg->expand_stop_range; - param_.stop_search_radius = - param_.step_length + std::hypot( - param_.vehicle_info.vehicle_width / 2.0 + param_.expand_stop_range, - param_.vehicle_info.vehicle_length / 2.0); -} - -autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownVelocity( - const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, - const autoware_planning_msgs::msg::Trajectory & input_path) -{ - autoware_planning_msgs::msg::TrajectoryPoint slow_down_end_trajectory_point; - auto output_path = input_path; - bool is_slow_down_end = false; - - for (size_t j = slow_down_start_point_idx; j < output_path.points.size() - 1; ++j) { - output_path.points.at(j).twist.linear.x = - std::min(slow_down_vel, output_path.points.at(j).twist.linear.x); - const auto dist = std::hypot( - output_path.points.at(j).pose.position.x - output_path.points.at(j + 1).pose.position.x, - output_path.points.at(j).pose.position.y - output_path.points.at(j + 1).pose.position.y); - slow_down_vel = std::max( - slow_down_target_vel, - std::sqrt( - std::max( - slow_down_vel * slow_down_vel - 2 * param_.max_deceleration * dist, - 0.0))); - if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { - slow_down_end_trajectory_point = output_path.points.at(j + 1); - is_slow_down_end = true; - } - } - if (!is_slow_down_end) { - slow_down_end_trajectory_point = output_path.points.back(); - } - debug_ptr_->pushPose(slow_down_end_trajectory_point.pose, PoseType::SlowDownEnd); - return output_path; -} - -double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) const -{ - return param_.min_slow_down_vel + - (param_.max_slow_down_vel - param_.min_slow_down_vel) * - std::max(lateral_deviation - param_.vehicle_info.vehicle_width / 2, 0.0) / - param_.expand_slow_down_range; + planner_->externalExpandStopRangeCallback(input_msg); } void ObstacleStopPlannerNode::dynamicObjectCallback( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg) { - object_ptr_ = input_msg; + planner_->dynamicObjectCallback(input_msg); } void ObstacleStopPlannerNode::currentVelocityCallback( const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg) { - current_velocity_ptr_ = input_msg; -} - -geometry_msgs::msg::Pose ObstacleStopPlannerNode::getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer) -{ - geometry_msgs::msg::Pose self_pose; - try { - geometry_msgs::msg::TransformStamped transform; - transform = - tf_buffer.lookupTransform( - header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds( - 0.1)); - self_pose = autoware_utils::transform2pose(transform.transform); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "could not get self pose from tf_buffer."); - } - return self_pose; + planner_->currentVelocityCallback(input_msg); + current_velocity_reveived_ = true; } } // namespace obstacle_stop_planner + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE( + obstacle_stop_planner::ObstacleStopPlannerNode) diff --git a/obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp b/obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp new file mode 100644 index 00000000..22be0a06 --- /dev/null +++ b/obstacle_stop_planner_refine/src/obstacle_stop_planner.cpp @@ -0,0 +1,496 @@ +// 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 "autoware_utils/geometry/geometry.hpp" +#include "pcl/filters/voxel_grid.h" +#include "tf2/utils.h" +#include "tf2_eigen/tf2_eigen.h" +#include "boost/assert.hpp" +#include "boost/assign/list_of.hpp" +#include "boost/format.hpp" +#include "boost/geometry.hpp" +#include "boost/geometry/geometries/linestring.hpp" +#include "boost/geometry/geometries/point_xy.hpp" +#include "obstacle_stop_planner/obstacle_stop_planner.hpp" +#include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/one_step_polygon.hpp" +#include "obstacle_stop_planner/trajectory.hpp" +#include "vehicle_info_util/vehicle_info.hpp" + + +namespace obstacle_stop_planner +{ +ObstacleStopPlanner::ObstacleStopPlanner( + rclcpp::Node * node, + const VehicleInfo & vehicle_info, + const StopControlParameter & stop_param, + const SlowDownControlParameter & slow_down_param, + const AdaptiveCruiseControlParameter & acc_param) +: node_(node), + vehicle_info_(vehicle_info), + stop_param_(stop_param), + slow_down_param_(slow_down_param), + acc_param_(acc_param) +{ + debug_ptr_ = std::make_shared( + node_, + vehicle_info_.wheel_base + + vehicle_info_.front_overhang); + + // Initializer + acc_controller_ = std::make_unique( + node_, vehicle_info_, acc_param_); +} + +void ObstacleStopPlanner::obstaclePointcloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +{ + obstacle_pointcloud_ptr_ = updatePointCloud(input_msg); +} + +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::pathCallback( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg, + tf2_ros::Buffer & tf_buffer) +{ + /* + * extend trajectory to consider obstacles after the goal + */ + const auto extended_trajectory = extendTrajectory(*input_msg, stop_param_.extend_distance); + + const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; + autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; + + /* + * trim trajectory from self pose + */ + auto self_pose = getSelfPose(input_msg->header, tf_buffer); + autoware_planning_msgs::msg::Trajectory trim_trajectory; + size_t trajectory_trim_index = 0; + std::tie(trim_trajectory, trajectory_trim_index) = + trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); + + /* + * decimate trajectory for calculation cost + */ + DecimateTrajectoryMap decimate_trajectory_map = decimateTrajectory( + trim_trajectory, stop_param_.step_length); + + autoware_planning_msgs::msg::Trajectory & trajectory = + decimate_trajectory_map.decimate_trajectory; + + /* + * search candidate obstacle pointcloud + */ + pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( + new pcl::PointCloud); + + // search obstacle candidate pointcloud to reduce calculation cost + const auto search_radius = slow_down_param_.enable_slow_down ? + slow_down_param_.slow_down_search_radius : stop_param_.stop_search_radius; + + obstacle_candidate_pointcloud_ptr = searchCandidateObstacle( + obstacle_pointcloud_ptr_, + tf_buffer, + trajectory, + search_radius, + vehicle_info_, + node_->get_logger()); + + /* + * check collision, slow_down + */ + // for collision + bool is_collision = false; + size_t decimate_trajectory_collision_index = 0; + Point3d nearest_collision_point; + rclcpp::Time nearest_collision_point_time; + // for slow down + bool candidate_slow_down = false; + bool is_slow_down = false; + size_t decimate_trajectory_slow_down_index = 0; + Point3d nearest_slow_down_point; + pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); + double lateral_deviation = 0.0; + + for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { + /* + * create one step circle center for vehicle + */ + const auto prev_center_pose = getVehicleCenterFromBase( + trajectory.points.at(i).pose, + vehicle_info_.vehicle_length, + vehicle_info_.rear_overhang); + const auto next_center_pose = getVehicleCenterFromBase( + trajectory.points.at(i + 1).pose, + vehicle_info_.vehicle_length, + vehicle_info_.rear_overhang); + + Point2d prev_center_point = autoware_utils::fromMsg(prev_center_pose.position).to_2d(); + Point2d next_center_point = autoware_utils::fromMsg(next_center_pose.position).to_2d(); + + /* + * create one step polygon for vehicle + */ + const auto move_vehicle_polygon = createOneStepPolygon( + trajectory.points.at(i).pose, + trajectory.points.at(i + 1).pose, + stop_param_.expand_stop_range, + vehicle_info_); + debug_ptr_->pushPolygon( + move_vehicle_polygon, + trajectory.points.at(i).pose.position.z, + PolygonType::Vehicle); + + Polygon2d move_slow_down_range_polygon; + if (slow_down_param_.enable_slow_down) { + /* + * create one step polygon for slow_down range + */ + move_slow_down_range_polygon = createOneStepPolygon( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + slow_down_param_.expand_slow_down_range, vehicle_info_); + debug_ptr_->pushPolygon( + move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, + PolygonType::SlowDownRange); + } + + // check within polygon + pcl::PointCloud::Ptr collision_pointcloud_ptr( + new pcl::PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + std::tie(candidate_slow_down, slow_down_pointcloud_ptr) = getSlowDownPointcloud( + is_slow_down, slow_down_param_.enable_slow_down, + obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, + slow_down_param_.slow_down_search_radius, + move_slow_down_range_polygon, slow_down_pointcloud_ptr, candidate_slow_down); + + std::tie(is_collision, collision_pointcloud_ptr) = getCollisionPointcloud( + slow_down_pointcloud_ptr, prev_center_point, next_center_point, + stop_param_.stop_search_radius, move_vehicle_polygon, + trajectory.points.at(i), collision_pointcloud_ptr, is_collision); + + if (candidate_slow_down && !is_collision && !is_slow_down) { + is_slow_down = true; + decimate_trajectory_slow_down_index = i; + debug_ptr_->pushPolygon( + move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, + PolygonType::SlowDown); + + const auto nearest_slow_down_pointstamped = PointHelper::getNearestPoint( + *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); + nearest_slow_down_point = nearest_slow_down_pointstamped.point; + nearest_collision_point_time = nearest_slow_down_pointstamped.time; + + const auto lateral_nearest_slow_down_point = PointHelper::getLateralNearestPoint( + *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); + lateral_deviation = lateral_nearest_slow_down_point.deviation; + debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); + } + + /* + * search nearest collision point by beginning of path + */ + if (is_collision) { + const auto nearest_collision_pointstamped = PointHelper::getNearestPoint( + *collision_pointcloud_ptr, trajectory.points.at(i).pose); + nearest_collision_point = nearest_collision_pointstamped.point; + nearest_collision_point_time = nearest_collision_pointstamped.time; + debug_ptr_->pushObstaclePoint(nearest_collision_point, PointType::Stop); + decimate_trajectory_collision_index = i; + break; + } + } + + /* + * insert max velocity and judge if there is a need to stop + */ + bool need_to_stop = is_collision; + if (is_collision) { + std::tie(need_to_stop, output_msg) = acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory_map.decimate_trajectory, + decimate_trajectory_collision_index, + self_pose, nearest_collision_point.to_2d(), + nearest_collision_point_time, object_ptr_, + current_velocity_ptr_, + output_msg); + } + + /* + * insert stop point + */ + if (need_to_stop) { + output_msg = insertStopPoint( + decimate_trajectory_map.index_map.at(decimate_trajectory_collision_index) + + trajectory_trim_index, + base_path, + nearest_collision_point.to_2d(), + output_msg); + } + + /* + * insert slow_down point + */ + if (is_slow_down) { + output_msg = insertSlowDownPoint( + decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), + base_path, + nearest_slow_down_point.to_2d(), + calcSlowDownTargetVel(lateral_deviation), + slow_down_param_.slow_down_margin, + output_msg); + } + debug_ptr_->publish(); + return output_msg; +} + +// collision +std::tuple::Ptr> +ObstacleStopPlanner::getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, + const Polygon2d & one_step_polygon, + const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, + const pcl::PointCloud::Ptr collision_pointcloud, + const bool is_collision) +{ + auto output_pointcloud = collision_pointcloud; + auto output_collision = is_collision; + + for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { + Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); + if ( + boost::geometry::distance(prev_center_point, point) < search_radius || + boost::geometry::distance(next_center_point, point) < search_radius) + { + if (boost::geometry::within(point, one_step_polygon)) { + output_pointcloud->push_back(slow_down_pointcloud->at(j)); + output_collision = true; + debug_ptr_->pushPolygon( + one_step_polygon, trajectory_point.pose.position.z, + PolygonType::Collision); + } + } + } + return std::make_tuple(output_collision, output_pointcloud); +} + +// slow down +std::tuple::Ptr> +ObstacleStopPlanner::getSlowDownPointcloud( + const bool is_slow_down, + const bool enable_slow_down, + const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, + const Polygon2d & one_step_polygon, + const pcl::PointCloud::Ptr slow_down_pointcloud, + const bool candidate_slow_down) +{ + auto output_pointcloud = slow_down_pointcloud; + auto output_candidate = candidate_slow_down; + + if (!is_slow_down && enable_slow_down) { + for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { + Point2d point( + obstacle_candidate_pointcloud->at(j).x, obstacle_candidate_pointcloud->at(j).y); + if ( + boost::geometry::distance(prev_center_point, point) < search_radius || + boost::geometry::distance(next_center_point, point) < search_radius) + { + if (boost::geometry::within(point, one_step_polygon)) { + output_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); + output_candidate = true; + } + } + } + } else { + output_pointcloud = obstacle_candidate_pointcloud; + } + return std::make_tuple(output_candidate, output_pointcloud); +} + +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::insertSlowDownPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_slow_down_point, + const double slow_down_target_vel, const double slow_down_margin, + const autoware_planning_msgs::msg::Trajectory & input_msg) +{ + auto output_msg = input_msg; + PointHelper point_helper; + + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { + const double yaw = + getYawFromQuaternion(base_path.points.at(i).pose.orientation); + const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; + const Point2d slow_down_point_vec { + nearest_slow_down_point.x() - base_path.points.at(i).pose.position.x, + nearest_slow_down_point.y() - base_path.points.at(i).pose.position.y}; + + if ( + trajectory_vec.dot(slow_down_point_vec) < 0.0 || + (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(slow_down_point_vec))) + { + const auto slow_down_start_point = point_helper.createSlowDownStartPoint( + i, slow_down_margin, slow_down_target_vel, trajectory_vec, slow_down_point_vec, + base_path, current_velocity_ptr_->twist.linear.x, + slow_down_param_); + + if (slow_down_start_point.index <= output_msg.points.size()) { + autoware_planning_msgs::msg::TrajectoryPoint slowdown_trajectory_point; + std::tie(slowdown_trajectory_point, output_msg) = + PointHelper::insertSlowDownStartPoint( + slow_down_start_point, base_path, output_msg); + debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); + output_msg = insertSlowDownVelocity( + slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, + output_msg); + } + break; + } + } + return output_msg; +} + +// stop +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::insertStopPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const Point2d & nearest_collision_point, + const autoware_planning_msgs::msg::Trajectory & input_msg) +{ + auto output_msg = input_msg; + PointHelper point_helper; + + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { + const double yaw = + getYawFromQuaternion(base_path.points.at(i).pose.orientation); + const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; + const Point2d collision_point_vec { + nearest_collision_point.x() - base_path.points.at(i).pose.position.x, + nearest_collision_point.y() - base_path.points.at(i).pose.position.y}; + + if ( + trajectory_vec.dot(collision_point_vec) < 0.0 || + (i + 1 == base_path.points.size() && 0.0 < trajectory_vec.dot(collision_point_vec))) + { + const auto stop_point = + point_helper.searchInsertPoint( + i, base_path, trajectory_vec, collision_point_vec, stop_param_); + if (stop_point.index <= output_msg.points.size()) { + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; + std::tie(trajectory_point, output_msg) = + PointHelper::insertStopPoint(stop_point, base_path, output_msg); + debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); + } + break; + } + } + return output_msg; +} + +void ObstacleStopPlanner::externalExpandStopRangeCallback( + const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg) +{ + stop_param_.expand_stop_range = input_msg->expand_stop_range; + stop_param_.stop_search_radius = + stop_param_.step_length + std::hypot( + vehicle_info_.vehicle_width / 2.0 + stop_param_.expand_stop_range, + vehicle_info_.vehicle_length / 2.0); +} + +autoware_planning_msgs::msg::Trajectory ObstacleStopPlanner::insertSlowDownVelocity( + const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, + const autoware_planning_msgs::msg::Trajectory & input_path) +{ + autoware_planning_msgs::msg::TrajectoryPoint slow_down_end_trajectory_point; + auto output_path = input_path; + bool is_slow_down_end = false; + + for (size_t j = slow_down_start_point_idx; j < output_path.points.size() - 1; ++j) { + output_path.points.at(j).twist.linear.x = + std::min(slow_down_vel, output_path.points.at(j).twist.linear.x); + const auto dist = std::hypot( + output_path.points.at(j).pose.position.x - output_path.points.at(j + 1).pose.position.x, + output_path.points.at(j).pose.position.y - output_path.points.at(j + 1).pose.position.y); + slow_down_vel = std::max( + slow_down_target_vel, + std::sqrt( + std::max( + slow_down_vel * slow_down_vel - 2 * slow_down_param_.max_deceleration * dist, + 0.0))); + if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { + slow_down_end_trajectory_point = output_path.points.at(j + 1); + is_slow_down_end = true; + } + } + if (!is_slow_down_end) { + slow_down_end_trajectory_point = output_path.points.back(); + } + debug_ptr_->pushPose(slow_down_end_trajectory_point.pose, PoseType::SlowDownEnd); + return output_path; +} + +double ObstacleStopPlanner::calcSlowDownTargetVel(const double lateral_deviation) const +{ + return slow_down_param_.min_slow_down_vel + + (slow_down_param_.max_slow_down_vel - slow_down_param_.min_slow_down_vel) * + std::max(lateral_deviation - vehicle_info_.vehicle_width / 2, 0.0) / + slow_down_param_.expand_slow_down_range; +} + +void ObstacleStopPlanner::dynamicObjectCallback( + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg) +{ + object_ptr_ = input_msg; +} + +void ObstacleStopPlanner::currentVelocityCallback( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg) +{ + current_velocity_ptr_ = input_msg; +} + +geometry_msgs::msg::Pose ObstacleStopPlanner::getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer) +{ + geometry_msgs::msg::Pose self_pose; + try { + geometry_msgs::msg::TransformStamped transform; + transform = + tf_buffer.lookupTransform( + header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds( + 0.1)); + self_pose = autoware_utils::transform2pose(transform.transform); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), + "could not get self pose from tf_buffer."); + } + return self_pose; +} +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 4e93eae9..8d561196 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -30,9 +30,9 @@ Point2d PointHelper::getBackwardPointFromBasePoint( const Point2d & line_point1, const Point2d & line_point2, const Point2d & base_point, const double backward_length) { - const auto line_vec = Eigen::Vector2d(line_point2) - line_point1; + const auto line_vec = line_point2 - line_point1; const auto backward_vec = backward_length * line_vec.normalized(); - const auto output_point = Eigen::Vector2d(base_point) + backward_vec; + const auto output_point = base_point + backward_vec; return Point2d{output_point.x(), output_point.y()}; } @@ -111,14 +111,15 @@ PointHelper::insertStopPoint( StopPoint PointHelper::searchInsertPoint( const size_t idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Point2d & trajectory_vec, const Point2d & collision_point_vec) const + const Point2d & trajectory_vec, const Point2d & collision_point_vec, + const StopControlParameter & param) const { const auto max_dist_stop_point = createTargetPoint( - idx, param_.stop_margin, trajectory_vec, collision_point_vec, + idx, param.stop_margin, trajectory_vec, collision_point_vec, base_path); const auto min_dist_stop_point = createTargetPoint( - idx, param_.min_behavior_stop_margin, trajectory_vec, collision_point_vec, base_path); + idx, param.min_behavior_stop_margin, trajectory_vec, collision_point_vec, base_path); // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; @@ -168,7 +169,8 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( const int idx, const double margin, const double slow_down_target_vel, const Point2d & trajectory_vec, const Point2d & slow_down_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path, - const double current_velocity_x) const + const double current_velocity_x, + const SlowDownControlParameter & param) const { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(slow_down_point_vec); @@ -197,7 +199,7 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( slow_down_point.velocity = std::max( std::sqrt( - slow_down_target_vel * slow_down_target_vel + 2 * param_.max_deceleration * + slow_down_target_vel * slow_down_target_vel + 2 * param.max_deceleration * length_sum), current_velocity_x); return slow_down_point; diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 5cdcc213..586da23f 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -45,8 +45,7 @@ autoware_planning_msgs::msg::Trajectory extendTrajectory( } DecimateTrajectoryMap decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & /*param*/) + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length) { DecimateTrajectoryMap output; output.orig_trajectory = input_trajectory; diff --git a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py new file mode 100644 index 00000000..bc1e1fe4 --- /dev/null +++ b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py @@ -0,0 +1,61 @@ +# Copyright 2021 the Autoware Foundation +# +# 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. +# +# Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +import os +import unittest + +import launch_testing +import pytest +from ament_index_python import get_package_share_directory +from launch.launch_description import LaunchDescription +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + + +@pytest.mark.launch_test +def generate_test_description(): + obstacle_stop_planner_node = Node( + package='obstacle_stop_planner_refine', + executable='obstacle_stop_planner_node_exe', + namespace='test', + parameters=[ + os.path.join( + get_package_share_directory('obstacle_stop_planner_refine'), + 'config/obstacle_stop_planner.param.yaml'), + os.path.join( + get_package_share_directory('obstacle_stop_planner_refine'), + 'config/adaptive_cruise_control.param.yaml'), + os.path.join( + get_package_share_directory('obstacle_stop_planner_refine'), + 'config/vehicle_info.param.yaml') + ]) + + context = {'obstacle_stop_planner_node': obstacle_stop_planner_node} + + return LaunchDescription([ + obstacle_stop_planner_node, + # Start tests right away - no need to wait for anything + ReadyToTest(), + ]), context + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + + def test_exit_code(self, proc_output, proc_info, obstacle_stop_planner_node): + # Check that process exits with code -15 code: termination request, sent to the program + assertExitCodes(proc_info, [-15], process=obstacle_stop_planner_node) diff --git a/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp b/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp index ae371120..29365791 100644 --- a/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp +++ b/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp @@ -15,8 +15,7 @@ #include "gtest/gtest.h" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" -namespace obstacle_stop_planner -{ + TEST(createVehicleFootprint, returnValue) { VehicleInfo vehicle_info; vehicle_info.front_overhang = 0.5; @@ -26,9 +25,8 @@ TEST(createVehicleFootprint, returnValue) { vehicle_info.left_overhang = 0.2; vehicle_info.right_overhang = 0.2; - const auto ring2d = createVehicleFootprint(vehicle_info); - EXPECT_EQ(ring2d.size(), 5); + const auto ring2d = obstacle_stop_planner::createVehicleFootprint(vehicle_info); + EXPECT_EQ(ring2d.size(), 5U); EXPECT_EQ(ring2d.at(0).x(), 2.5); EXPECT_EQ(ring2d.at(0).y(), 0.25); } -} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp new file mode 100644 index 00000000..4e2f3ee9 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp @@ -0,0 +1,115 @@ +// Copyright 2021 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 "gtest/gtest.h" + +#include "obstacle_stop_planner/obstacle_stop_planner.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +using obstacle_stop_planner::StopControlParameter; +using obstacle_stop_planner::SlowDownControlParameter; +using obstacle_stop_planner::AdaptiveCruiseControlParameter; +using obstacle_stop_planner::ObstacleStopPlanner; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +VehicleInfo createVehicleParam() +{ + VehicleInfo i; + i.wheel_radius = 1.0F; + i.wheel_width = 1.0F; + i.wheel_base = 1.0F; + i.wheel_tread = 1.0F; + i.front_overhang = 1.0F; + i.rear_overhang = 1.0F; + i.left_overhang = 1.0F; + i.rear_overhang = 1.0F; + i.vehicle_height = 1.0F; + + i.vehicle_length = 1.0F; + i.vehicle_width = 1.0F; + i.min_longitudinal_offset = 1.0F; + i.max_longitudinal_offset = 2.0F; + i.min_lateral_offset = 1.0F; + i.max_lateral_offset = 2.0F; + i.min_height_offset = 1.0F; + i.max_height_offset = 2.0F; + return i; +} + +StopControlParameter createStopParam() +{ + StopControlParameter i; + i.stop_margin = 1.0F; + i.min_behavior_stop_margin = 1.0F; + i.step_length = 1.0F; + i.extend_distance = 1.0F; + i.expand_stop_range = 1.0F; + i.stop_search_radius = 1.0F; + return i; +} + +SlowDownControlParameter createSlowDownParam() +{ + SlowDownControlParameter i; + i.slow_down_margin = 1.0F; + i.expand_slow_down_range = 1.0F; + i.max_slow_down_vel = 2.0F; + i.min_slow_down_vel = 1.0F; + i.max_deceleration = 1.0F; + i.enable_slow_down = true; + i.slow_down_search_radius = 1.0F; + return i; +} + +AdaptiveCruiseControlParameter createAccParam() +{ + AdaptiveCruiseControlParameter i; + i.use_object_to_est_vel = true; + // add parameter + return i; +} + +// class ObstacleStopPlannerTest : public ::testing::Test +// { +// public: +// ObstacleStopPlannerTest() +// { +// const VehicleInfo vehicle_info = createVehicleParam(); + +// const StopControlParameter stop_param = createStopParam(); + +// const SlowDownControlParameter slow_down_param = createSlowDownParam(); + +// const AdaptiveCruiseControlParameter acc_param = createAccParam(); + +// rclcpp::Node dummy_node{"dummy_node"}; + +// planner_ = std::make_shared( +// &dummy_node, +// vehicle_info, +// stop_param, +// slow_down_param, +// acc_param); +// } +// std::shared_ptr planner_; +// }; + +// TEST_F(ObstacleStopPlannerTest, example) +// { +// EXPECT_EQ(1, 1); +// } diff --git a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp new file mode 100644 index 00000000..f81ed983 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp @@ -0,0 +1,183 @@ +// Copyright 2020 The Autoware Foundation +// +// 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 "gtest/gtest.h" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +using sensor_msgs::msg::PointCloud2; +using autoware_planning_msgs::msg::Trajectory; +using namespace std::chrono_literals; + +void init_pcl_msg( + sensor_msgs::msg::PointCloud2 & msg, + const std::string & frame_id, + const std::size_t size) +{ + msg.height = 1U; + msg.is_bigendian = false; + msg.is_dense = false; + msg.header.frame_id = frame_id; + // set the fields + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields( + 4U, + "x", 1U, sensor_msgs::msg::PointField::FLOAT32, + "y", 1U, sensor_msgs::msg::PointField::FLOAT32, + "z", 1U, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1U, sensor_msgs::msg::PointField::FLOAT32); + // allocate memory so that iterators can be used + modifier.resize(size); +} + +Trajectory convertPointsToTrajectoryWithYaw( + const std::vector & points) +{ + Trajectory trajectory; + for (size_t i = 0; i < points.size(); i++) { + autoware_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose.position = autoware_utils::toMsg(points[i]); + double yaw = 0; + if (i > 0) { + const double dx = points[i].x() - points[i - 1].x(); + const double dy = points[i].y() - points[i - 1].y(); + yaw = std::atan2(dy, dx); + } else if (i == 0 && points.size() > 1) { + const double dx = points[i + 1].x() - points[i].x(); + const double dy = points[i + 1].y() - points[i].y(); + yaw = std::atan2(dy, dx); + } + const double roll = 0; + const double pitch = 0; + tf2::Quaternion quaternion; + quaternion.setRPY(roll, pitch, yaw); + traj_point.pose.orientation = tf2::toMsg(quaternion); + trajectory.points.push_back(traj_point); + } + return trajectory; +} +class ObstacleStopPlannerNodeTest : public ::testing::Test +{ +public: + void SetUp() override + { + ASSERT_FALSE(rclcpp::ok()); + rclcpp::init(0, nullptr); + ASSERT_TRUE(rclcpp::ok()); + + fake_node_ = std::make_shared("fake_node"); + + using PubAllocT = rclcpp::PublisherOptionsWithAllocator>; + dummy_pointcloud_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/pointcloud", rclcpp::QoS{1}, PubAllocT{}); + dummy_path_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/trajectory", rclcpp::QoS{1}, PubAllocT{}); + dummy_velocity_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/twist", rclcpp::QoS{1}, PubAllocT{}); + dummy_object_pub_ = + fake_node_->create_publisher( + "/obstacle_stop_planner/input/objects", rclcpp::QoS{1}, PubAllocT{}); + path_sub_ = + fake_node_->create_subscription( + "/obstacle_stop_planner/output/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::SharedPtr msg) + {this->path_msg_ = *msg;}); + + rclcpp::NodeOptions node_options{}; + + node_options.append_parameter_override("wheel_radius", 0.39F); + node_options.append_parameter_override("wheel_width", 0.42F); + node_options.append_parameter_override("wheel_base", 2.74F); + node_options.append_parameter_override("wheel_tread", 1.63F); + node_options.append_parameter_override("front_overhang", 1.0F); + node_options.append_parameter_override("rear_overhang", 1.03F); + node_options.append_parameter_override("left_overhang", 0.1F); + node_options.append_parameter_override("right_overhang", 0.1F); + node_options.append_parameter_override("vehicle_height", 2.5F); + + node_options.append_parameter_override( + "adaptive_cruise_control.use_object_to_estimate_vel", false); + + planner_ = std::make_shared(node_options); + } + + std::shared_ptr planner_; + rclcpp::Node::SharedPtr fake_node_{nullptr}; + rclcpp::Publisher::SharedPtr dummy_pointcloud_pub_; + rclcpp::Publisher::SharedPtr dummy_path_pub_; + rclcpp::Publisher::SharedPtr dummy_velocity_pub_; + rclcpp::Publisher::SharedPtr dummy_object_pub_; + rclcpp::Subscription::SharedPtr path_sub_; + boost::optional path_msg_; +}; + +TEST_F(ObstacleStopPlannerNodeTest, plan_simple_trajectory) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(fake_node_); + executor.add_node(planner_); + + // publish point cloud + const uint32_t max_cloud_size = 10; + + sensor_msgs::msg::PointCloud2 test_msg; + init_pcl_msg(test_msg, "base_link", max_cloud_size); + test_msg.header.stamp = fake_node_->now(); + dummy_pointcloud_pub_->publish(test_msg); + + // current velocity + geometry_msgs::msg::TwistStamped twist_msg; + twist_msg.twist.angular.x = 0.0F; + dummy_velocity_pub_->publish(twist_msg); + + executor.spin_some(); + + // create trajectory + std::vector points; + points.emplace_back(0.0, 0.0, 0.0); + points.emplace_back(1.0, 0.0, 0.0); + points.emplace_back(2.0, 0.0, 0.0); + points.emplace_back(3.0, 0.0, 0.0); + points.emplace_back(4.0, 0.0, 0.0); + points.emplace_back(5.0, 0.0, 0.0); + auto trajectory = convertPointsToTrajectoryWithYaw(points); + trajectory.header.frame_id = "base_link"; + trajectory.header.stamp = fake_node_->now(); + dummy_path_pub_->publish(trajectory); + + using namespace std::chrono_literals; + rclcpp::WallRate rate(100ms); + while (rclcpp::ok()) { + executor.spin_some(); + if (path_msg_.has_value()) { + break; + } + rate.sleep(); + } + + // Check data + for (size_t i = 1; i < path_msg_.get().points.size(); i++) { + ASSERT_EQ(trajectory.points[i - 1], path_msg_.get().points[i]); + } +} diff --git a/obstacle_stop_planner_refine/test/src/test_point_helper.cpp b/obstacle_stop_planner_refine/test/src/test_point_helper.cpp new file mode 100644 index 00000000..77baee0f --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_point_helper.cpp @@ -0,0 +1,46 @@ +// Copyright 2021 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 "gtest/gtest.h" +#include "obstacle_stop_planner/point_helper.hpp" + + +TEST(getBackwardPointFromBasePoint, returnValue) { + Point2d line1{1.0, 1.0}; + Point2d line2{1.0, -1.0}; + Point2d base_point{0.0, 0.0}; + double backward_length = 1.0; + auto ret = obstacle_stop_planner::PointHelper::getBackwardPointFromBasePoint( + line1, line2, base_point, backward_length); + + Point2d expect = Point2d{0.0, -1.0}; + EXPECT_EQ(expect, ret); +} + +TEST(getNearestPoint, returnValue) { + geometry_msgs::msg::Pose base_pose; + pcl::PointCloud pc; + pcl::PointXYZ point; + point.x = 10.0; + point.y = 10.0; + point.z = 10.0; + pc.points.push_back(point); + point.x = 20.0; + point.y = 20.0; + point.z = 20.0; + pc.points.push_back(point); + auto ret = obstacle_stop_planner::PointHelper::getNearestPoint(pc, base_pose); + EXPECT_EQ(10.0, ret.point.x()); + EXPECT_EQ(10.0, ret.point.y()); +} diff --git a/obstacle_stop_planner_refine/test/src/test_util.cpp b/obstacle_stop_planner_refine/test/src/test_util.cpp index 5cb11c15..ab504268 100644 --- a/obstacle_stop_planner_refine/test/src/test_util.cpp +++ b/obstacle_stop_planner_refine/test/src/test_util.cpp @@ -15,12 +15,8 @@ #include "gtest/gtest.h" #include "obstacle_stop_planner/util.hpp" -namespace obstacle_stop_planner -{ + TEST(getYawFromQuaternion, returnValue) { geometry_msgs::msg::Quaternion quat; EXPECT_EQ(0.0, getYawFromQuaternion(quat)); } - - -} // namespace obstacle_stop_planner From c55df3c8b733277a9b6ac1ada8dc6198d307cb93 Mon Sep 17 00:00:00 2001 From: Kenji Miyake Date: Tue, 30 Mar 2021 00:31:19 +0900 Subject: [PATCH 57/62] Fix lint Signed-off-by: Kenji Miyake --- .../test/obstacle_stop_planner_node_launch_test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py index bc1e1fe4..2798e953 100644 --- a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py +++ b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py @@ -17,13 +17,13 @@ import os import unittest -import launch_testing -import pytest from ament_index_python import get_package_share_directory from launch.launch_description import LaunchDescription from launch_ros.actions import Node +import launch_testing from launch_testing.actions import ReadyToTest from launch_testing.asserts import assertExitCodes +import pytest @pytest.mark.launch_test From dd4fa9fd11017feb2673723e05034bacfb4f7804 Mon Sep 17 00:00:00 2001 From: Kenji Miyake Date: Tue, 30 Mar 2021 00:31:31 +0900 Subject: [PATCH 58/62] Add .isort.cfg Signed-off-by: Kenji Miyake --- .isort.cfg | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 .isort.cfg diff --git a/.isort.cfg b/.isort.cfg new file mode 100644 index 00000000..a9facc1f --- /dev/null +++ b/.isort.cfg @@ -0,0 +1,3 @@ +[settings] +force_sort_within_sections=true +known_third_party=launch From 7c130396935a393e07f5053788edfa82ca7b7a5b Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 30 Mar 2021 11:19:36 +0900 Subject: [PATCH 59/62] Fix usage of ament_cmake_auto_gtest --- obstacle_stop_planner_refine/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 1654cb48..d1c2e3bf 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -43,10 +43,8 @@ if(BUILD_TESTING) ) find_package(ament_cmake_auto_gtest REQUIRED) - file(GLOB_RECURSE test_files test/**/*.cpp) ament_auto_add_gtest(test_obstacle_stop_planner - test/test_obstacle_stop_planner.test - ${test_files} + test/ ) endif() From 20f124ba646e2ecee2c8f0c3d1f467c1800b9bd1 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 30 Mar 2021 13:14:33 +0900 Subject: [PATCH 60/62] Cleanup, modify copyright --- .../test/obstacle_stop_planner_node_launch_test.py | 2 +- .../test/src/test_obstacle_stop_planner.cpp | 5 ----- .../test/src/test_obstacle_stop_planner_node.cpp | 2 +- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py index 2798e953..780ef193 100644 --- a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py +++ b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py @@ -1,4 +1,4 @@ -# Copyright 2021 the Autoware Foundation +# Copyright 2021 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. diff --git a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp index 4e2f3ee9..4a6d1888 100644 --- a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp +++ b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner.cpp @@ -108,8 +108,3 @@ AdaptiveCruiseControlParameter createAccParam() // } // std::shared_ptr planner_; // }; - -// TEST_F(ObstacleStopPlannerTest, example) -// { -// EXPECT_EQ(1, 1); -// } diff --git a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp index f81ed983..51afbd83 100644 --- a/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp +++ b/obstacle_stop_planner_refine/test/src/test_obstacle_stop_planner_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 The Autoware Foundation +// Copyright 2021 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. From 9d1825abc35a052c90a82f1d3a0a5c4e586a06cc Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 30 Mar 2021 16:53:19 +0900 Subject: [PATCH 61/62] Fix usage of ament_auto_add_gtest --- .../cmake/ament_auto_add_gtest.cmake | 14 ++++---------- obstacle_stop_planner_refine/CMakeLists.txt | 2 +- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake b/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake index 093ca5ce..96dcee28 100644 --- a/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake +++ b/ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake @@ -63,16 +63,10 @@ # macro(ament_auto_add_gtest target) cmake_parse_arguments(ARG - # ament_add_gtest flags - "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" - "RUNNER;TIMEOUT;WORKING_DIRECTORY" - "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" - # ament_add_executable flags - "WIN32;MACOSX_BUNDLE;EXCLUDE_FROM_ALL;NO_TARGET_LINK_LIBRARIES" - # ament_auto_add_gtest flags - "DIRECTORY" - "" - ${ARGN}) + "WIN32;MACOSX_BUNDLE;EXCLUDE_FROM_ALL;NO_TARGET_LINK_LIBRARIES;SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY;DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + ${ARGN}) if(NOT ARG_DIRECTORY AND NOT ARG_UNPARSED_ARGUMENTS) message(FATAL_ERROR "ament_auto_add_executable() called without any " "source files and without a DIRECTORY argument") diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index d1c2e3bf..f233a6c6 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -44,7 +44,7 @@ if(BUILD_TESTING) find_package(ament_cmake_auto_gtest REQUIRED) ament_auto_add_gtest(test_obstacle_stop_planner - test/ + DIRECTORY "test/" ) endif() From 454b54365920dd65207d8a480a788db5b2b17f2a Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 31 Mar 2021 11:05:41 +0900 Subject: [PATCH 62/62] Rename vehicle_info.param.yaml --- .../{vehicle_info.param.yaml => test_vehicle_info.param.yaml} | 0 .../test/obstacle_stop_planner_node_launch_test.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename obstacle_stop_planner_refine/config/{vehicle_info.param.yaml => test_vehicle_info.param.yaml} (100%) diff --git a/obstacle_stop_planner_refine/config/vehicle_info.param.yaml b/obstacle_stop_planner_refine/config/test_vehicle_info.param.yaml similarity index 100% rename from obstacle_stop_planner_refine/config/vehicle_info.param.yaml rename to obstacle_stop_planner_refine/config/test_vehicle_info.param.yaml diff --git a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py index 780ef193..78b1e129 100644 --- a/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py +++ b/obstacle_stop_planner_refine/test/obstacle_stop_planner_node_launch_test.py @@ -41,7 +41,7 @@ def generate_test_description(): 'config/adaptive_cruise_control.param.yaml'), os.path.join( get_package_share_directory('obstacle_stop_planner_refine'), - 'config/vehicle_info.param.yaml') + 'config/test_vehicle_info.param.yaml') ]) context = {'obstacle_stop_planner_node': obstacle_stop_planner_node}

H!G*3qju{ln|p?jjnuE?JYI_KOnis9}+l zFD{49?$K1Rq4Xpoyo`*6ECEK`+Yz>wW|6A^!F=V#vL1Z~-mUjlGU>}3()sR+wHwpB z`~iSO2fA(CTT1&pmabOO`s6RL&u6OT0r4i>sjFDpWqAM0*_|x7Qa51yeXsTknsO%I z-C=}D#lc|}WWxA<_DV#3EeyXTpRt_C9ev%QIUUt`UT$o}A?0^VeL<9c?_GVU&6^j# z{uoEE=ir>#h1wz3e5HTUZ8B{2^q#%g3R|5fSgoWNtrIMDu|LLjO{$`7v>fk6$li;d z6+>lVUL)`NMQcP%8$9SYM0*KoURl}s^Riq$vi|Y_qRJ0G=x%p z0Bpgg_mOR{_1o0vuoN%j`cTKJJ$imlfHvjSMroKI^KwJfxDluw-a2QCJ`&CK=tdJ5 zDt~BJlnW3pvdMtn7??g+(qm1nBLEA&#-9N_KSi(P@Rt#LqfqzqVdT|NA(Myac<;K5 zv58?j6$CPFc578&wSxW%5xM{dlKAg~Z`uI5G{j-4?>Y%y(pzIoG(x>@;1reNYo1r} zKi48hpgvi;c5U4rZnfv;lmyHm`=_I7ZC18=bsF^fuD;U0WW8HX(DrA>i-5pMszjq5 z8Y>ij@kg5Fet(e6a3lV+U=t%QUu*x7UgLvhDflKOY`X+yE_Uv;cEPFcoN0<8??t>v z?}3RQ3HE*06_)o*=Sq8V-K#eef;$Z;iJO zy{0+aoWh38>{bShgiBJR6Wtp`8YGB94i=`FG5tP?ikQ@e z7XgK)s_E~?H+%72I9b;s>1LOc6lH-DwX`enoSg0K%NBr*hnAy@qxufvke`>40>i)+ z{xVV`egRxWXC>qHzQ97vKo3-g!1U+;+BBRXLFJz+f>YK3;XKPPFhhuV=;3%!3FplR)--FxwPiX zz`kG7+{+q2qsj#m&z|J{Ety8+Oj##RQ_ZL&5ITSRM@oZd`f$es8jKyb1To@kN^{FX z9oFrI6?*696|NtU()N7`Y?^mz3M(1M+gDKc8%roplY2_{{+YEUL38}!1&RfQ;y~;L zARNkXTD!JPD&$|~QRXg0UjaWxRc$+3!SG6_1x%u1nnp4B^#uBk)8Gp4AB$RB)yt}l z+Prk@NV_Fpcz8jbOH8WhLpC8#=Y@syv7~UD#(b1@o`oLs0y7FwwjXk-cBpSjEZ$yp z>kc=vbOXbZyW~8!dKNosh)SR*z}Gm?cde}WLWDB+N##{uc>ZlOxfi67?3k)2Q(N}o z{7-NwcsPu!+?HeM_?x!kS-@0=o9x1<3y8C=)v}Bo9$5eIR;6)fho$7h2pmeyfQ`^<8tOPpkR#op^ji>3XOb6a3ZHs;l{XslhN(MO4n zYjmFB5|UwD3h2>=zApx{-FT&<(#HhR_QK8t>__Hx%R+(;-RL+qyU|#!%xk^j2Qeja zpa+hwpDDj_`WRf^;}=+C4D}lYZzka+X)!{*voC>i3IgKdlr_a7ZTbEy8eAgJPl=~j zh)uJEpDJi=5y2YrJ;i!vRNBy4PneYcgwb?@JwPcK8{!3P@Tm&Uh`7!^HsXl@M!t9q z9}~hLW<0>=taD=NM8~iwukLI$G%{iDCduLS8Ru&tnF*lhe-hPr5-Bild%kTJoaY00 z{L|V!cEz4{wytk0lhaF$!(D_#NnLPf*}8`jSK2F>;rYS=;;jnQE==q-{j zmXPh?)LAB)#`T{4c%v6-#kAflTDvIjHGI)}{otNI1ssR+cwn4-kA*4sTe8=Geb$AK z2Gdq3H)NA^d@uHc0c!Hj=?aNbGVYPcP!a3GlD^9C`$$Pi@#yZm9NZjcx}QXfz#}ZC z*MWB<^1FKV6g>E?a%^Tq*Gi7f!8yKecIu6>m{5CoE2n`2L#B{#tFn39$FsX07Vqoq z#MbPQy$eym^C5GkT&r;WcQu2I(lv1jSBA3@Dudkvh*1l2=gP7(OPm460kVvMQ!7nF8 zYGUBOb9Z3Z(So}8vOQ004FKxNTWspuLY2A_F$x*qKpq_1N<3#ryh&hqMxL8-;FO_j zl(cs42~=Sq>gjz*szKq_dI{c~pCmUWf}$ViOaw?ny0exK>rQM#jUKX2GUqWAQlJ$k zQJnQ93QF723KFP5Zn;S*IrRSX5YYYHn1hxi-i_Jm>rLD3VJer+!Ae&s3$DycfntRb zyT`Iaf?A74TVQPqKTxzF3*7d`@yk?BqAmi%%~k*I-CJg@_1UKzFfb$;62bz?>=zeN zo15qPIRixiOuhuQYDm~(G&?jWW)mM%3|Ywo5qU_?uNXGL`ui4&rdLs7W7(S(D|~hr z!E3;LBn4aedLX!4EOW=nYm;wr^Wh-!lF#-rgU9_-rKiE@(qb$4GqXGr7%$ zOP?kDsDqFwul_v%2cuph*UDihw|Jn&JPfxE=@3TOOVeU+qeCrBRc88)@m%@0*NWaV z%U25Fqcp5|Mu?&SPw~o#0C3mc5 zuK@0?UT8lvPo-@EnU}B>-uiApS1d*XsrL2?7VwkGv-OBpx-{ZP2^NCY1m?{Xj>MsKZ>5rTAb<}sK3ejDw$ZAE)njs2 zsU+wICPUf}`#n#5?=Gj=9ynub5kyV?lp5`LTM-!2GiooZ-x-EK0G{8Xx$3tV%ahvg z_wN$vc{w$$UOsp=dxGmCk#0BXlbW(%kQ;9}L&VeWOlP8}e#wgUNstDUsMVzz2>-D` zHb2QWH{jAQ8xY;}Q=J%3ilOTap@eRsEa}g)gWj1$0#DwDqrjSE$_=Nv!I-=>#ftQ; zDK<^qYkpATq#Tj;t~yx#J-*kciN@3Z$N^g&Rj6{nGoN0y0+y7;FtDzYNR>hub1~ZVUOl8b($7S+m^4)^G)j zEMr06kBru)pnq4^;s5D)Q1o%1V5+mf5XgA|^fkcl+#mDB+yyd(xz{1llB|dPVlaD0 zZ&@Aibct(eHyB7UX}P1`%EX&c(g8y$SGroFYg zn)%fWWMpLS%R7gKeV6??r@;jaMazrcoX8~U{9gtG;JPqraE(UIig_)g?jo5LW@0A8 zYHQnYHqY@tzW`1#%Da7@z*2-8RRk(2xHIIPh^+#R0$`5;X`Mw&ul5FJrS$TnzW5uU z%yuxYL~FrPjOMFIgKf(%Uw-MgzYx8-J5JU%IJ`=P_Ej>`9F2?OeZm#1wLEgC_XT(z{MYR{<}< z)1(iKSD3C*Um`l@3}s>#iEwM1IvIC+oeqhdrHS)buAs8d7HzMBbm>NGzr*xygs|kL zxfurLBt3qF`g8dj;8cxthuQ;r4iaW87cP^ivKm~=gMn*wt@aq^7@);(^L0%S4^VyC z;H_OgrHtQTc=@tuhL#heF8Bt$qS1 zAu%y~!iEaI><$CN6lKmyS%VMK&DP<^m-G9Aha^(z_L8j$yCGP+gl;x?AYE?Z5P(9R zg~E^*QSl8_Kj$Aa(t+Y)z7n=dHhPqQKwe@MV0SO$4x5gDPpAwU$ni7A;1z-NmWY$6 z{U6t);DJ;BiBxwU=1E5fW4#cNm|qw%PtF^)$vs-Lhb|+^l9MtUpXsu(rH@_g8kjD~jSPWkS9g zgE*KV^?(vaY3c%yhw=Sv&jDRiSS;9@@-%UevmvXx%n6*qtc5#`CeOo}in2Lt!-}`k zAQu2=6ex07Jn&SaQLF@{&y{{dgn;z~k~x_ga{68ox;b_{YR5-ntDsb^zBbY*ANYcM z&ym>8NSMsr@HiWg#H<*|*>yQ#hB}IJQzpGxOXq^yj1GCahaJMpIQyM149yf`nP-1@ zDHGVPOnw^QC8vqn&gI(5&-}{IcQGaJi71)Z?_9+NdpA@ocwk@ zv#Y3+H~R*;P!oyWZVZuphY2w}gQl0V!)^dw2829{4_>DZxoL4(seb41uqyCmRWO5F zT73Up1kiHsiS&nr+elLgNH%zsFvC zBtWaz5gQ8F;|t;G2$$7c7VxiR3apAlcq_WEJTfg3soD?hQn}U0orOx)J2+Q^_%0Lu z*0ltSmoU?^@*%iq-_`aiMhTl5mV`CMC$C)0p$mku1p!hEEUrIU`ZuU@OwB~jxcUU= z@`lvvw^#87*W;|i8kY{Aw(d`z+fJh$IJN`cv(^kawmsfI)mh9577g=E+IRS^+ z2#Q;RbqwltGv%|qfw$`}j@ndJ)A9p3j!@5Xd_tKF@?6qm1$*N@>ZRm6Uv@F}IsiW$ zInD($2Qn|5p(F?9ts4lBxikfXI4R}Xji`dY$iPyWRYbh?kE+$eBHCECIO@AUe+)iY zwuX1t5kMQAf!Y))?(5u@D+2~5_!b=Wi z(G|FZYAmS)5;P?Ep#Wu3Qr)}E?}t1Jib9_s6$1QVQ9ntO_Hy#h-AzhP$Jok?bong3 zsnw63w`kW8ikJCB(>Q?HUuY?q3V&b)nn7kz_tRwTGZCn_p8d#K8m2j8yvTp8jrO1| z`as6B9YYWSDkiKIekX=XNKLF5g<}NxV8qLzu9+hfyOF0)=0pCZ5KmKp+tudEga!#rT?eH zh|im~>L|218y}J(I44_|hw+GA?f>Yidpai8peVg3^2UH?n5Eu-3XPPA*+dP`{vnqh z`rx<1f481WKa!0}D#zQvCdKCDiy<_QoI&G=XKhA15@PstG@~_wZK$(05^`;T^)?tq2ss3d$%P zZRhdd&Ha@FH<~$}T5ioc-}e7>bLHVsx7}ZF^$5ut*@w)KwJq@6 zLnLSO;~y&=0{5gAmJ4{==TZufYhP=_`0!=DPuaeU z42UrLmL@WTj-yjzf!CAC4?`hZI~&^*y>7Uc*0{!8B?W0lt^c5JC}Z|bPS?{utp&2P zI1eL8qM1age4?t?wFL`sB}#tOV(!B@VUO6K&?fK&11thTs22=iJw6=Vd8U7N?)91h z_oC;GM0vayKV@EajpC`>CqCuBncib;iCgI1x0~VF$EHDF@*Kx7cu@wFT~rn`f~(#YlB*p#WV}m_ zWmI0*B%Q34s)uEJD)1f8T}3AP89eayN|&0n6e}vN(&XmZN#PrEj~w0C75*7qK=gH- zPRv+oI`$3!$Iwo-9U;2(MuyCM2#Nl)oE&Cf*VhpeWgn!Bv}b5W;FPT&J_PK2SBh+U zvO3|BqPkY}=!&b?`6UZ@p_;9ZJMRM#K4Rp(de+`|2Y&h*Vz@Lnx%op=;e{{J6$0LH z6-<@XxhN+^l1u8 z)|P{{D@Z210e3Xu)=xLZ;&N<5PNFAWL||@iR6RAy>+8Y!sDG}k;JfFo{DPk)22xah zY5}=z6j=o0@)hBDPQpJ<^qYLd# zD>|4^q1!6B$IE-~Pv0L^(83kyTQifZG?r7)3$huXJTv2!R%R2r#@OGPsZJP$GASr1m^v6@MAX6GawZ+rVj4tu zehl@>jgF4C_icdsC=P81oB1f!@@n%2coRsK;Xzkyn}^LC`&{Mk%Wvv6T&O%S*FSjS zzGcTyP!^@TloH(P5k^p5Cp2~Wb*!X42K%*{0nHdY>FnQ_UlrM~U^pbhg-ew^UdGqq za#fiF51QwsL}WeT%R zI1dCuq$kKJtYr~2N_pz?D5w17di6KkOqOxP;Ir%WQJM@$Cl5Jpky0qRVL3GuR~*M6 zVlap#8mwA)D%6s$y&U5TUM`Mx;-Sd-iXnCL6>>yVJ>}`w@&nN`GQHztrYcU3j;iNy zk&9CW!%o_c+k`$H=QL2YdiPrRngUnIXdCD4*9)N4oq+_pbh$Xp2%>Ii;Nnzf-{jko z0cY!9Ki#1&@BQwu(vm{tk7w~bNau^nQ+J(17W*_@zGRx|K40yiH*Q0Qe#@-;HlcQ-1!b7ngO z(Rl))tLR6~n;{9*ljDjU@B9EedXupPcfIeSHU8@i&zwy#EmxmN=-~k(`xvRayBiq6 zC!)BNz$adprFJuvYw`I>$IHirR_AuyvC+X=8eoFQIu!K!B%QGSJrfI-BDtlz;PQX` z%%pTqEov)}OX|+Nt0TkmYKB93deWyaD!mosAXet@nyBuXayZ-bkFx%kq%{sV7 zzW7`q3>(~b{_4{Y^Zr)rZl4AT*bZ;4OY@*Z8BhGNZu3(7qxEE60euOClQ-$?Sa*S; zr>alo;E4YiNsce5B6Kk~1Nv@iU1Kmt&KOp4>+YihD&$29RUojUh`YoY> zJi-MZ@PRv^!O|(kZ>Q$(US~@)h6=yEFDgGT zq^H*`Hg$E{ijGa}5wZYL* zf#Y}|JRj4S;4*e97;kI+cXEvJ@~ke5)8sgFNN* z(4=ixo8P$l3oh?On1CIMGcKs3ofwxFGC~nIo4{`I!LyE-jG-T+&#KS#n>gIeH4u?9 zwfEZ=Tg`;6Oj|RDJ4xp)d@qrw5Lu;Ybgze8r`oFbsZqzv__n|i#e+W0pI<+oGddS_ zg1VLr5^`VOou4jDcjFYR5)q6TT@+IZ%8@R`JK+5}H7y_>Z#dW9KX7~7J!DMKLL*{O z>FAi4^nNX^sedCSe?pjvN|N+B^03EETH-lE*xb}q9btVnGz?D&y0j$B5xO!}C~k#@ zsKTaB23`KHwzy#NidaRL2YOMSo$4+KY{>e~b=>p1vAx>-NDu)!*XV=^9ta{drP)BY zyFTwklI_*mu5qn16)N-SqUYKEDpbAlP0G6Rl)kh@amP=5N^{PloW+%OS^|pubSjFm z+v;xY))a?5u8(a#R7+b|m$oPUbynjx8Bbk|=o!A{97zsbPbEY?`|l z44p|jptjDf>7@Z3(PA7SqGD<0`IkAY50XKXsCQrF^KiAnZA3V-O#iV88)d3uSleHw zIv13CUu_U18>->$ktN*+2T|EV>MxygApb7Fs;@2d0MG08)R@7l#BTrI&_(U9t}kVu zsy>kzofD0LAKLewxqC|v%N|!FVh3`W5b~GUHW}ky)UQk-s%8U;M^Y0R-jOc!&136W z+S`m+WifQdxN2nYR(JhzHcoHy$A>0N^7;`Julhi=w?3v5`%Tge`!jBAFsok^+*9O; zld4a=KMb3n>=Pyk=EJr)!{ZfMu?siEn?5(VyQvZy-q5*9f|1f6W}Ps>O;eLZ4Q7A} zBVOpsXIQ>!S0C76AT^LcUj#$(Y0__v=LP>E8&s^DR1`VhPtA-qq5;yP6BAkVu-b8u z+)FE()2l2Nn_(2P2fKBL27_#EW2f<_S6M7c5}^jT)O3~;}Q6w zqU|xl3u{Y;F(7D#S9rQ-KsI@?bwKxn=HC20AZB%`lwdZNnA zni75>bX03ICwW+5c7kRk0|!4c{~wijJM~ec?`^C`eu7_hMAJ1Yq`P7$-bKm6Up=Ge zUw$d?(9aQSe62{lBxu<7jsNu>V5$Ho%4viFbIfrg0H|=flYwOM5hDIqW1vDyDYGpFpDSE%ndDd|bl z%~n%IQCr1I!S^lR=nu(FAVKpP?az;`@22I!I{i~}YdRm^*( zYLhgjYL*5iLgo8tx>W0~fB!-Qa-KC}VhMY= zM5~=Qf?E Date: Tue, 2 Mar 2021 14:30:41 +0900 Subject: [PATCH 12/62] insert final newline --- obstacle_stop_planner_refine/design/class_diagram.pu | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obstacle_stop_planner_refine/design/class_diagram.pu b/obstacle_stop_planner_refine/design/class_diagram.pu index 03a0183b..45701623 100644 --- a/obstacle_stop_planner_refine/design/class_diagram.pu +++ b/obstacle_stop_planner_refine/design/class_diagram.pu @@ -40,4 +40,4 @@ namespace vehicle_info_util { class VehicleInfo } -@enduml \ No newline at end of file +@enduml From 56ef212be9cee83a641a7dc178dcdc5bdb037f2e Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 3 Mar 2021 12:04:01 +0900 Subject: [PATCH 13/62] add gtest --- obstacle_stop_planner_refine/CMakeLists.txt | 25 ++-- obstacle_stop_planner_refine/package.xml | 2 + .../cmake/ament_cmake_auto_gtest-extras.cmake | 115 ++++++++++++++++++ .../test/src/test_one_step_polygon.cpp | 24 ++++ .../test/test_obstacle_stop_planner.test | 5 + 5 files changed, 162 insertions(+), 9 deletions(-) create mode 100644 obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake create mode 100644 obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp create mode 100644 obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test diff --git a/obstacle_stop_planner_refine/CMakeLists.txt b/obstacle_stop_planner_refine/CMakeLists.txt index 517911de..77a6b2f8 100644 --- a/obstacle_stop_planner_refine/CMakeLists.txt +++ b/obstacle_stop_planner_refine/CMakeLists.txt @@ -43,15 +43,22 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - # find_package(ament_cmake_gtest REQUIRED) - - # file(GLOB_RECURSE test_files test/**/*.cpp) - - # ament_add_gtest(test_obstacle_stop_planner test/test_obstacle_stop_planner.test ${test_files}) - - # target_link_libraries(test_obstacle_stop_planner - # obstacle_stop_planner_node - # ) + find_package(ament_cmake_gtest REQUIRED) + include(test/cmake/ament_cmake_auto_gtest-extras.cmake) + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_auto_add_gtest(test_obstacle_stop_planner + test/test_obstacle_stop_planner.test + ${test_files} + ) + target_include_directories(test_obstacle_stop_planner + PRIVATE + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ) + target_link_libraries(test_obstacle_stop_planner + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} + ) endif() ament_auto_package( diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index 4833a530..dee3c7e0 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -27,8 +27,10 @@ visualization_msgs libopencv-dev + ament_cmake_gtest ament_lint_auto ament_lint_common + ament_cmake diff --git a/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake b/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake new file mode 100644 index 00000000..fa1070bc --- /dev/null +++ b/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake @@ -0,0 +1,115 @@ +# Copyright 2014 Open Source Robotics Foundation, 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. + +# +# Add a gtest, automatically linking and including dependencies. +# +# Call add_executable(target ARGN), link it against the gtest libraries +# and register the executable as a test. +# +# If gtest is not available the specified target is not being created and +# therefore the target existence should be checked before being used. +# +# +# All arguments of the CMake function ``add_executable()`` can be +# used beside the custom arguments ``DIRECTORY`` and +# ``NO_TARGET_LINK_LIBRARIES``. +# +# :param target: the name of the executable target +# :type target: string +# :param DIRECTORY: the directory to recursively glob for source +# files with the following extensions: c, cc, cpp, cxx +# :type DIRECTORY: string +# :param NO_TARGET_LINK_LIBRARIES: if set skip linking against +# ``${PROJECT_NAME}_LIBRARIES`` +# :type NO_TARGET_LINK_LIBRARIES: option +# :param ARGN: the list of source files +# :type ARGN: list of strings +# :param RUNNER: the path to the test runner script (default: see ament_add_test). +# :type RUNNER: string +# :param TIMEOUT: the test timeout in seconds, +# default defined by ``ament_add_test()`` +# :type TIMEOUT: integer +# :param WORKING_DIRECTORY: the working directory for invoking the +# executable in, default defined by ``ament_add_test()`` +# :type WORKING_DIRECTORY: string +# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest +# main libraries +# :type SKIP_LINKING_MAIN_LIBRARIES: option +# :param SKIP_TEST: if set mark the test as being skipped +# :type SKIP_TEST: option +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# +# @public +# +macro(ament_auto_add_gtest target) + cmake_parse_arguments(ARG + # ament_add_gtest flags + "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST" + "RUNNER;TIMEOUT;WORKING_DIRECTORY" + "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV" + # ament_add_executable flags + "WIN32;MACOSX_BUNDLE;EXCLUDE_FROM_ALL;NO_TARGET_LINK_LIBRARIES" + # ament_auto_add_gtest flags + "DIRECTORY" + "" + ${ARGN}) + if(NOT ARG_DIRECTORY AND NOT ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "ament_auto_add_executable() called without any " + "source files and without a DIRECTORY argument") + endif() + + set(_source_files "") + if(ARG_DIRECTORY) + # glob all source files + file( + GLOB_RECURSE + _source_files + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${ARG_DIRECTORY}/*.c" + "${ARG_DIRECTORY}/*.cc" + "${ARG_DIRECTORY}/*.cpp" + "${ARG_DIRECTORY}/*.cxx" + ) + if(NOT _source_files) + message(FATAL_ERROR "ament_auto_add_executable() no source files found " + "in directory '${CMAKE_CURRENT_SOURCE_DIR}/${ARG_DIRECTORY}'") + endif() + endif() + + # parse again to "remove" custom arguments + cmake_parse_arguments(ARG "NO_TARGET_LINK_LIBRARIES" "DIRECTORY" "" ${ARGN}) + ament_add_gtest("${target}" ${ARG_UNPARSED_ARGUMENTS} ${_source_files}) + + # add include directory of this package if it exists + if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include") + target_include_directories("${target}" PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include") + endif() + # link against other libraries of this package + if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "" AND + NOT ARG_NO_TARGET_LINK_LIBRARIES) + target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES}) + endif() + + # add exported information from found build dependencies + ament_target_dependencies(${target} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endmacro() diff --git a/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp b/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp new file mode 100644 index 00000000..ed86242c --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp @@ -0,0 +1,24 @@ +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +class TestSuite : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + } + void TearDown() + { + (void)rclcpp::shutdown(); + } +}; // sanity_check + +// write test here + + +int32_t main(int32_t argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test b/obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test new file mode 100644 index 00000000..c4ed7808 --- /dev/null +++ b/obstacle_stop_planner_refine/test/test_obstacle_stop_planner.test @@ -0,0 +1,5 @@ + + + + + From b4cd78ce541e7c602b8695a9320664433bea94ec Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 3 Mar 2021 08:52:11 +0900 Subject: [PATCH 14/62] wip --- .../obstacle_stop_planner/debug_marker.hpp | 13 +- .../one_step_polygon.hpp | 136 +++++++++++------- .../include/obstacle_stop_planner/util.hpp | 2 +- .../util/create_vehicle_footprint.hpp | 47 ++++++ obstacle_stop_planner_refine/package.xml | 1 + .../src/debug_marker.cpp | 104 ++++---------- 6 files changed, 168 insertions(+), 135 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp index c8fbc388..497721a6 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp @@ -28,6 +28,7 @@ #include "opencv2/core/core.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" +#include "autoware_utils/autoware_utils.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -45,8 +46,8 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const std::vector & polygon, const double z, const PolygonType & type); - bool pushPolygon(const std::vector & polygon, const PolygonType & type); + const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + bool pushPolygon(const autoware_utils::Polygon3d & polygon, 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); bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); @@ -66,10 +67,10 @@ class ObstacleStopPlannerDebugNode std::shared_ptr slow_down_end_pose_ptr_; std::shared_ptr stop_obstacle_point_ptr_; std::shared_ptr slow_down_obstacle_point_ptr_; - std::vector> vehicle_polygons_; - std::vector> slow_down_range_polygons_; - std::vector> collision_polygons_; - std::vector> slow_down_polygons_; + std::vector vehicle_polygons_; + std::vector slow_down_range_polygons_; + std::vector collision_polygons_; + std::vector slow_down_polygons_; }; } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index d58198cb..cabbf3d1 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -23,13 +23,14 @@ #include "boost/geometry.hpp" #include "boost/geometry/geometries/linestring.hpp" #include "boost/geometry/geometries/point_xy.hpp" +// #include "boost/geometry" #include "opencv2/core/core.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "obstacle_stop_planner/util.hpp" #include "obstacle_stop_planner/vehicle.hpp" - +#include "autoware_utils/autoware_utils.hpp" +#include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" namespace motion_planning { @@ -40,11 +41,11 @@ class OneStepPolygon const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width); Polygon GetBoostPolygon() const {return boost_polygon_;} - std::vector GetPolygon() const {return polygon_;} + autoware_utils::Polygon2d GetPolygon() const {return polygon_;} void SetVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} private: - std::vector polygon_; + autoware_utils::Polygon2d polygon_; Polygon boost_polygon_; std::shared_ptr vehicle_info_; @@ -69,65 +70,90 @@ inline void OneStepPolygon::Create( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width) { - std::vector one_step_move_vehicle_corner_points; + autoware_utils::Polygon2d one_step_move_vehicle_corner_points; + const auto footprint = createVehicleFootprint(*vehicle_info_, expand_width); + // start step { double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + + bg::strategy::transform::rotate_transformer rotate(yaw); + autoware_utils::LinearRing2d transformed_footprint; + bg::transform(footprint, transformed_footprint, rotate); + + bg::strategy::transform::translate_transformer translate(base_step_pose.position.x, base_step_pose.position.y); + bg::transform(transformed_footprint, transformed_footprint, translate); + one_step_move_vehicle_corner_points.outer() = transformed_footprint; + + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + // base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + // base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + // base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + // base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); } // next step { double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - one_step_move_vehicle_corner_points.push_back( - cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + bg::strategy::transform::rotate_transformer rotate(yaw); + autoware_utils::LinearRing2d transformed_footprint; + bg::transform(footprint, transformed_footprint, rotate); + + bg::strategy::transform::translate_transformer translate(base_step_pose.position.x, base_step_pose.position.y); + bg::transform(transformed_footprint, transformed_footprint, translate); + for (const auto& item : transformed_footprint) { + one_step_move_vehicle_corner_points.outer().emplace_back(item); + } + + + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + // next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - + // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + // next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + + // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), + // next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); + // one_step_move_vehicle_corner_points.push_back( + // cv::Point2d( + // next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - + // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), + // next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + + // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); } - polygon_ = convexHull(one_step_move_vehicle_corner_points); - boost_polygon_ = ConvertToBoostPolygon(polygon_); + + bg::convex_hull(one_step_move_vehicle_corner_points, polygon_); + // polygon_ = convexHull(one_step_move_vehicle_corner_points); + // boost_polygon_ = ConvertToBoostPolygon(polygon_); } inline std::vector OneStepPolygon::convexHull( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 4f498b34..d733c101 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -27,7 +27,7 @@ namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; -using Polygon = bg::model::polygon; +using Polygon = bg::model::polygon; // clockwise = false using Line = bg::model::linestring; namespace diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp new file mode 100644 index 00000000..c482672a --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp @@ -0,0 +1,47 @@ +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ + +#include +#include + +inline autoware_utils::LinearRing2d createVehicleFootprint( + const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) +{ + using autoware_utils::LinearRing2d; + using autoware_utils::Point2d; + + const auto & i = vehicle_info; + + const double x_front = i.front_overhang_m_ + i.wheel_base_m_ + margin; + const double x_center = i.wheel_base_m_ / 2.0; + const double x_rear = -(i.rear_overhang_m_ + margin); + const double y_left = i.wheel_tread_m_ / 2.0 + i.left_overhang_m_ + margin; + const double y_right = -(i.wheel_tread_m_ / 2.0 + i.right_overhang_m_ + margin); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_center, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_center, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} + +#endif // OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index dee3c7e0..1f6bbe94 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -12,6 +12,7 @@ rclcpp autoware_debug_msgs + autoware_utils autoware_perception_msgs autoware_planning_msgs autoware_utils diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index 35b99c08..50a4635b 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -17,10 +17,7 @@ #include "obstacle_stop_planner/debug_marker.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" - -// namespace { -// convertPose2Transform -// } +#include "autoware_utils/autoware_utils.hpp" namespace motion_planning { @@ -35,32 +32,30 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const std::vector & polygon, const double z, const PolygonType & type) + const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) { - std::vector eigen_polygon; - for (const auto & point : polygon) { - Eigen::Vector3d eigen_point; - eigen_point << point.x, point.y, z; - eigen_polygon.push_back(eigen_point); + autoware_utils::Polygon3d polygon3d; + for (const auto & point : polygon.outer()) { + polygon3d.outer().emplace_back(point.to_3d(z)); } - return pushPolygon(eigen_polygon, type); + return pushPolygon(polygon3d, type); } bool ObstacleStopPlannerDebugNode::pushPolygon( - const std::vector & polygon, const PolygonType & type) + const autoware_utils::Polygon3d & polygon, const PolygonType & type) { switch (type) { case PolygonType::Vehicle: - if (!polygon.empty()) {vehicle_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {vehicle_polygons_.emplace_back(polygon);} return true; case PolygonType::Collision: - if (!polygon.empty()) {collision_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {collision_polygons_.emplace_back(polygon);} return true; case PolygonType::SlowDownRange: - if (!polygon.empty()) {slow_down_range_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {slow_down_range_polygons_.emplace_back(polygon);} return true; case PolygonType::SlowDown: - if (!polygon.empty()) {slow_down_polygons_.push_back(polygon);} + if (!polygon.outer().empty()) {slow_down_polygons_.emplace_back(polygon);} return true; default: return false; @@ -165,26 +160,16 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.g = 1.0; marker.color.b = 0.0; for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { - for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + for (size_t j = 0; j < vehicle_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = vehicle_polygons_.at(i).at(j).x(); - point.y = vehicle_polygons_.at(i).at(j).y(); - point.z = vehicle_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(vehicle_polygons_.at(i).outer().at(j)); marker.points.push_back(point); } - if (j + 1 == vehicle_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = vehicle_polygons_.at(i).at(0).x(); - point.y = vehicle_polygons_.at(i).at(0).y(); - point.z = vehicle_polygons_.at(i).at(0).z(); + if (j + 1 == vehicle_polygons_.at(i).outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg(vehicle_polygons_.at(i).outer().at(0)); marker.points.push_back(point); - } else { - geometry_msgs::msg::Point point; - point.x = vehicle_polygons_.at(i).at(j + 1).x(); - point.y = vehicle_polygons_.at(i).at(j + 1).y(); - point.z = vehicle_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(vehicle_polygons_.at(i).outer().at(j + 1)); marker.points.push_back(point); } } @@ -216,26 +201,17 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.g = 0.0; marker.color.b = 0.0; for (size_t i = 0; i < collision_polygons_.size(); ++i) { - for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + for (size_t j = 0; j < collision_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = collision_polygons_.at(i).at(j).x(); - point.y = collision_polygons_.at(i).at(j).y(); - point.z = collision_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(collision_polygons_.at(i).outer().at(j)); marker.points.push_back(point); } - if (j + 1 == collision_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = collision_polygons_.at(i).at(0).x(); - point.y = collision_polygons_.at(i).at(0).y(); - point.z = collision_polygons_.at(i).at(0).z(); + if (j + 1 == collision_polygons_.at(i).outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg(collision_polygons_.at(i).outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point; - point.x = collision_polygons_.at(i).at(j + 1).x(); - point.y = collision_polygons_.at(i).at(j + 1).y(); - point.z = collision_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(collision_polygons_.at(i).outer().at(j + 1)); marker.points.push_back(point); } } @@ -267,26 +243,17 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.g = 1.0; marker.color.b = 0.0; for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { - for (size_t j = 0; j < slow_down_range_polygons_.at(i).size(); ++j) { + for (size_t j = 0; j < slow_down_range_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = slow_down_range_polygons_.at(i).at(j).x(); - point.y = slow_down_range_polygons_.at(i).at(j).y(); - point.z = slow_down_range_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_range_polygons_.at(i).outer().at(j)); marker.points.push_back(point); } - if (j + 1 == slow_down_range_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = slow_down_range_polygons_.at(i).at(0).x(); - point.y = slow_down_range_polygons_.at(i).at(0).y(); - point.z = slow_down_range_polygons_.at(i).at(0).z(); + if (j + 1 == slow_down_range_polygons_.at(i).outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_range_polygons_.at(i).outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point; - point.x = slow_down_range_polygons_.at(i).at(j + 1).x(); - point.y = slow_down_range_polygons_.at(i).at(j + 1).y(); - point.z = slow_down_range_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_range_polygons_.at(i).outer().at(j + 1)); marker.points.push_back(point); } } @@ -318,26 +285,17 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza marker.color.g = 1.0; marker.color.b = 0.0; for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { - for (size_t j = 0; j < slow_down_polygons_.at(i).size(); ++j) { + for (size_t j = 0; j < slow_down_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point; - point.x = slow_down_polygons_.at(i).at(j).x(); - point.y = slow_down_polygons_.at(i).at(j).y(); - point.z = slow_down_polygons_.at(i).at(j).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_polygons_.at(i).outer().at(j)); marker.points.push_back(point); } - if (j + 1 == slow_down_polygons_.at(i).size()) { - geometry_msgs::msg::Point point; - point.x = slow_down_polygons_.at(i).at(0).x(); - point.y = slow_down_polygons_.at(i).at(0).y(); - point.z = slow_down_polygons_.at(i).at(0).z(); + if (j + 1 == slow_down_polygons_.at(i).outer().size()) { + geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_polygons_.at(i).outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point; - point.x = slow_down_polygons_.at(i).at(j + 1).x(); - point.y = slow_down_polygons_.at(i).at(j + 1).y(); - point.z = slow_down_polygons_.at(i).at(j + 1).z(); + geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_polygons_.at(i).outer().at(j + 1)); marker.points.push_back(point); } } From 1de69cf2649676268ee8525408f1c1806031369c Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 3 Mar 2021 15:00:34 +0900 Subject: [PATCH 15/62] modify createVehicleFootprint() --- .../util/create_vehicle_footprint.hpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp index c482672a..5990e89d 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp @@ -19,26 +19,23 @@ #include inline autoware_utils::LinearRing2d createVehicleFootprint( - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + const vehicle_info_util::VehicleInfo & vehicle_info, const double top_margin = 0.0, const double side_margin = 0.0) { using autoware_utils::LinearRing2d; using autoware_utils::Point2d; const auto & i = vehicle_info; - const double x_front = i.front_overhang_m_ + i.wheel_base_m_ + margin; - const double x_center = i.wheel_base_m_ / 2.0; - const double x_rear = -(i.rear_overhang_m_ + margin); - const double y_left = i.wheel_tread_m_ / 2.0 + i.left_overhang_m_ + margin; - const double y_right = -(i.wheel_tread_m_ / 2.0 + i.right_overhang_m_ + margin); + const double x_front = i.front_overhang_m_ + i.wheel_base_m_ + top_margin; + const double x_rear = -(i.rear_overhang_m_ + top_margin); + const double y_left = i.wheel_tread_m_ / 2.0 + i.left_overhang_m_ + side_margin; + const double y_right = -(i.wheel_tread_m_ / 2.0 + i.right_overhang_m_ + side_margin); LinearRing2d footprint; footprint.push_back(Point2d{x_front, y_left}); footprint.push_back(Point2d{x_front, y_right}); - footprint.push_back(Point2d{x_center, y_right}); footprint.push_back(Point2d{x_rear, y_right}); footprint.push_back(Point2d{x_rear, y_left}); - footprint.push_back(Point2d{x_center, y_left}); footprint.push_back(Point2d{x_front, y_left}); return footprint; From b4f2b0fef8cd35979083bfa4a2d3ae63be8f5713 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 3 Mar 2021 15:04:30 +0900 Subject: [PATCH 16/62] cleanup code --- .../include/obstacle_stop_planner/node.hpp | 2 +- .../one_step_polygon.hpp | 102 +----------------- obstacle_stop_planner_refine/package.xml | 1 - obstacle_stop_planner_refine/src/node.cpp | 6 +- 4 files changed, 5 insertions(+), 106 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 85ba0562..0bc0c45e 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -99,7 +99,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path); double calcSlowDownTargetVel(const double lateral_deviation); - void getSlowDownPointcloud(const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const Polygon& one_step_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down); + void getSlowDownPointcloud(const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const autoware_utils::Polygon2d& one_step_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down); void getCollisionPointcloud(const pcl::PointCloud::Ptr slow_down_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const OneStepPolygon& one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool& is_collision); void insertStopPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_collision_point, const PointHelper& point_helper, autoware_planning_msgs::msg::Trajectory & output_msg, diagnostic_msgs::msg::DiagnosticStatus& stop_reason_diag); void insertSlowDownPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_slow_down_point, const PointHelper& point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index cabbf3d1..36f88780 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -23,10 +23,6 @@ #include "boost/geometry.hpp" #include "boost/geometry/geometries/linestring.hpp" #include "boost/geometry/geometries/point_xy.hpp" -// #include "boost/geometry" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/vehicle.hpp" #include "autoware_utils/autoware_utils.hpp" @@ -40,38 +36,20 @@ class OneStepPolygon void Create( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width); - Polygon GetBoostPolygon() const {return boost_polygon_;} autoware_utils::Polygon2d GetPolygon() const {return polygon_;} void SetVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} private: autoware_utils::Polygon2d polygon_; - Polygon boost_polygon_; std::shared_ptr vehicle_info_; - - Polygon ConvertToBoostPolygon(const std::vector& polygon); - std::vector convexHull(const std::vector pointcloud); }; -inline Polygon OneStepPolygon::ConvertToBoostPolygon(const std::vector& polygon) -{ - // convert boost polygon - Polygon boost_polygon; - for (const auto & point : polygon) { - boost_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_polygon.outer().push_back( - bg::make( - polygon.front().x, polygon.front().y)); - return boost_polygon; -} - inline void OneStepPolygon::Create( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width) { autoware_utils::Polygon2d one_step_move_vehicle_corner_points; - const auto footprint = createVehicleFootprint(*vehicle_info_, expand_width); + const auto footprint = createVehicleFootprint(*vehicle_info_, 0.0, expand_width); // start step { @@ -84,32 +62,6 @@ inline void OneStepPolygon::Create( bg::strategy::transform::translate_transformer translate(base_step_pose.position.x, base_step_pose.position.y); bg::transform(transformed_footprint, transformed_footprint, translate); one_step_move_vehicle_corner_points.outer() = transformed_footprint; - - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - // base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); - - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // base_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - // base_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - // base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // base_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - // base_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); } // next step { @@ -123,61 +75,9 @@ inline void OneStepPolygon::Create( for (const auto& item : transformed_footprint) { one_step_move_vehicle_corner_points.outer().emplace_back(item); } - - - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - // next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // next_step_pose.position.x + std::cos(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) - - // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - // next_step_pose.position.y + std::sin(yaw) * (vehicle_info_->wheel_base_m_ + vehicle_info_->front_overhang_m_) + - // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - // std::sin(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width), - // next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - // std::cos(yaw) * (-vehicle_info_->vehicle_width_m_ / 2.0 - expand_width))); - // one_step_move_vehicle_corner_points.push_back( - // cv::Point2d( - // next_step_pose.position.x + std::cos(yaw) * (-vehicle_info_->rear_overhang_m_) - - // std::sin(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width), - // next_step_pose.position.y + std::sin(yaw) * (-vehicle_info_->rear_overhang_m_) + - // std::cos(yaw) * (vehicle_info_->vehicle_width_m_ / 2.0 + expand_width))); } bg::convex_hull(one_step_move_vehicle_corner_points, polygon_); - // polygon_ = convexHull(one_step_move_vehicle_corner_points); - // boost_polygon_ = ConvertToBoostPolygon(polygon_); -} - -inline std::vector OneStepPolygon::convexHull( - const std::vector pointcloud) -{ - auto centroid = calcCentroid(pointcloud); - - std::vector normalized_pointcloud; - std::vector normalized_polygon_points; - for (size_t i = 0; i < pointcloud.size(); ++i) { - normalized_pointcloud.push_back( - cv::Point( - (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0)); - } - cv::convexHull(normalized_pointcloud, normalized_polygon_points); - - std::vector polygon_points{normalized_polygon_points.size()}; - 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.emplace_back(polygon_point); - } - return polygon_points; } } // namespace motion_planning diff --git a/obstacle_stop_planner_refine/package.xml b/obstacle_stop_planner_refine/package.xml index 1f6bbe94..72366d5d 100644 --- a/obstacle_stop_planner_refine/package.xml +++ b/obstacle_stop_planner_refine/package.xml @@ -31,7 +31,6 @@ ament_cmake_gtest ament_lint_auto ament_lint_common - ament_cmake diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 6b0ace42..5f317541 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -202,7 +202,7 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - getSlowDownPointcloud(is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point,next_center_point, vehicle_info_.slow_down_search_radius_, move_slow_down_range_polygon.GetBoostPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); + getSlowDownPointcloud(is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point,next_center_point, vehicle_info_.slow_down_search_radius_, move_slow_down_range_polygon.GetPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); getCollisionPointcloud(slow_down_pointcloud_ptr, prev_center_point, next_center_point, vehicle_info_.stop_search_radius_, move_vehicle_polygon, trajectory.points.at(i), collision_pointcloud_ptr, is_collision); @@ -271,7 +271,7 @@ void ObstacleStopPlannerNode::getCollisionPointcloud(const pcl::PointCloudpush_back(slow_down_pointcloud->at(j)); is_collision = true; debug_ptr_->pushPolygon( @@ -283,7 +283,7 @@ void ObstacleStopPlannerNode::getCollisionPointcloud(const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const Polygon& boost_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down) +void ObstacleStopPlannerNode::getSlowDownPointcloud(const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const autoware_utils::Polygon2d& boost_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down) { if (!is_slow_down && enable_slow_down) { for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { From aeef6aa3d3062acf7d2928725c089e2da15698bc Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 11:11:22 +0900 Subject: [PATCH 17/62] change namespace --- .../obstacle_stop_planner/adaptive_cruise_control.hpp | 4 ++-- .../include/obstacle_stop_planner/debug_marker.hpp | 4 ++-- .../include/obstacle_stop_planner/node.hpp | 6 +++--- .../include/obstacle_stop_planner/obstacle_point_cloud.hpp | 4 ++-- .../include/obstacle_stop_planner/one_step_polygon.hpp | 4 ++-- .../include/obstacle_stop_planner/point_helper.hpp | 4 ++-- .../include/obstacle_stop_planner/trajectory.hpp | 4 ++-- .../include/obstacle_stop_planner/vehicle.hpp | 4 ++-- .../src/adaptive_cruise_control.cpp | 4 ++-- obstacle_stop_planner_refine/src/debug_marker.cpp | 4 ++-- obstacle_stop_planner_refine/src/main.cpp | 2 +- obstacle_stop_planner_refine/src/node.cpp | 6 +++--- obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp | 4 ++-- obstacle_stop_planner_refine/src/point_helper.cpp | 4 ++-- obstacle_stop_planner_refine/src/trajectory.cpp | 4 ++-- 15 files changed, 31 insertions(+), 31 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 9b6283f9..0a65e469 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -27,7 +27,7 @@ #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -namespace motion_planning +namespace obstacle_stop_planner { class AdaptiveCruiseController { @@ -221,6 +221,6 @@ class AdaptiveCruiseController static constexpr unsigned int num_debug_values_ = 10; }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp index 497721a6..fed06f2c 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp @@ -32,7 +32,7 @@ #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" -namespace motion_planning +namespace obstacle_stop_planner { enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown }; @@ -73,6 +73,6 @@ class ObstacleStopPlannerDebugNode std::vector slow_down_polygons_; }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 0bc0c45e..4ba765fe 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -45,7 +45,7 @@ #include "obstacle_stop_planner/one_step_polygon.hpp" #include "obstacle_stop_planner/point_helper.hpp" -namespace motion_planning +namespace obstacle_stop_planner { class ObstacleStopPlannerNode : public rclcpp::Node { @@ -78,7 +78,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node /* * Parameter */ - std::unique_ptr acc_controller_; + std::unique_ptr acc_controller_; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_; rclcpp::Time prev_col_point_time_; @@ -105,6 +105,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node void insertSlowDownPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_slow_down_point, const PointHelper& point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg); }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__NODE_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 39901735..f9a9df3a 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -28,7 +28,7 @@ #include "autoware_planning_msgs/msg/trajectory.hpp" #include "obstacle_stop_planner/vehicle.hpp" -namespace motion_planning { +namespace obstacle_stop_planner { class ObstaclePointCloud { @@ -54,6 +54,6 @@ class ObstaclePointCloud std::shared_ptr vehicle_info_; }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 36f88780..f35b6580 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -28,7 +28,7 @@ #include "autoware_utils/autoware_utils.hpp" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" -namespace motion_planning { +namespace obstacle_stop_planner { class OneStepPolygon { @@ -80,6 +80,6 @@ inline void OneStepPolygon::Create( bg::convex_hull(one_step_move_vehicle_corner_points, polygon_); } -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index b5954cf5..a9e8ec1b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -30,7 +30,7 @@ #include "eigen3/Eigen/Geometry" -namespace motion_planning { +namespace obstacle_stop_planner { struct StopPoint { @@ -92,6 +92,6 @@ class PointHelper private: std::shared_ptr vehicle_info_; }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index 743cc923..e90e7ced 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -23,7 +23,7 @@ #include "obstacle_stop_planner/vehicle.hpp" #include "obstacle_stop_planner/point_helper.hpp" -namespace motion_planning { +namespace obstacle_stop_planner { class Trajectory { @@ -49,6 +49,6 @@ class Trajectory const double extend_distance, autoware_planning_msgs::msg::Trajectory & output_trajectory); }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp index fb9595d8..abc7c1e0 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/util.hpp" -namespace motion_planning { +namespace obstacle_stop_planner { class VehicleInfo : public vehicle_info_util::VehicleInfo { @@ -93,6 +93,6 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo private: }; -} // namespace motion_planning +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index e2426750..9e9e10c0 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -28,7 +28,7 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/util.hpp" -namespace motion_planning +namespace obstacle_stop_planner { AdaptiveCruiseController::AdaptiveCruiseController( rclcpp::Node * node, const double vehicle_width, const double vehicle_length, @@ -596,4 +596,4 @@ double AdaptiveCruiseController::lowpass_filter( return gain * prev_value + (1.0 - gain) * current_value; } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index 50a4635b..a841b457 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -19,7 +19,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "autoware_utils/autoware_utils.hpp" -namespace motion_planning +namespace obstacle_stop_planner { ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) @@ -580,4 +580,4 @@ autoware_planning_msgs::msg::StopReasonArray ObstacleStopPlannerDebugNode::makeS return stop_reason_array; } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/main.cpp b/obstacle_stop_planner_refine/src/main.cpp index f002a4cd..21c0d8f6 100644 --- a/obstacle_stop_planner_refine/src/main.cpp +++ b/obstacle_stop_planner_refine/src/main.cpp @@ -20,7 +20,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 5f317541..57ff575b 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -38,7 +38,7 @@ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" -namespace motion_planning +namespace obstacle_stop_planner { namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; @@ -56,7 +56,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() debug_ptr_ = std::make_shared(this, vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_); // Initializer - acc_controller_ = std::make_unique( + acc_controller_ = std::make_unique( this, vehicle_info_.vehicle_width_m_, vehicle_info_.vehicle_length_m_, vehicle_info_.wheel_base_m_, vehicle_info_.front_overhang_m_); // Publishers @@ -446,4 +446,4 @@ bool ObstacleStopPlannerNode::getSelfPose( return false; } } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 55cffcad..45a684d4 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -17,7 +17,7 @@ #include "tf2/utils.h" #include "tf2_eigen/tf2_eigen.h" -namespace motion_planning +namespace obstacle_stop_planner { ObstaclePointCloud::ObstaclePointCloud(rclcpp::Logger logger) : logger_(logger) @@ -117,4 +117,4 @@ bool ObstaclePointCloud::searchPointcloudNearTrajectory( return true; } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index b96c2b89..6dbfdb83 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -15,7 +15,7 @@ #include "obstacle_stop_planner/point_helper.hpp" #include "pcl_conversions/pcl_conversions.h" -namespace motion_planning +namespace obstacle_stop_planner { bool PointHelper::getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, @@ -229,4 +229,4 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::getExtendTrajectoryPoi return extend_trajectory_point; } -} // namespace motion_planning +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 96fc96e4..06eedc57 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -14,7 +14,7 @@ #include "obstacle_stop_planner/trajectory.hpp" -namespace motion_planning +namespace obstacle_stop_planner { bool Trajectory::decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, @@ -126,4 +126,4 @@ bool Trajectory::extendTrajectory( return true; } -} // namespace motion_planning +} // namespace obstacle_stop_planner From 5568f74b62e0b095a0cfda566afb4d0d595630ee Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 13:44:52 +0900 Subject: [PATCH 18/62] use camelCase for function name --- .../obstacle_point_cloud.hpp | 10 +++--- .../one_step_polygon.hpp | 8 ++--- .../obstacle_stop_planner/point_helper.hpp | 4 +-- obstacle_stop_planner_refine/src/node.cpp | 32 +++++++++---------- .../src/obstacle_point_cloud.cpp | 10 +++--- 5 files changed, 32 insertions(+), 32 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index f9a9df3a..091d1df0 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -35,12 +35,12 @@ class ObstaclePointCloud public: explicit ObstaclePointCloud(rclcpp::Logger logger); - void SetPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void SetSearchRadius(const double value); - void SetVehicleInfo(const VehicleInfo vehicle_info); + void setPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void setSearchRadius(const double value); + void setVehicleInfo(const VehicleInfo vehicle_info); - bool IsDataReceived(); - pcl::PointCloud::Ptr SearchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory); + bool isDataReceived(); + pcl::PointCloud::Ptr searchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory); private: bool searchPointcloudNearTrajectory( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index f35b6580..bd9ddac2 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -33,18 +33,18 @@ namespace obstacle_stop_planner { class OneStepPolygon { public: - void Create( + void create( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width); - autoware_utils::Polygon2d GetPolygon() const {return polygon_;} - void SetVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} + autoware_utils::Polygon2d getPolygon() const {return polygon_;} + void setVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} private: autoware_utils::Polygon2d polygon_; std::shared_ptr vehicle_info_; }; -inline void OneStepPolygon::Create( +inline void OneStepPolygon::create( const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width) { diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index a9e8ec1b..b4d3b7aa 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -48,8 +48,8 @@ struct SlowDownPoint class PointHelper { public: - void SetVehicleInfo(const VehicleInfo vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } - void SetVehicleInfo(std::shared_ptr vehicle_info) { vehicle_info_ = vehicle_info; } + void setVehicleInfo(const VehicleInfo vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } + void setVehicleInfo(std::shared_ptr vehicle_info) { vehicle_info_ = vehicle_info; } bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 57ff575b..ab14ce3e 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -52,7 +52,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() , obstacle_pointcloud_(this->get_logger()) , vehicle_info_(vehicle_info_util::VehicleInfo::create(*this), this->get_node_parameters_interface()) { - obstacle_pointcloud_.SetVehicleInfo(vehicle_info_); + obstacle_pointcloud_.setVehicleInfo(vehicle_info_); debug_ptr_ = std::make_shared(this, vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_); // Initializer @@ -88,12 +88,12 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() void ObstacleStopPlannerNode::obstaclePointcloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { - obstacle_pointcloud_.SetPointCloud(input_msg); + obstacle_pointcloud_.setPointCloud(input_msg); } void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) { - if (!obstacle_pointcloud_.IsDataReceived()) { + if (!obstacle_pointcloud_.isDataReceived()) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for obstacle pointcloud..."); @@ -144,8 +144,8 @@ void ObstacleStopPlannerNode::pathCallback( // search obstacle candidate pointcloud to reduce calculation cost const double search_radius = vehicle_info_.getSearchRadius(); - obstacle_pointcloud_.SetSearchRadius(search_radius); - obstacle_pointcloud_.SearchCandidateObstacle(tf_buffer_, trajectory); + obstacle_pointcloud_.setSearchRadius(search_radius); + obstacle_pointcloud_.searchCandidateObstacle(tf_buffer_, trajectory); /* * check collision, slow_down @@ -165,7 +165,7 @@ void ObstacleStopPlannerNode::pathCallback( double lateral_deviation = 0.0; PointHelper point_helper; - point_helper.SetVehicleInfo(vehicle_info_); + point_helper.setVehicleInfo(vehicle_info_); for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { /* @@ -179,22 +179,22 @@ void ObstacleStopPlannerNode::pathCallback( * create one step polygon for vehicle */ OneStepPolygon move_vehicle_polygon; - move_vehicle_polygon.SetVehicleInfo(vehicle_info_); - move_vehicle_polygon.Create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + move_vehicle_polygon.setVehicleInfo(vehicle_info_); + move_vehicle_polygon.create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_stop_range_); debug_ptr_->pushPolygon( - move_vehicle_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); + move_vehicle_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); OneStepPolygon move_slow_down_range_polygon; - move_slow_down_range_polygon.SetVehicleInfo(vehicle_info_); + move_slow_down_range_polygon.setVehicleInfo(vehicle_info_); if (vehicle_info_.enable_slow_down_) { /* * create one step polygon for slow_down range */ - move_slow_down_range_polygon.Create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + move_slow_down_range_polygon.create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_slow_down_range_); debug_ptr_->pushPolygon( - move_slow_down_range_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, + move_slow_down_range_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); } @@ -202,7 +202,7 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - getSlowDownPointcloud(is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point,next_center_point, vehicle_info_.slow_down_search_radius_, move_slow_down_range_polygon.GetPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); + getSlowDownPointcloud(is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point,next_center_point, vehicle_info_.slow_down_search_radius_, move_slow_down_range_polygon.getPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); getCollisionPointcloud(slow_down_pointcloud_ptr, prev_center_point, next_center_point, vehicle_info_.stop_search_radius_, move_vehicle_polygon, trajectory.points.at(i), collision_pointcloud_ptr, is_collision); @@ -210,7 +210,7 @@ void ObstacleStopPlannerNode::pathCallback( is_slow_down = true; decimate_trajectory_slow_down_index = i; debug_ptr_->pushPolygon( - move_slow_down_range_polygon.GetPolygon(), trajectory.points.at(i).pose.position.z, + move_slow_down_range_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); point_helper.getNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_slow_down_point, @@ -271,11 +271,11 @@ void ObstacleStopPlannerNode::getCollisionPointcloud(const pcl::PointCloudpush_back(slow_down_pointcloud->at(j)); is_collision = true; debug_ptr_->pushPolygon( - one_step_polygon.GetPolygon(), trajectory_point.pose.position.z, + one_step_polygon.getPolygon(), trajectory_point.pose.position.z, PolygonType::Collision); } } diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 45a684d4..51a681cc 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -24,7 +24,7 @@ ObstaclePointCloud::ObstaclePointCloud(rclcpp::Logger logger) { } -void ObstaclePointCloud::SetPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +void ObstaclePointCloud::setPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { obstacle_ros_pointcloud_ptr_ = std::make_shared(); pcl::VoxelGrid filter; @@ -45,22 +45,22 @@ void ObstaclePointCloud::SetPointCloud(const sensor_msgs::msg::PointCloud2::Cons obstacle_ros_pointcloud_ptr_->header = msg->header; } -void ObstaclePointCloud::SetSearchRadius(const double value) +void ObstaclePointCloud::setSearchRadius(const double value) { search_radius_ = value; } -void ObstaclePointCloud::SetVehicleInfo(const VehicleInfo vehicle_info) +void ObstaclePointCloud::setVehicleInfo(const VehicleInfo vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } -bool ObstaclePointCloud::IsDataReceived() +bool ObstaclePointCloud::isDataReceived() { return obstacle_ros_pointcloud_ptr_ != nullptr ? true : false; } -pcl::PointCloud::Ptr ObstaclePointCloud::SearchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory) +pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory) { pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( new pcl::PointCloud); From 0ca8a0eca4e5f44a9e586d2ee78327cc2911a22d Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 13:48:18 +0900 Subject: [PATCH 19/62] apply uncrustify --- .../include/obstacle_stop_planner/node.hpp | 31 ++++- .../obstacle_point_cloud.hpp | 7 +- .../one_step_polygon.hpp | 16 ++- .../obstacle_stop_planner/point_helper.hpp | 10 +- .../obstacle_stop_planner/trajectory.hpp | 9 +- .../include/obstacle_stop_planner/util.hpp | 24 ++-- .../util/create_vehicle_footprint.hpp | 3 +- .../include/obstacle_stop_planner/vehicle.hpp | 79 ++++++++---- .../src/adaptive_cruise_control.cpp | 4 +- .../src/debug_marker.cpp | 48 +++++-- obstacle_stop_planner_refine/src/node.cpp | 121 +++++++++++++----- .../src/obstacle_point_cloud.cpp | 3 +- .../src/point_helper.cpp | 14 +- .../src/trajectory.cpp | 26 ++-- .../test/src/test_one_step_polygon.cpp | 14 ++ 15 files changed, 294 insertions(+), 115 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 4ba765fe..e4046056 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -99,10 +99,33 @@ class ObstacleStopPlannerNode : public rclcpp::Node const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path); double calcSlowDownTargetVel(const double lateral_deviation); - void getSlowDownPointcloud(const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const autoware_utils::Polygon2d& one_step_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down); - void getCollisionPointcloud(const pcl::PointCloud::Ptr slow_down_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const OneStepPolygon& one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool& is_collision); - void insertStopPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_collision_point, const PointHelper& point_helper, autoware_planning_msgs::msg::Trajectory & output_msg, diagnostic_msgs::msg::DiagnosticStatus& stop_reason_diag); - void insertSlowDownPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_slow_down_point, const PointHelper& point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg); + void getSlowDownPointcloud( + const bool is_slow_down, const bool enable_slow_down, + const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, + const Point & prev_center_point, const Point & next_center_point, const double search_radius, + const autoware_utils::Polygon2d & one_step_polygon, + pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down); + void getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, + const Point & prev_center_point, const Point & next_center_point, + const double search_radius, const OneStepPolygon & one_step_polygon, + const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, + pcl::PointCloud::Ptr collision_pointcloud, + bool & is_collision); + void insertStopPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const pcl::PointXYZ & nearest_collision_point, + const PointHelper & point_helper, + autoware_planning_msgs::msg::Trajectory & output_msg, + diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag); + void insertSlowDownPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const pcl::PointXYZ & nearest_slow_down_point, + const PointHelper & point_helper, const double slow_down_target_vel, + const double slow_down_margin, + autoware_planning_msgs::msg::Trajectory & output_msg); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 091d1df0..e8ec6098 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -28,7 +28,8 @@ #include "autoware_planning_msgs/msg/trajectory.hpp" #include "obstacle_stop_planner/vehicle.hpp" -namespace obstacle_stop_planner { +namespace obstacle_stop_planner +{ class ObstaclePointCloud { @@ -40,7 +41,9 @@ class ObstaclePointCloud void setVehicleInfo(const VehicleInfo vehicle_info); bool isDataReceived(); - pcl::PointCloud::Ptr searchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory); + pcl::PointCloud::Ptr searchCandidateObstacle( + const tf2_ros::Buffer & tf_buffer, + const autoware_planning_msgs::msg::Trajectory & trajectory); private: bool searchPointcloudNearTrajectory( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index bd9ddac2..54d3bcb5 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -28,7 +28,8 @@ #include "autoware_utils/autoware_utils.hpp" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" -namespace obstacle_stop_planner { +namespace obstacle_stop_planner +{ class OneStepPolygon { @@ -37,7 +38,10 @@ class OneStepPolygon const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, const double expand_width); autoware_utils::Polygon2d getPolygon() const {return polygon_;} - void setVehicleInfo(VehicleInfo vehicle_info) {vehicle_info_ = std::make_shared(vehicle_info);} + void setVehicleInfo(VehicleInfo vehicle_info) + { + vehicle_info_ = std::make_shared(vehicle_info); + } private: autoware_utils::Polygon2d polygon_; @@ -59,7 +63,8 @@ inline void OneStepPolygon::create( autoware_utils::LinearRing2d transformed_footprint; bg::transform(footprint, transformed_footprint, rotate); - bg::strategy::transform::translate_transformer translate(base_step_pose.position.x, base_step_pose.position.y); + bg::strategy::transform::translate_transformer translate( + base_step_pose.position.x, base_step_pose.position.y); bg::transform(transformed_footprint, transformed_footprint, translate); one_step_move_vehicle_corner_points.outer() = transformed_footprint; } @@ -70,9 +75,10 @@ inline void OneStepPolygon::create( autoware_utils::LinearRing2d transformed_footprint; bg::transform(footprint, transformed_footprint, rotate); - bg::strategy::transform::translate_transformer translate(base_step_pose.position.x, base_step_pose.position.y); + bg::strategy::transform::translate_transformer translate( + base_step_pose.position.x, base_step_pose.position.y); bg::transform(transformed_footprint, transformed_footprint, translate); - for (const auto& item : transformed_footprint) { + for (const auto & item : transformed_footprint) { one_step_move_vehicle_corner_points.outer().emplace_back(item); } } diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index b4d3b7aa..5a122f0d 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -30,7 +30,8 @@ #include "eigen3/Eigen/Geometry" -namespace obstacle_stop_planner { +namespace obstacle_stop_planner +{ struct StopPoint { @@ -48,8 +49,11 @@ struct SlowDownPoint class PointHelper { public: - void setVehicleInfo(const VehicleInfo vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } - void setVehicleInfo(std::shared_ptr vehicle_info) { vehicle_info_ = vehicle_info; } + void setVehicleInfo(const VehicleInfo vehicle_info) + { + vehicle_info_ = std::make_shared(vehicle_info); + } + void setVehicleInfo(std::shared_ptr vehicle_info) {vehicle_info_ = vehicle_info;} bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index e90e7ced..2c83cb57 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -23,16 +23,19 @@ #include "obstacle_stop_planner/vehicle.hpp" #include "obstacle_stop_planner/point_helper.hpp" -namespace obstacle_stop_planner { +namespace obstacle_stop_planner +{ class Trajectory { public: bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, + const VehicleInfo & vehicle_info, autoware_planning_msgs::msg::Trajectory & output_trajectory); bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, + const VehicleInfo & vehicle_info, autoware_planning_msgs::msg::Trajectory & output_trajectory, std::map & index_map); bool trimTrajectoryFromSelfPose( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index d733c101..c81d997b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -71,8 +71,7 @@ inline cv::Point2d calcCentroid(const std::vector pointcloud) cv::Point2d centroid; centroid.x = 0; centroid.y = 0; - for (const auto &point : pointcloud) - { + for (const auto & point : pointcloud) { centroid.x += point.x; centroid.y += point.y; } @@ -81,13 +80,13 @@ inline cv::Point2d calcCentroid(const std::vector pointcloud) return centroid; } -inline Point convertPointRosToBoost(const geometry_msgs::msg::Point &point) +inline Point convertPointRosToBoost(const geometry_msgs::msg::Point & point) { const Point point2d(point.x, point.y); return point2d; } -inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion &q) +inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) { tf2::Quaternion quat(q.x, q.y, q.z, q.w); tf2::Matrix3x3 mat(quat); @@ -100,8 +99,9 @@ inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quatern return rpy; } -inline Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size, - const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) +inline Polygon getPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, + const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) { Polygon obj_poly; geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); @@ -109,8 +109,9 @@ inline Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_m double l = size.x * std::cos(obj_rpy.y) + l_margin; double w = size.y * std::cos(obj_rpy.x) + w_margin; double co = center_offset; - bg::exterior_ring(obj_poly) = boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( - -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); + bg::exterior_ring(obj_poly) = + boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( + -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); // rotate polygon bg::strategy::transform::rotate_transformer rotate(-obj_rpy.z); // original:clockwise @@ -118,13 +119,16 @@ inline Polygon getPolygon(const geometry_msgs::msg::Pose &pose, const geometry_m Polygon rotate_obj_poly; bg::transform(obj_poly, rotate_obj_poly, rotate); // translate polygon - bg::strategy::transform::translate_transformer translate(pose.position.x, pose.position.y); + bg::strategy::transform::translate_transformer translate(pose.position.x, + pose.position.y); Polygon translate_obj_poly; bg::transform(rotate_obj_poly, translate_obj_poly, translate); return translate_obj_poly; } -inline double getDistanceFromTwoPoint(const geometry_msgs::msg::Point &point1, const geometry_msgs::msg::Point &point2) +inline double getDistanceFromTwoPoint( + const geometry_msgs::msg::Point & point1, + const geometry_msgs::msg::Point & point2) { const double dx = point1.x - point2.x; const double dy = point1.y - point2.y; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp index 5990e89d..3297706b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp @@ -19,7 +19,8 @@ #include inline autoware_utils::LinearRing2d createVehicleFootprint( - const vehicle_info_util::VehicleInfo & vehicle_info, const double top_margin = 0.0, const double side_margin = 0.0) + const vehicle_info_util::VehicleInfo & vehicle_info, const double top_margin = 0.0, + const double side_margin = 0.0) { using autoware_utils::LinearRing2d; using autoware_utils::Point2d; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp index abc7c1e0..7717b7c1 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -21,40 +21,65 @@ #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/util.hpp" -namespace obstacle_stop_planner { +namespace obstacle_stop_planner +{ class VehicleInfo : public vehicle_info_util::VehicleInfo { public: - VehicleInfo(vehicle_info_util::VehicleInfo super, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter) - : vehicle_info_util::VehicleInfo(super) - { - // Parameters - stop_margin_ = parameter->declare_parameter("stop_planner.stop_margin", rclcpp::ParameterValue(5.0)).get(); - min_behavior_stop_margin_ = parameter->declare_parameter("stop_planner.min_behavior_stop_margin", rclcpp::ParameterValue(2.0)).get(); - step_length_ = parameter->declare_parameter("stop_planner.step_length", rclcpp::ParameterValue(1.0)).get(); - extend_distance_ = parameter->declare_parameter("stop_planner.extend_distance", rclcpp::ParameterValue(0.0)).get(); - expand_stop_range_ = parameter->declare_parameter("stop_planner.expand_stop_range", rclcpp::ParameterValue(0.0)).get(); + VehicleInfo( + vehicle_info_util::VehicleInfo super, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter) + : vehicle_info_util::VehicleInfo(super) + { + // Parameters + stop_margin_ = + parameter->declare_parameter( + "stop_planner.stop_margin", + rclcpp::ParameterValue(5.0)).get(); + min_behavior_stop_margin_ = parameter->declare_parameter( + "stop_planner.min_behavior_stop_margin", rclcpp::ParameterValue(2.0)).get(); + step_length_ = + parameter->declare_parameter( + "stop_planner.step_length", + rclcpp::ParameterValue(1.0)).get(); + extend_distance_ = parameter->declare_parameter( + "stop_planner.extend_distance", rclcpp::ParameterValue( + 0.0)).get(); + expand_stop_range_ = parameter->declare_parameter( + "stop_planner.expand_stop_range", rclcpp::ParameterValue( + 0.0)).get(); - slow_down_margin_ = parameter->declare_parameter("slow_down_planner.slow_down_margin", rclcpp::ParameterValue(5.0)).get(); - expand_slow_down_range_ = parameter->declare_parameter("slow_down_planner.expand_slow_down_range", rclcpp::ParameterValue(1.0)).get(); - max_slow_down_vel_ = parameter->declare_parameter("slow_down_planner.max_slow_down_vel", rclcpp::ParameterValue(4.0)).get(); - min_slow_down_vel_ = parameter->declare_parameter("slow_down_planner.min_slow_down_vel", rclcpp::ParameterValue(2.0)).get(); - max_deceleration_ = parameter->declare_parameter("slow_down_planner.max_deceleration", rclcpp::ParameterValue(2.0)).get(); - enable_slow_down_ = parameter->declare_parameter("enable_slow_down", rclcpp::ParameterValue(false)).get(); + slow_down_margin_ = parameter->declare_parameter( + "slow_down_planner.slow_down_margin", rclcpp::ParameterValue( + 5.0)).get(); + expand_slow_down_range_ = parameter->declare_parameter( + "slow_down_planner.expand_slow_down_range", rclcpp::ParameterValue(1.0)).get(); + max_slow_down_vel_ = parameter->declare_parameter( + "slow_down_planner.max_slow_down_vel", rclcpp::ParameterValue( + 4.0)).get(); + min_slow_down_vel_ = parameter->declare_parameter( + "slow_down_planner.min_slow_down_vel", rclcpp::ParameterValue( + 2.0)).get(); + max_deceleration_ = parameter->declare_parameter( + "slow_down_planner.max_deceleration", rclcpp::ParameterValue( + 2.0)).get(); + enable_slow_down_ = + parameter->declare_parameter("enable_slow_down", rclcpp::ParameterValue(false)).get(); - stop_margin_ += wheel_base_m_ + front_overhang_m_; - min_behavior_stop_margin_ += wheel_base_m_ + front_overhang_m_; - slow_down_margin_ += wheel_base_m_ + front_overhang_m_; - stop_search_radius_ = - step_length_ + std::hypot(vehicle_width_m_ / 2.0 + expand_stop_range_, vehicle_length_m_ / 2.0); - slow_down_search_radius_ = - step_length_ + - std::hypot(vehicle_width_m_ / 2.0 + expand_slow_down_range_, vehicle_length_m_ / 2.0); - } + stop_margin_ += wheel_base_m_ + front_overhang_m_; + min_behavior_stop_margin_ += wheel_base_m_ + front_overhang_m_; + slow_down_margin_ += wheel_base_m_ + front_overhang_m_; + stop_search_radius_ = + step_length_ + + std::hypot(vehicle_width_m_ / 2.0 + expand_stop_range_, vehicle_length_m_ / 2.0); + slow_down_search_radius_ = + step_length_ + + std::hypot(vehicle_width_m_ / 2.0 + expand_slow_down_range_, vehicle_length_m_ / 2.0); + } geometry_msgs::msg::Pose getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose) const + const geometry_msgs::msg::Pose & base_pose) const { geometry_msgs::msg::Pose center_pose; const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); @@ -69,7 +94,7 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo double getSearchRadius() const { - if(enable_slow_down_) { + if (enable_slow_down_) { return slow_down_search_radius_; } else { return stop_search_radius_; diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 9e9e10c0..1c145ad5 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -133,12 +133,12 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( */ if (param_.use_pcl_to_est_vel) { success_estm_vel = estimatePointVelocityFromPcl( - traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity); + traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity); } if (param_.use_object_to_est_vel) { success_estm_vel = estimatePointVelocityFromObject( - object_ptr, traj_yaw, nearest_collision_point, &point_velocity); + object_ptr, traj_yaw, nearest_collision_point, &point_velocity); } if (param_.use_rough_est_vel && !success_estm_vel) { diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index a841b457..2a6a98e8 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -162,14 +162,20 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { for (size_t j = 0; j < vehicle_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point = autoware_utils::toMsg(vehicle_polygons_.at(i).outer().at(j)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + vehicle_polygons_.at(i).outer().at( + j)); marker.points.push_back(point); } if (j + 1 == vehicle_polygons_.at(i).outer().size()) { - geometry_msgs::msg::Point point = autoware_utils::toMsg(vehicle_polygons_.at(i).outer().at(0)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + vehicle_polygons_.at(i).outer().at( + 0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point = autoware_utils::toMsg(vehicle_polygons_.at(i).outer().at(j + 1)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + vehicle_polygons_.at(i).outer().at( + j + 1)); marker.points.push_back(point); } } @@ -203,15 +209,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza for (size_t i = 0; i < collision_polygons_.size(); ++i) { for (size_t j = 0; j < collision_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point = autoware_utils::toMsg(collision_polygons_.at(i).outer().at(j)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + collision_polygons_.at( + i).outer().at(j)); marker.points.push_back(point); } if (j + 1 == collision_polygons_.at(i).outer().size()) { - geometry_msgs::msg::Point point = autoware_utils::toMsg(collision_polygons_.at(i).outer().at(0)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + collision_polygons_.at( + i).outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point = autoware_utils::toMsg(collision_polygons_.at(i).outer().at(j + 1)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + collision_polygons_.at( + i).outer().at(j + 1)); marker.points.push_back(point); } } @@ -245,15 +257,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { for (size_t j = 0; j < slow_down_range_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_range_polygons_.at(i).outer().at(j)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_range_polygons_.at( + i).outer().at(j)); marker.points.push_back(point); } if (j + 1 == slow_down_range_polygons_.at(i).outer().size()) { - geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_range_polygons_.at(i).outer().at(0)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_range_polygons_.at( + i).outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_range_polygons_.at(i).outer().at(j + 1)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_range_polygons_.at( + i).outer().at(j + 1)); marker.points.push_back(point); } } @@ -287,15 +305,21 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { for (size_t j = 0; j < slow_down_polygons_.at(i).outer().size(); ++j) { { - geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_polygons_.at(i).outer().at(j)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_polygons_.at( + i).outer().at(j)); marker.points.push_back(point); } if (j + 1 == slow_down_polygons_.at(i).outer().size()) { - geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_polygons_.at(i).outer().at(0)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_polygons_.at( + i).outer().at(0)); marker.points.push_back(point); } else { - geometry_msgs::msg::Point point = autoware_utils::toMsg(slow_down_polygons_.at(i).outer().at(j + 1)); + geometry_msgs::msg::Point point = autoware_utils::toMsg( + slow_down_polygons_.at( + i).outer().at(j + 1)); marker.points.push_back(point); } } diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index ab14ce3e..b98cdeb4 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -46,18 +46,23 @@ using Polygon = bg::model::polygon; using Line = bg::model::linestring; ObstacleStopPlannerNode::ObstacleStopPlannerNode() -: Node("obstacle_stop_planner") -, tf_buffer_(this->get_clock()) -, tf_listener_(tf_buffer_) -, obstacle_pointcloud_(this->get_logger()) -, vehicle_info_(vehicle_info_util::VehicleInfo::create(*this), this->get_node_parameters_interface()) +: Node("obstacle_stop_planner"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + obstacle_pointcloud_(this->get_logger()), + vehicle_info_(vehicle_info_util::VehicleInfo::create(*this), + this->get_node_parameters_interface()) { obstacle_pointcloud_.setVehicleInfo(vehicle_info_); - debug_ptr_ = std::make_shared(this, vehicle_info_.wheel_base_m_ + vehicle_info_.front_overhang_m_); + debug_ptr_ = std::make_shared( + this, + vehicle_info_.wheel_base_m_ + + vehicle_info_.front_overhang_m_); // Initializer acc_controller_ = std::make_unique( - this, vehicle_info_.vehicle_width_m_, vehicle_info_.vehicle_length_m_, vehicle_info_.wheel_base_m_, vehicle_info_.front_overhang_m_); + this, vehicle_info_.vehicle_width_m_, vehicle_info_.vehicle_length_m_, + vehicle_info_.wheel_base_m_, vehicle_info_.front_overhang_m_); // Publishers path_pub_ = @@ -124,7 +129,9 @@ void ObstacleStopPlannerNode::pathCallback( getSelfPose(input_msg->header, tf_buffer_, self_pose); autoware_planning_msgs::msg::Trajectory trim_trajectory; size_t trajectory_trim_index; - trajectory_.trimTrajectoryWithIndexFromSelfPose(base_path, self_pose, trim_trajectory, trajectory_trim_index); + trajectory_.trimTrajectoryWithIndexFromSelfPose( + base_path, self_pose, trim_trajectory, + trajectory_trim_index); /* * decimate trajectory for calculation cost @@ -132,7 +139,8 @@ void ObstacleStopPlannerNode::pathCallback( autoware_planning_msgs::msg::Trajectory decimate_trajectory; std::map decimate_trajectory_index_map; trajectory_.decimateTrajectory( - trim_trajectory, vehicle_info_.step_length_, vehicle_info_, decimate_trajectory, decimate_trajectory_index_map); + trim_trajectory, vehicle_info_.step_length_, vehicle_info_, decimate_trajectory, + decimate_trajectory_index_map); autoware_planning_msgs::msg::Trajectory & trajectory = decimate_trajectory; @@ -171,19 +179,25 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step circle center for vehicle */ - const auto prev_center_pose = vehicle_info_.getVehicleCenterFromBase(trajectory.points.at(i).pose); + const auto prev_center_pose = vehicle_info_.getVehicleCenterFromBase( + trajectory.points.at( + i).pose); Point prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = vehicle_info_.getVehicleCenterFromBase(trajectory.points.at(i + 1).pose); + const auto next_center_pose = vehicle_info_.getVehicleCenterFromBase( + trajectory.points.at( + i + 1).pose); Point next_center_point(next_center_pose.position.x, next_center_pose.position.y); /* * create one step polygon for vehicle */ OneStepPolygon move_vehicle_polygon; move_vehicle_polygon.setVehicleInfo(vehicle_info_); - move_vehicle_polygon.create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + move_vehicle_polygon.create( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_stop_range_); debug_ptr_->pushPolygon( - move_vehicle_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); + move_vehicle_polygon.getPolygon(), trajectory.points.at( + i).pose.position.z, PolygonType::Vehicle); OneStepPolygon move_slow_down_range_polygon; move_slow_down_range_polygon.setVehicleInfo(vehicle_info_); @@ -191,7 +205,8 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step polygon for slow_down range */ - move_slow_down_range_polygon.create(trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, + move_slow_down_range_polygon.create( + trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_slow_down_range_); debug_ptr_->pushPolygon( move_slow_down_range_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, @@ -202,9 +217,16 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - getSlowDownPointcloud(is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point,next_center_point, vehicle_info_.slow_down_search_radius_, move_slow_down_range_polygon.getPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); + getSlowDownPointcloud( + is_slow_down, vehicle_info_.enable_slow_down_, + obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, + vehicle_info_.slow_down_search_radius_, + move_slow_down_range_polygon.getPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); - getCollisionPointcloud(slow_down_pointcloud_ptr, prev_center_point, next_center_point, vehicle_info_.stop_search_radius_, move_vehicle_polygon, trajectory.points.at(i), collision_pointcloud_ptr, is_collision); + getCollisionPointcloud( + slow_down_pointcloud_ptr, prev_center_point, next_center_point, + vehicle_info_.stop_search_radius_, move_vehicle_polygon, trajectory.points.at( + i), collision_pointcloud_ptr, is_collision); if (candidate_slow_down && !is_collision && !is_slow_down) { is_slow_down = true; @@ -248,14 +270,20 @@ void ObstacleStopPlannerNode::pathCallback( * insert stop point */ if (need_to_stop) { - insertStopPoint(decimate_trajectory_index_map.at(decimate_trajectory_collision_index) + trajectory_trim_index, base_path, nearest_collision_point, point_helper, output_msg, stop_reason_diag); + insertStopPoint( + decimate_trajectory_index_map.at( + decimate_trajectory_collision_index) + trajectory_trim_index, base_path, nearest_collision_point, point_helper, output_msg, + stop_reason_diag); } /* * insert slow_down point */ if (is_slow_down) { - insertSlowDownPoint(decimate_trajectory_index_map.at(decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, point_helper, calcSlowDownTargetVel(lateral_deviation), vehicle_info_.slow_down_margin_, output_msg); + insertSlowDownPoint( + decimate_trajectory_index_map.at( + decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, point_helper, calcSlowDownTargetVel( + lateral_deviation), vehicle_info_.slow_down_margin_, output_msg); } path_pub_->publish(output_msg); stop_reason_diag_pub_->publish(stop_reason_diag); @@ -263,7 +291,12 @@ void ObstacleStopPlannerNode::pathCallback( } // collision -void ObstacleStopPlannerNode::getCollisionPointcloud(const pcl::PointCloud::Ptr slow_down_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const OneStepPolygon& one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool& is_collision) +void ObstacleStopPlannerNode::getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, const Point & prev_center_point, + const Point & next_center_point, const double search_radius, + const OneStepPolygon & one_step_polygon, + const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, + pcl::PointCloud::Ptr collision_pointcloud, bool & is_collision) { for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { Point point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); @@ -283,7 +316,13 @@ void ObstacleStopPlannerNode::getCollisionPointcloud(const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point& prev_center_point, const Point& next_center_point, const double search_radius, const autoware_utils::Polygon2d& boost_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool& candidate_slow_down) +void ObstacleStopPlannerNode::getSlowDownPointcloud( + const bool is_slow_down, + const bool enable_slow_down, + const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, + const Point & prev_center_point, const Point & next_center_point, const double search_radius, + const autoware_utils::Polygon2d & boost_polygon, + pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down) { if (!is_slow_down && enable_slow_down) { for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { @@ -304,10 +343,14 @@ void ObstacleStopPlannerNode::getSlowDownPointcloud(const bool is_slow_down, con } } -void ObstacleStopPlannerNode::insertSlowDownPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_slow_down_point, const PointHelper& point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg) +void ObstacleStopPlannerNode::insertSlowDownPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const pcl::PointXYZ & nearest_slow_down_point, const PointHelper & point_helper, + const double slow_down_target_vel, const double slow_down_margin, + autoware_planning_msgs::msg::Trajectory & output_msg) { - for (size_t i = search_start_index; i < base_path.points.size(); ++i) - { + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { Eigen::Vector2d trajectory_vec; { const double yaw = @@ -327,7 +370,8 @@ void ObstacleStopPlannerNode::insertSlowDownPoint(const size_t search_start_inde base_path, current_velocity_ptr_->twist.linear.x); if (slow_down_start_point.index <= output_msg.points.size()) { - const auto slowdown_trajectory_point = point_helper.insertSlowDownStartPoint(slow_down_start_point, base_path, output_msg); + const auto slowdown_trajectory_point = point_helper.insertSlowDownStartPoint( + slow_down_start_point, base_path, output_msg); debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); insertSlowDownVelocity( slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, @@ -339,10 +383,14 @@ void ObstacleStopPlannerNode::insertSlowDownPoint(const size_t search_start_inde } // stop -void ObstacleStopPlannerNode::insertStopPoint(const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory &base_path, const pcl::PointXYZ& nearest_collision_point, const PointHelper& point_helper, autoware_planning_msgs::msg::Trajectory & output_msg, diagnostic_msgs::msg::DiagnosticStatus& stop_reason_diag) +void ObstacleStopPlannerNode::insertStopPoint( + const size_t search_start_index, + const autoware_planning_msgs::msg::Trajectory & base_path, + const pcl::PointXYZ & nearest_collision_point, const PointHelper & point_helper, + autoware_planning_msgs::msg::Trajectory & output_msg, + diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag) { - for (size_t i = search_start_index; i < base_path.points.size(); ++i) - { + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { Eigen::Vector2d trajectory_vec; { const double yaw = @@ -360,7 +408,8 @@ void ObstacleStopPlannerNode::insertStopPoint(const size_t search_start_index, c const auto stop_point = point_helper.searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); if (stop_point.index <= output_msg.points.size()) { - const auto trajectory_point = point_helper.insertStopPoint(stop_point, base_path, output_msg); + const auto trajectory_point = + point_helper.insertStopPoint(stop_point, base_path, output_msg); stop_reason_diag = makeStopReasonDiag("obstacle", trajectory_point.pose); debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); } @@ -374,7 +423,9 @@ void ObstacleStopPlannerNode::externalExpandStopRangeCallback( { vehicle_info_.expand_stop_range_ = input_msg->expand_stop_range; vehicle_info_.stop_search_radius_ = - vehicle_info_.step_length_ + std::hypot(vehicle_info_.vehicle_width_m_ / 2.0 + vehicle_info_.expand_stop_range_, vehicle_info_.vehicle_length_m_ / 2.0); + vehicle_info_.step_length_ + std::hypot( + vehicle_info_.vehicle_width_m_ / 2.0 + vehicle_info_.expand_stop_range_, + vehicle_info_.vehicle_length_m_ / 2.0); } void ObstacleStopPlannerNode::insertSlowDownVelocity( @@ -392,7 +443,10 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( output_path.points.at(j).pose.position.y - output_path.points.at(j + 1).pose.position.y); slow_down_vel = std::max( slow_down_target_vel, - std::sqrt(std::max(slow_down_vel * slow_down_vel - 2 * vehicle_info_.max_deceleration_ * dist, 0.0))); + std::sqrt( + std::max( + slow_down_vel * slow_down_vel - 2 * vehicle_info_.max_deceleration_ * dist, + 0.0))); if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { slow_down_end_trajectory_point = output_path.points.at(j + 1); is_slow_down_end = true; @@ -407,9 +461,10 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) { - return vehicle_info_.min_slow_down_vel_ + (vehicle_info_.max_slow_down_vel_ - vehicle_info_.min_slow_down_vel_) * - std::max(lateral_deviation - vehicle_info_.vehicle_width_m_ / 2, 0.0) / - vehicle_info_.expand_slow_down_range_; + return vehicle_info_.min_slow_down_vel_ + + (vehicle_info_.max_slow_down_vel_ - vehicle_info_.min_slow_down_vel_) * + std::max(lateral_deviation - vehicle_info_.vehicle_width_m_ / 2, 0.0) / + vehicle_info_.expand_slow_down_range_; } void ObstacleStopPlannerNode::dynamicObjectCallback( diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 51a681cc..37f5d3a1 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -60,7 +60,8 @@ bool ObstaclePointCloud::isDataReceived() return obstacle_ros_pointcloud_ptr_ != nullptr ? true : false; } -pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle(const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory) +pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( + const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory) { pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( new pcl::PointCloud); diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 6dbfdb83..f7800582 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -19,7 +19,8 @@ namespace obstacle_stop_planner { bool PointHelper::getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) const + const Eigen::Vector2d & base_point, const double backward_length, + Eigen::Vector2d & output_point) const { Eigen::Vector2d line_vec = line_point2 - line_point1; Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); @@ -94,7 +95,9 @@ StopPoint PointHelper::searchInsertPoint( const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) const { const auto max_dist_stop_point = - createTargetPoint(idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, base_path); + createTargetPoint( + idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, + base_path); const auto min_dist_stop_point = createTargetPoint( idx, vehicle_info_->min_behavior_stop_margin_, trajectory_vec, collision_point_vec, base_path); @@ -182,7 +185,9 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( } slow_down_point.velocity = std::max( - std::sqrt(slow_down_target_vel * slow_down_target_vel + 2 * vehicle_info_->max_deceleration_ * length_sum), + std::sqrt( + slow_down_target_vel * slow_down_target_vel + 2 * vehicle_info_->max_deceleration_ * + length_sum), current_velocity_x); return slow_down_point; } @@ -210,7 +215,8 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertSlowDownStartPoi } autoware_planning_msgs::msg::TrajectoryPoint PointHelper::getExtendTrajectoryPoint( - const double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const + const double extend_distance, + const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const { tf2::Transform map2goal; tf2::fromMsg(goal_point.pose, map2goal); diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 06eedc57..38872f9f 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -17,15 +17,19 @@ namespace obstacle_stop_planner { bool Trajectory::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, + const VehicleInfo & vehicle_info, autoware_planning_msgs::msg::Trajectory & output_trajectory) { std::map index_map; - return decimateTrajectory(input_trajectory, step_length, vehicle_info, output_trajectory, index_map); + return decimateTrajectory( + input_trajectory, step_length, vehicle_info, output_trajectory, + index_map); } bool Trajectory::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const VehicleInfo & vehicle_info, + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, + const VehicleInfo & vehicle_info, autoware_planning_msgs::msg::Trajectory & output_trajectory, std::map & index_map) { @@ -35,7 +39,7 @@ bool Trajectory::decimateTrajectory( const double epsilon = 0.001; Eigen::Vector2d point1, point2; PointHelper point_helper; - point_helper.SetVehicleInfo(vehicle_info); + point_helper.setVehicleInfo(vehicle_info); for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { if (next_length <= trajectory_length_sum + epsilon) { @@ -57,8 +61,10 @@ bool Trajectory::decimateTrajectory( continue; } trajectory_length_sum += std::hypot( - input_trajectory.points.at(i).pose.position.x - input_trajectory.points.at(i + 1).pose.position.x, - input_trajectory.points.at(i).pose.position.y - input_trajectory.points.at(i + 1).pose.position.y); + input_trajectory.points.at(i).pose.position.x - input_trajectory.points.at( + i + 1).pose.position.x, + input_trajectory.points.at(i).pose.position.y - input_trajectory.points.at( + i + 1).pose.position.y); } if (!input_trajectory.points.empty()) { output_trajectory.points.push_back(input_trajectory.points.back()); @@ -116,11 +122,15 @@ bool Trajectory::extendTrajectory( double extend_sum = 0.0; while (extend_sum <= (extend_distance - interpolation_distance)) { - const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( + extend_sum, + goal_point); output_trajectory.points.push_back(extend_trajectory_point); extend_sum += interpolation_distance; } - const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( + extend_distance, + goal_point); output_trajectory.points.push_back(extend_trajectory_point); return true; diff --git a/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp b/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp index ed86242c..9151315a 100644 --- a/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp +++ b/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp @@ -1,3 +1,17 @@ +// Copyright 2021 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 "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" From 7602798441ad64a1a9b51deffbfa70c2d170afb5 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 14:55:08 +0900 Subject: [PATCH 20/62] test passed --- .../include/obstacle_stop_planner/node.hpp | 1 - .../obstacle_point_cloud.hpp | 5 ++--- .../obstacle_stop_planner/one_step_polygon.hpp | 1 + .../include/obstacle_stop_planner/util.hpp | 7 +++++-- obstacle_stop_planner_refine/src/node.cpp | 17 ++++++++++++----- .../src/obstacle_point_cloud.cpp | 1 + .../src/point_helper.cpp | 2 ++ obstacle_stop_planner_refine/src/trajectory.cpp | 2 ++ .../cmake/ament_cmake_auto_gtest-extras.cmake | 3 ++- 9 files changed, 27 insertions(+), 12 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index e4046056..3fe65d72 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -126,7 +126,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node const PointHelper & point_helper, const double slow_down_target_vel, const double slow_down_margin, autoware_planning_msgs::msg::Trajectory & output_msg); - }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index e8ec6098..0f126491 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -16,15 +16,14 @@ #define OBSTACLE_STOP_PLANNER__OBSTACLE_POINT_CLOUD_HPP_ #include - -#include -#include #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" #include "pcl/point_cloud.h" #include "pcl/common/transforms.h" #include "pcl/filters/voxel_grid.h" #include "tf2_ros/transform_listener.h" +#include "rclcpp/logger.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "obstacle_stop_planner/vehicle.hpp" diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 54d3bcb5..44f99967 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -15,6 +15,7 @@ #ifndef OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ #define OBSTACLE_STOP_PLANNER__ONE_STEP_POLYGON_HPP_ +#include #include #include #include "boost/assert.hpp" diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index c81d997b..732e307f 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -14,7 +14,9 @@ #ifndef OBSTACLE_STOP_PLANNER__UTIL_HPP_ #define OBSTACLE_STOP_PLANNER__UTIL_HPP_ +#include #include +#include #include "opencv2/core/core.hpp" #include "geometry_msgs/msg/point.hpp" @@ -114,8 +116,9 @@ inline Polygon getPolygon( -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); // rotate polygon - bg::strategy::transform::rotate_transformer rotate(-obj_rpy.z); // original:clockwise - // rotation + // original:clockwise + bg::strategy::transform::rotate_transformer rotate(-obj_rpy.z); + // rotation Polygon rotate_obj_poly; bg::transform(obj_poly, rotate_obj_poly, rotate); // translate polygon diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index b98cdeb4..a5148d0b 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -271,8 +271,11 @@ void ObstacleStopPlannerNode::pathCallback( */ if (need_to_stop) { insertStopPoint( - decimate_trajectory_index_map.at( - decimate_trajectory_collision_index) + trajectory_trim_index, base_path, nearest_collision_point, point_helper, output_msg, + decimate_trajectory_index_map.at(decimate_trajectory_collision_index) + trajectory_trim_index, + base_path, + nearest_collision_point, + point_helper, + output_msg, stop_reason_diag); } @@ -281,9 +284,13 @@ void ObstacleStopPlannerNode::pathCallback( */ if (is_slow_down) { insertSlowDownPoint( - decimate_trajectory_index_map.at( - decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, point_helper, calcSlowDownTargetVel( - lateral_deviation), vehicle_info_.slow_down_margin_, output_msg); + decimate_trajectory_index_map.at(decimate_trajectory_slow_down_index), + base_path, + nearest_slow_down_point, + point_helper, + calcSlowDownTargetVel(lateral_deviation), + vehicle_info_.slow_down_margin_, + output_msg); } path_pub_->publish(output_msg); stop_reason_diag_pub_->publish(stop_reason_diag); diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 37f5d3a1..1554b65d 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "obstacle_stop_planner/obstacle_point_cloud.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/utils.h" diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index f7800582..9d0605e2 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include "obstacle_stop_planner/point_helper.hpp" #include "pcl_conversions/pcl_conversions.h" diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 38872f9f..aebcc9a3 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include "obstacle_stop_planner/trajectory.hpp" namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake b/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake index fa1070bc..093ca5ce 100644 --- a/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake +++ b/obstacle_stop_planner_refine/test/cmake/ament_cmake_auto_gtest-extras.cmake @@ -36,7 +36,8 @@ # :type NO_TARGET_LINK_LIBRARIES: option # :param ARGN: the list of source files # :type ARGN: list of strings -# :param RUNNER: the path to the test runner script (default: see ament_add_test). +# :param RUNNER: the path to the test runner script +# (default: see ament_add_test). # :type RUNNER: string # :param TIMEOUT: the test timeout in seconds, # default defined by ``ament_add_test()`` From 863d31c23a36d51baad3cdc34f85211b6464db3d Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 15:55:04 +0900 Subject: [PATCH 21/62] use const reference for non-primitive types --- .../obstacle_stop_planner/adaptive_cruise_control.hpp | 4 ++-- .../include/obstacle_stop_planner/obstacle_point_cloud.hpp | 2 +- .../include/obstacle_stop_planner/one_step_polygon.hpp | 6 +++--- .../include/obstacle_stop_planner/point_helper.hpp | 4 ++-- .../include/obstacle_stop_planner/trajectory.hpp | 4 ++-- .../include/obstacle_stop_planner/util.hpp | 2 +- .../src/adaptive_cruise_control.cpp | 4 ++-- obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp | 2 +- obstacle_stop_planner_refine/src/trajectory.cpp | 4 ++-- 9 files changed, 16 insertions(+), 16 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 0a65e469..3cb22a9e 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -172,7 +172,7 @@ class AdaptiveCruiseController }; Param param_; - double getMedianVel(const std::vector vel_que); + double getMedianVel(const std::vector & vel_que); double lowpass_filter(const double current_value, const double prev_value, const double gain); double calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, @@ -198,7 +198,7 @@ class AdaptiveCruiseController const double current_vel, const double current_dist, const double obj_vel); void insertMaxVelocityToPath( - const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + const geometry_msgs::msg::Pose & self_pose, const double current_vel, const double target_vel, const double dist_to_collision_point, autoware_planning_msgs::msg::Trajectory & output_trajectory); void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 0f126491..d7177dee 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -37,7 +37,7 @@ class ObstaclePointCloud void setPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void setSearchRadius(const double value); - void setVehicleInfo(const VehicleInfo vehicle_info); + void setVehicleInfo(const VehicleInfo & vehicle_info); bool isDataReceived(); pcl::PointCloud::Ptr searchCandidateObstacle( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 44f99967..ec527baf 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -36,10 +36,10 @@ class OneStepPolygon { public: void create( - const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, const double expand_width); autoware_utils::Polygon2d getPolygon() const {return polygon_;} - void setVehicleInfo(VehicleInfo vehicle_info) + void setVehicleInfo(VehicleInfo & vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } @@ -50,7 +50,7 @@ class OneStepPolygon }; inline void OneStepPolygon::create( - const geometry_msgs::msg::Pose base_step_pose, const geometry_msgs::msg::Pose next_step_pose, + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, const double expand_width) { autoware_utils::Polygon2d one_step_move_vehicle_corner_points; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index 5a122f0d..d1181e3c 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -49,11 +49,11 @@ struct SlowDownPoint class PointHelper { public: - void setVehicleInfo(const VehicleInfo vehicle_info) + void setVehicleInfo(const VehicleInfo & vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } - void setVehicleInfo(std::shared_ptr vehicle_info) {vehicle_info_ = vehicle_info;} + void setVehicleInfo(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index 2c83cb57..472d2bd7 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -40,11 +40,11 @@ class Trajectory std::map & index_map); bool trimTrajectoryFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, + const geometry_msgs::msg::Pose & self_pose, autoware_planning_msgs::msg::Trajectory & output_trajectory); bool trimTrajectoryWithIndexFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, + const geometry_msgs::msg::Pose & self_pose, autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index); bool extendTrajectory( diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 732e307f..88e6c8bc 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -68,7 +68,7 @@ inline diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( return stop_reason_diag; } -inline cv::Point2d calcCentroid(const std::vector pointcloud) +inline cv::Point2d calcCentroid(const std::vector & pointcloud) { cv::Point2d centroid; centroid.x = 0; diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 1c145ad5..28d3776a 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -482,7 +482,7 @@ double AdaptiveCruiseController::calcTargetVelocityByPID( } void AdaptiveCruiseController::insertMaxVelocityToPath( - const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, + const geometry_msgs::msg::Pose & self_pose, const double current_vel, const double target_vel, const double dist_to_collision_point, autoware_planning_msgs::msg::Trajectory & output_trajectory) { // plus distance from self to next nearest point @@ -560,7 +560,7 @@ void AdaptiveCruiseController::registerQueToVelocity( } double AdaptiveCruiseController::getMedianVel( - const std::vector vel_que) + const std::vector& vel_que) { if (vel_que.size() == 0) { RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 1554b65d..c6eef548 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -51,7 +51,7 @@ void ObstaclePointCloud::setSearchRadius(const double value) search_radius_ = value; } -void ObstaclePointCloud::setVehicleInfo(const VehicleInfo vehicle_info) +void ObstaclePointCloud::setVehicleInfo(const VehicleInfo & vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index aebcc9a3..050266c0 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -78,7 +78,7 @@ bool Trajectory::decimateTrajectory( bool Trajectory::trimTrajectoryWithIndexFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, + const geometry_msgs::msg::Pose & self_pose, autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index) { double min_distance = 0.0; @@ -104,7 +104,7 @@ bool Trajectory::trimTrajectoryWithIndexFromSelfPose( bool Trajectory::trimTrajectoryFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose self_pose, + const geometry_msgs::msg::Pose & self_pose, autoware_planning_msgs::msg::Trajectory & output_trajectory) { size_t index; From 9579da8a3bc9e56970308f7d1be4b8c9b633b1fa Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 16:33:38 +0900 Subject: [PATCH 22/62] remove setter function --- .../obstacle_point_cloud.hpp | 10 ++++----- .../one_step_polygon.hpp | 10 +++++---- .../obstacle_stop_planner/point_helper.hpp | 4 ++-- .../obstacle_stop_planner/trajectory.hpp | 2 +- obstacle_stop_planner_refine/src/node.cpp | 19 +++++----------- .../src/obstacle_point_cloud.cpp | 22 +++++++------------ .../src/trajectory.cpp | 11 +++++----- 7 files changed, 32 insertions(+), 46 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index d7177dee..982a0e28 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -35,25 +35,23 @@ class ObstaclePointCloud public: explicit ObstaclePointCloud(rclcpp::Logger logger); - void setPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void setSearchRadius(const double value); - void setVehicleInfo(const VehicleInfo & vehicle_info); + void updatePointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); bool isDataReceived(); pcl::PointCloud::Ptr searchCandidateObstacle( const tf2_ros::Buffer & tf_buffer, - const autoware_planning_msgs::msg::Trajectory & trajectory); + const autoware_planning_msgs::msg::Trajectory & trajectory, + const VehicleInfo & vehicle_info); private: bool searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, + const VehicleInfo & vehicle_info, pcl::PointCloud::Ptr output_pointcloud_ptr); sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; rclcpp::Logger logger_; - double search_radius_; - std::shared_ptr vehicle_info_; }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index ec527baf..0b654012 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -35,14 +35,16 @@ namespace obstacle_stop_planner class OneStepPolygon { public: + OneStepPolygon(const VehicleInfo & vehicle_info) + { + vehicle_info_ = std::make_shared(vehicle_info); + } + OneStepPolygon(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} + void create( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, const double expand_width); autoware_utils::Polygon2d getPolygon() const {return polygon_;} - void setVehicleInfo(VehicleInfo & vehicle_info) - { - vehicle_info_ = std::make_shared(vehicle_info); - } private: autoware_utils::Polygon2d polygon_; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index d1181e3c..dfc7f98a 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -49,11 +49,11 @@ struct SlowDownPoint class PointHelper { public: - void setVehicleInfo(const VehicleInfo & vehicle_info) + PointHelper(const VehicleInfo & vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } - void setVehicleInfo(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} + PointHelper(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index 472d2bd7..f760eddd 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -49,7 +49,7 @@ class Trajectory size_t & index); bool extendTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const double extend_distance, + const VehicleInfo & vehicle_info, autoware_planning_msgs::msg::Trajectory & output_trajectory); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index a5148d0b..fef0a776 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -53,7 +53,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() vehicle_info_(vehicle_info_util::VehicleInfo::create(*this), this->get_node_parameters_interface()) { - obstacle_pointcloud_.setVehicleInfo(vehicle_info_); debug_ptr_ = std::make_shared( this, vehicle_info_.wheel_base_m_ + @@ -93,7 +92,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() void ObstacleStopPlannerNode::obstaclePointcloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { - obstacle_pointcloud_.setPointCloud(input_msg); + obstacle_pointcloud_.updatePointCloud(input_msg); } void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) @@ -116,7 +115,7 @@ void ObstacleStopPlannerNode::pathCallback( * extend trajectory to consider obstacles after the goal */ autoware_planning_msgs::msg::Trajectory extended_trajectory; - trajectory_.extendTrajectory(*input_msg, vehicle_info_.extend_distance_, extended_trajectory); + trajectory_.extendTrajectory(*input_msg, vehicle_info_, extended_trajectory); const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; @@ -151,9 +150,7 @@ void ObstacleStopPlannerNode::pathCallback( new pcl::PointCloud); // search obstacle candidate pointcloud to reduce calculation cost - const double search_radius = vehicle_info_.getSearchRadius(); - obstacle_pointcloud_.setSearchRadius(search_radius); - obstacle_pointcloud_.searchCandidateObstacle(tf_buffer_, trajectory); + obstacle_pointcloud_.searchCandidateObstacle(tf_buffer_, trajectory, vehicle_info_); /* * check collision, slow_down @@ -171,9 +168,7 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointXYZ lateral_nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; - - PointHelper point_helper; - point_helper.setVehicleInfo(vehicle_info_); + PointHelper point_helper {vehicle_info_}; for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { /* @@ -190,8 +185,7 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step polygon for vehicle */ - OneStepPolygon move_vehicle_polygon; - move_vehicle_polygon.setVehicleInfo(vehicle_info_); + OneStepPolygon move_vehicle_polygon{vehicle_info_}; move_vehicle_polygon.create( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, vehicle_info_.expand_stop_range_); @@ -199,8 +193,7 @@ void ObstacleStopPlannerNode::pathCallback( move_vehicle_polygon.getPolygon(), trajectory.points.at( i).pose.position.z, PolygonType::Vehicle); - OneStepPolygon move_slow_down_range_polygon; - move_slow_down_range_polygon.setVehicleInfo(vehicle_info_); + OneStepPolygon move_slow_down_range_polygon{vehicle_info_}; if (vehicle_info_.enable_slow_down_) { /* * create one step polygon for slow_down range diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index c6eef548..3f603cde 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -25,7 +25,7 @@ ObstaclePointCloud::ObstaclePointCloud(rclcpp::Logger logger) { } -void ObstaclePointCloud::setPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +void ObstaclePointCloud::updatePointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { obstacle_ros_pointcloud_ptr_ = std::make_shared(); pcl::VoxelGrid filter; @@ -46,23 +46,15 @@ void ObstaclePointCloud::setPointCloud(const sensor_msgs::msg::PointCloud2::Cons obstacle_ros_pointcloud_ptr_->header = msg->header; } -void ObstaclePointCloud::setSearchRadius(const double value) -{ - search_radius_ = value; -} - -void ObstaclePointCloud::setVehicleInfo(const VehicleInfo & vehicle_info) -{ - vehicle_info_ = std::make_shared(vehicle_info); -} - bool ObstaclePointCloud::isDataReceived() { return obstacle_ros_pointcloud_ptr_ != nullptr ? true : false; } pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( - const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory) + const tf2_ros::Buffer & tf_buffer, + const autoware_planning_msgs::msg::Trajectory & trajectory, + const VehicleInfo & vehicle_info) { pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( new pcl::PointCloud); @@ -96,6 +88,7 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( // search obstacle candidate pointcloud to reduce calculation cost searchPointcloudNearTrajectory( trajectory, transformed_obstacle_pointcloud_ptr, + vehicle_info, obstacle_candidate_pointcloud_ptr); obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; return obstacle_candidate_pointcloud_ptr; @@ -104,11 +97,12 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( bool ObstaclePointCloud::searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, + const VehicleInfo & vehicle_info, pcl::PointCloud::Ptr output_pointcloud_ptr) { - const double squared_radius = search_radius_ * search_radius_; + const double squared_radius = vehicle_info.getSearchRadius() * vehicle_info.getSearchRadius(); for (const auto & trajectory_point : trajectory.points) { - const auto center_pose = vehicle_info_->getVehicleCenterFromBase(trajectory_point.pose); + const auto center_pose = vehicle_info.getVehicleCenterFromBase(trajectory_point.pose); for (const auto & point : input_pointcloud_ptr->points) { const double x = center_pose.position.x - point.x; const double y = center_pose.position.y - point.y; diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 050266c0..b6d5e822 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -40,8 +40,7 @@ bool Trajectory::decimateTrajectory( double next_length = 0.0; const double epsilon = 0.001; Eigen::Vector2d point1, point2; - PointHelper point_helper; - point_helper.setVehicleInfo(vehicle_info); + PointHelper point_helper {vehicle_info}; for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { if (next_length <= trajectory_length_sum + epsilon) { @@ -114,16 +113,16 @@ bool Trajectory::trimTrajectoryFromSelfPose( bool Trajectory::extendTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const double extend_distance, + const VehicleInfo & vehicle_info, autoware_planning_msgs::msg::Trajectory & output_trajectory) { output_trajectory = input_trajectory; const auto goal_point = input_trajectory.points.back(); double interpolation_distance = 0.1; - PointHelper point_helper; + PointHelper point_helper {vehicle_info}; double extend_sum = 0.0; - while (extend_sum <= (extend_distance - interpolation_distance)) { + while (extend_sum <= (vehicle_info.extend_distance_ - interpolation_distance)) { const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( extend_sum, goal_point); @@ -131,7 +130,7 @@ bool Trajectory::extendTrajectory( extend_sum += interpolation_distance; } const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( - extend_distance, + vehicle_info.extend_distance_, goal_point); output_trajectory.points.push_back(extend_trajectory_point); From 21424967c93e4e68e51b6067ee90d52e24708409 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 16:57:58 +0900 Subject: [PATCH 23/62] modify one_step_polygon class --- .../one_step_polygon.hpp | 29 ++++--------------- .../src/adaptive_cruise_control.cpp | 2 +- obstacle_stop_planner_refine/src/node.cpp | 28 +++++++++--------- 3 files changed, 21 insertions(+), 38 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 0b654012..c26a12a5 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -32,31 +32,12 @@ namespace obstacle_stop_planner { -class OneStepPolygon -{ -public: - OneStepPolygon(const VehicleInfo & vehicle_info) - { - vehicle_info_ = std::make_shared(vehicle_info); - } - OneStepPolygon(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} - - void create( - const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const double expand_width); - autoware_utils::Polygon2d getPolygon() const {return polygon_;} - -private: - autoware_utils::Polygon2d polygon_; - std::shared_ptr vehicle_info_; -}; - -inline void OneStepPolygon::create( +autoware_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const double expand_width) + const double expand_width, const VehicleInfo & vehicle_info) { autoware_utils::Polygon2d one_step_move_vehicle_corner_points; - const auto footprint = createVehicleFootprint(*vehicle_info_, 0.0, expand_width); + const auto footprint = createVehicleFootprint(vehicle_info, 0.0, expand_width); // start step { @@ -86,7 +67,9 @@ inline void OneStepPolygon::create( } } - bg::convex_hull(one_step_move_vehicle_corner_points, polygon_); + autoware_utils::Polygon2d polygon; + bg::convex_hull(one_step_move_vehicle_corner_points, polygon); + return polygon; } } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 28d3776a..08c13a4d 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -560,7 +560,7 @@ void AdaptiveCruiseController::registerQueToVelocity( } double AdaptiveCruiseController::getMedianVel( - const std::vector& vel_que) + const std::vector & vel_que) { if (vel_que.size() == 0) { RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index fef0a776..0b205226 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -185,24 +185,24 @@ void ObstacleStopPlannerNode::pathCallback( /* * create one step polygon for vehicle */ - OneStepPolygon move_vehicle_polygon{vehicle_info_}; - move_vehicle_polygon.create( + const auto move_vehicle_polygon = createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - vehicle_info_.expand_stop_range_); + vehicle_info_.expand_stop_range_, vehicle_info_); debug_ptr_->pushPolygon( - move_vehicle_polygon.getPolygon(), trajectory.points.at( - i).pose.position.z, PolygonType::Vehicle); + move_vehicle_polygon, + trajectory.points.at(i).pose.position.z, + PolygonType::Vehicle); - OneStepPolygon move_slow_down_range_polygon{vehicle_info_}; + autoware_utils::Polygon2d move_slow_down_range_polygon; if (vehicle_info_.enable_slow_down_) { /* * create one step polygon for slow_down range */ - move_slow_down_range_polygon.create( + move_slow_down_range_polygon = createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - vehicle_info_.expand_slow_down_range_); + vehicle_info_.expand_slow_down_range_, vehicle_info_); debug_ptr_->pushPolygon( - move_slow_down_range_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, + move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); } @@ -214,7 +214,7 @@ void ObstacleStopPlannerNode::pathCallback( is_slow_down, vehicle_info_.enable_slow_down_, obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, vehicle_info_.slow_down_search_radius_, - move_slow_down_range_polygon.getPolygon(), slow_down_pointcloud_ptr, candidate_slow_down); + move_slow_down_range_polygon, slow_down_pointcloud_ptr, candidate_slow_down); getCollisionPointcloud( slow_down_pointcloud_ptr, prev_center_point, next_center_point, @@ -225,7 +225,7 @@ void ObstacleStopPlannerNode::pathCallback( is_slow_down = true; decimate_trajectory_slow_down_index = i; debug_ptr_->pushPolygon( - move_slow_down_range_polygon.getPolygon(), trajectory.points.at(i).pose.position.z, + move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); point_helper.getNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_slow_down_point, @@ -294,7 +294,7 @@ void ObstacleStopPlannerNode::pathCallback( void ObstacleStopPlannerNode::getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, const Point & prev_center_point, const Point & next_center_point, const double search_radius, - const OneStepPolygon & one_step_polygon, + const autoware_utils::Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool & is_collision) { @@ -304,11 +304,11 @@ void ObstacleStopPlannerNode::getCollisionPointcloud( bg::distance(prev_center_point, point) < search_radius || bg::distance(next_center_point, point) < search_radius) { - if (bg::within(point, one_step_polygon.getPolygon())) { + if (bg::within(point, one_step_polygon)) { collision_pointcloud->push_back(slow_down_pointcloud->at(j)); is_collision = true; debug_ptr_->pushPolygon( - one_step_polygon.getPolygon(), trajectory_point.pose.position.z, + one_step_polygon, trajectory_point.pose.position.z, PolygonType::Collision); } } From 2acc8076dbfaa004e9fb632d079b203799aa4144 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 16:59:10 +0900 Subject: [PATCH 24/62] apply colcon test --- .../include/obstacle_stop_planner/node.hpp | 2 +- .../include/obstacle_stop_planner/one_step_polygon.hpp | 3 +-- .../include/obstacle_stop_planner/point_helper.hpp | 4 ++-- .../obstacle_stop_planner/util/create_vehicle_footprint.hpp | 3 +++ 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 3fe65d72..929f50ab 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -108,7 +108,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node void getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, const Point & prev_center_point, const Point & next_center_point, - const double search_radius, const OneStepPolygon & one_step_polygon, + const double search_radius, const autoware_utils::Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool & is_collision); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index c26a12a5..755695f4 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -31,8 +31,7 @@ namespace obstacle_stop_planner { - -autoware_utils::Polygon2d createOneStepPolygon( +inline autoware_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, const double expand_width, const VehicleInfo & vehicle_info) { diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index dfc7f98a..b61ed508 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -49,11 +49,11 @@ struct SlowDownPoint class PointHelper { public: - PointHelper(const VehicleInfo & vehicle_info) + explicit PointHelper(const VehicleInfo & vehicle_info) { vehicle_info_ = std::make_shared(vehicle_info); } - PointHelper(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} + explicit PointHelper(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp index 3297706b..634b2565 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp @@ -18,6 +18,8 @@ #include #include +namespace obstacle_stop_planner +{ inline autoware_utils::LinearRing2d createVehicleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const double top_margin = 0.0, const double side_margin = 0.0) @@ -41,5 +43,6 @@ inline autoware_utils::LinearRing2d createVehicleFootprint( return footprint; } +} // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ From c34396754b5dfe9e95b185a0388d12986ff0f095 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 17:42:24 +0900 Subject: [PATCH 25/62] replace boost type to autoware_util --- .../include/obstacle_stop_planner/node.hpp | 7 ++- .../one_step_polygon.hpp | 21 ++++---- .../include/obstacle_stop_planner/util.hpp | 32 ++++++------- .../src/adaptive_cruise_control.cpp | 8 ++-- obstacle_stop_planner_refine/src/node.cpp | 48 ++++++++++--------- 5 files changed, 61 insertions(+), 55 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 929f50ab..002cfd22 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -102,12 +102,15 @@ class ObstacleStopPlannerNode : public rclcpp::Node void getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, - const Point & prev_center_point, const Point & next_center_point, const double search_radius, + const autoware_utils::Point2d & prev_center_point, + const autoware_utils::Point2d & next_center_point, + const double search_radius, const autoware_utils::Polygon2d & one_step_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down); void getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, - const Point & prev_center_point, const Point & next_center_point, + const autoware_utils::Point2d & prev_center_point, + const autoware_utils::Point2d & next_center_point, const double search_radius, const autoware_utils::Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 755695f4..d4b23a60 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -26,7 +26,6 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/vehicle.hpp" -#include "autoware_utils/autoware_utils.hpp" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" namespace obstacle_stop_planner @@ -42,32 +41,34 @@ inline autoware_utils::Polygon2d createOneStepPolygon( { double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); - bg::strategy::transform::rotate_transformer rotate(yaw); + boost::geometry::strategy::transform::rotate_transformer< + boost::geometry::radian, double, 2, 2> rotate(yaw); autoware_utils::LinearRing2d transformed_footprint; - bg::transform(footprint, transformed_footprint, rotate); + boost::geometry::transform(footprint, transformed_footprint, rotate); - bg::strategy::transform::translate_transformer translate( + boost::geometry::strategy::transform::translate_transformer translate( base_step_pose.position.x, base_step_pose.position.y); - bg::transform(transformed_footprint, transformed_footprint, translate); + boost::geometry::transform(transformed_footprint, transformed_footprint, translate); one_step_move_vehicle_corner_points.outer() = transformed_footprint; } // next step { double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); - bg::strategy::transform::rotate_transformer rotate(yaw); + boost::geometry::strategy::transform::rotate_transformer< + boost::geometry::radian, double, 2, 2> rotate(yaw); autoware_utils::LinearRing2d transformed_footprint; - bg::transform(footprint, transformed_footprint, rotate); + boost::geometry::transform(footprint, transformed_footprint, rotate); - bg::strategy::transform::translate_transformer translate( + boost::geometry::strategy::transform::translate_transformer translate( base_step_pose.position.x, base_step_pose.position.y); - bg::transform(transformed_footprint, transformed_footprint, translate); + boost::geometry::transform(transformed_footprint, transformed_footprint, translate); for (const auto & item : transformed_footprint) { one_step_move_vehicle_corner_points.outer().emplace_back(item); } } autoware_utils::Polygon2d polygon; - bg::convex_hull(one_step_move_vehicle_corner_points, polygon); + boost::geometry::convex_hull(one_step_move_vehicle_corner_points, polygon); return polygon; } diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 88e6c8bc..4ad3383d 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -26,11 +26,7 @@ #include "tf2/utils.h" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" - -namespace bg = boost::geometry; -using Point = bg::model::d2::point_xy; -using Polygon = bg::model::polygon; // clockwise = false -using Line = bg::model::linestring; +#include "autoware_utils/autoware_utils.hpp" namespace { @@ -82,9 +78,9 @@ inline cv::Point2d calcCentroid(const std::vector & pointcloud) return centroid; } -inline Point convertPointRosToBoost(const geometry_msgs::msg::Point & point) +inline autoware_utils::Point2d convertPointRosToBoost(const geometry_msgs::msg::Point & point) { - const Point point2d(point.x, point.y); + const autoware_utils::Point2d point2d(point.x, point.y); return point2d; } @@ -101,31 +97,33 @@ inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quatern return rpy; } -inline Polygon getPolygon( +inline autoware_utils::Polygon2d getPolygon( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) { - Polygon obj_poly; + autoware_utils::Polygon2d obj_poly; geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); double l = size.x * std::cos(obj_rpy.y) + l_margin; double w = size.y * std::cos(obj_rpy.x) + w_margin; double co = center_offset; - bg::exterior_ring(obj_poly) = - boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( + boost::geometry::exterior_ring(obj_poly) = + boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); // rotate polygon // original:clockwise - bg::strategy::transform::rotate_transformer rotate(-obj_rpy.z); + boost::geometry::strategy::transform::rotate_transformer< + boost::geometry::radian, double, 2, 2> rotate(-obj_rpy.z); // rotation - Polygon rotate_obj_poly; - bg::transform(obj_poly, rotate_obj_poly, rotate); + autoware_utils::Polygon2d rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); // translate polygon - bg::strategy::transform::translate_transformer translate(pose.position.x, + boost::geometry::strategy::transform::translate_transformer translate( + pose.position.x, pose.position.y); - Polygon translate_obj_poly; - bg::transform(rotate_obj_poly, translate_obj_poly, translate); + autoware_utils::Polygon2d translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); return translate_obj_poly; } diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 08c13a4d..545e6cc6 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -198,10 +198,10 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( self_size.x = vehicle_length_; self_size.y = vehicle_width_; double self_offset = (wheel_base_ + front_overhang_) - vehicle_length_ / 2.0; - Polygon self_poly = getPolygon(self_pose, self_size, self_offset); + const auto self_poly = getPolygon(self_pose, self_size, self_offset); // get nearest point - Point nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); + autoware_utils::Point2d nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); if (nearest_point_idx <= 2) { // if too nearest collision point, return direct distance @@ -263,9 +263,9 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( bool get_obj = false; double obj_vel; double obj_yaw; - const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); + const auto collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); for (const auto & obj : object_ptr->objects) { - const Polygon obj_poly = getPolygon( + const auto obj_poly = getPolygon( obj.state.pose_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 0b205226..88aedc6a 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -40,11 +40,6 @@ namespace obstacle_stop_planner { -namespace bg = boost::geometry; -using Point = bg::model::d2::point_xy; -using Polygon = bg::model::polygon; -using Line = bg::model::linestring; - ObstacleStopPlannerNode::ObstacleStopPlannerNode() : Node("obstacle_stop_planner"), tf_buffer_(this->get_clock()), @@ -177,11 +172,14 @@ void ObstacleStopPlannerNode::pathCallback( const auto prev_center_pose = vehicle_info_.getVehicleCenterFromBase( trajectory.points.at( i).pose); - Point prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + autoware_utils::Point2d prev_center_point( + prev_center_pose.position.x, + prev_center_pose.position.y); const auto next_center_pose = vehicle_info_.getVehicleCenterFromBase( - trajectory.points.at( - i + 1).pose); - Point next_center_point(next_center_pose.position.x, next_center_pose.position.y); + trajectory.points.at(i + 1).pose); + autoware_utils::Point2d next_center_point( + next_center_pose.position.x, + next_center_pose.position.y); /* * create one step polygon for vehicle */ @@ -292,19 +290,22 @@ void ObstacleStopPlannerNode::pathCallback( // collision void ObstacleStopPlannerNode::getCollisionPointcloud( - const pcl::PointCloud::Ptr slow_down_pointcloud, const Point & prev_center_point, - const Point & next_center_point, const double search_radius, + const pcl::PointCloud::Ptr slow_down_pointcloud, + const autoware_utils::Point2d & prev_center_point, + const autoware_utils::Point2d & next_center_point, + const double search_radius, const autoware_utils::Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, - pcl::PointCloud::Ptr collision_pointcloud, bool & is_collision) + pcl::PointCloud::Ptr collision_pointcloud, + bool & is_collision) { for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { - Point point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); + autoware_utils::Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); if ( - bg::distance(prev_center_point, point) < search_radius || - bg::distance(next_center_point, point) < search_radius) + boost::geometry::distance(prev_center_point, point) < search_radius || + boost::geometry::distance(next_center_point, point) < search_radius) { - if (bg::within(point, one_step_polygon)) { + if (boost::geometry::within(point, one_step_polygon)) { collision_pointcloud->push_back(slow_down_pointcloud->at(j)); is_collision = true; debug_ptr_->pushPolygon( @@ -320,19 +321,22 @@ void ObstacleStopPlannerNode::getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, - const Point & prev_center_point, const Point & next_center_point, const double search_radius, + const autoware_utils::Point2d & prev_center_point, + const autoware_utils::Point2d & next_center_point, + const double search_radius, const autoware_utils::Polygon2d & boost_polygon, - pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down) + pcl::PointCloud::Ptr slow_down_pointcloud, + bool & candidate_slow_down) { if (!is_slow_down && enable_slow_down) { for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { - Point point( + autoware_utils::Point2d point( obstacle_candidate_pointcloud->at(j).x, obstacle_candidate_pointcloud->at(j).y); if ( - bg::distance(prev_center_point, point) < search_radius || - bg::distance(next_center_point, point) < search_radius) + boost::geometry::distance(prev_center_point, point) < search_radius || + boost::geometry::distance(next_center_point, point) < search_radius) { - if (bg::within(point, boost_polygon)) { + if (boost::geometry::within(point, boost_polygon)) { slow_down_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); candidate_slow_down = true; } From 4c2c1e6da2071dd77d0c0592a2414575a046c7f5 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Thu, 4 Mar 2021 18:31:30 +0900 Subject: [PATCH 26/62] rename function --- .../obstacle_stop_planner/one_step_polygon.hpp | 4 ++-- .../include/obstacle_stop_planner/util.hpp | 8 +------- .../include/obstacle_stop_planner/vehicle.hpp | 2 +- .../src/adaptive_cruise_control.cpp | 12 ++++++------ obstacle_stop_planner_refine/src/node.cpp | 4 ++-- obstacle_stop_planner_refine/src/point_helper.cpp | 6 +++--- 6 files changed, 15 insertions(+), 21 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index d4b23a60..872b67ea 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -39,7 +39,7 @@ inline autoware_utils::Polygon2d createOneStepPolygon( // start step { - double yaw = getYawFromGeometryMsgsQuaternion(base_step_pose.orientation); + double yaw = getYawFromQuaternion(base_step_pose.orientation); boost::geometry::strategy::transform::rotate_transformer< boost::geometry::radian, double, 2, 2> rotate(yaw); @@ -53,7 +53,7 @@ inline autoware_utils::Polygon2d createOneStepPolygon( } // next step { - double yaw = getYawFromGeometryMsgsQuaternion(next_step_pose.orientation); + double yaw = getYawFromQuaternion(next_step_pose.orientation); boost::geometry::strategy::transform::rotate_transformer< boost::geometry::radian, double, 2, 2> rotate(yaw); autoware_utils::LinearRing2d transformed_footprint; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 4ad3383d..8531fb59 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -30,7 +30,7 @@ namespace { -inline double getYawFromGeometryMsgsQuaternion(const geometry_msgs::msg::Quaternion & quat) +inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); double roll, pitch, yaw; @@ -78,12 +78,6 @@ inline cv::Point2d calcCentroid(const std::vector & pointcloud) return centroid; } -inline autoware_utils::Point2d convertPointRosToBoost(const geometry_msgs::msg::Point & point) -{ - const autoware_utils::Point2d point2d(point.x, point.y); - return point2d; -} - inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) { tf2::Quaternion quat(q.x, q.y, q.z, q.w); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp index 7717b7c1..4927ad43 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp @@ -82,7 +82,7 @@ class VehicleInfo : public vehicle_info_util::VehicleInfo const geometry_msgs::msg::Pose & base_pose) const { geometry_msgs::msg::Pose center_pose; - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); + const double yaw = getYawFromQuaternion(base_pose.orientation); center_pose.position.x = base_pose.position.x + (vehicle_length_m_ / 2.0 - rear_overhang_m_) * std::cos(yaw); center_pose.position.y = diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 545e6cc6..8ae8974e 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -214,8 +214,8 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( double dist_to_point = 0; // get distance from self to next nearest point dist_to_point += boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(trajectory.points.at(1).pose.position)); + autoware_utils::fromMsg(self_pose.position).to_2d(), + autoware_utils::fromMsg(trajectory.points.at(1).pose.position).to_2d()); // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx for (int i = 1; i < nearest_point_idx - 1; i++) { @@ -226,7 +226,7 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( // add distance from nearest_collision_point to prev point of nearest_point_idx dist_to_point += boost::geometry::distance( nearest_point2d, - convertPointRosToBoost(trajectory.points.at(nearest_point_idx - 1).pose.position)); + autoware_utils::fromMsg(trajectory.points.at(nearest_point_idx - 1).pose.position).to_2d()); // subtract base_link to front dist_to_point -= param_.min_behavior_stop_margin; @@ -263,7 +263,7 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( bool get_obj = false; double obj_vel; double obj_yaw; - const auto collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); + const auto collision_point_2d = autoware_utils::fromMsg(nearest_collision_p_ros).to_2d(); for (const auto & obj : object_ptr->objects) { const auto obj_poly = getPolygon( obj.state.pose_covariance.pose, obj.shape.dimensions, 0.0, @@ -490,8 +490,8 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( double dist_to_first_point = 0.0; if (output_trajectory.points.size() > 1) { dist_to_first_point = boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(output_trajectory.points.at(1).pose.position)); + autoware_utils::fromMsg(self_pose.position).to_2d(), + autoware_utils::fromMsg(output_trajectory.points.at(1).pose.position).to_2d()); } dist += dist_to_first_point; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 88aedc6a..6c355a8d 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -358,7 +358,7 @@ void ObstacleStopPlannerNode::insertSlowDownPoint( Eigen::Vector2d trajectory_vec; { const double yaw = - getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); + getYawFromQuaternion(base_path.points.at(i).pose.orientation); trajectory_vec << std::cos(yaw), std::sin(yaw); } Eigen::Vector2d slow_down_point_vec; @@ -398,7 +398,7 @@ void ObstacleStopPlannerNode::insertStopPoint( Eigen::Vector2d trajectory_vec; { const double yaw = - getYawFromGeometryMsgsQuaternion(base_path.points.at(i).pose.orientation); + getYawFromQuaternion(base_path.points.at(i).pose.orientation); trajectory_vec << std::cos(yaw), std::sin(yaw); } Eigen::Vector2d collision_point_vec; diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 9d0605e2..d187efde 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -36,7 +36,7 @@ void PointHelper::getNearestPoint( { double min_norm = 0.0; bool is_init = false; - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); + const double yaw = getYawFromQuaternion(base_pose.orientation); Eigen::Vector2d base_pose_vec; base_pose_vec << std::cos(yaw), std::sin(yaw); @@ -59,7 +59,7 @@ void PointHelper::getLateralNearestPoint( pcl::PointXYZ * lateral_nearest_point, double * deviation) const { double min_norm = std::numeric_limits::max(); - const double yaw = getYawFromGeometryMsgsQuaternion(base_pose.orientation); + const double yaw = getYawFromQuaternion(base_pose.orientation); Eigen::Vector2d base_pose_vec; base_pose_vec << std::cos(yaw), std::sin(yaw); for (size_t i = 0; i < pointcloud.size(); ++i) { @@ -131,7 +131,7 @@ StopPoint PointHelper::createTargetPoint( { line_start_point << base_path.points.at(0).pose.position.x, base_path.points.at(0).pose.position.y; - const double yaw = getYawFromGeometryMsgsQuaternion(base_path.points.at(0).pose.orientation); + const double yaw = getYawFromQuaternion(base_path.points.at(0).pose.orientation); line_end_point << std::cos(yaw), std::sin(yaw); } From 14f7786a7275cd0dc9bce722dfdaa1b340ef1cbc Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Fri, 5 Mar 2021 13:51:43 +0900 Subject: [PATCH 27/62] follow lane_departure_checker's way --- .../src/trajectory.cpp | 38 +++++++++---------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index b6d5e822..0ae6ebf6 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -39,33 +39,30 @@ bool Trajectory::decimateTrajectory( double trajectory_length_sum = 0.0; double next_length = 0.0; const double epsilon = 0.001; - Eigen::Vector2d point1, point2; PointHelper point_helper {vehicle_info}; for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { if (next_length <= trajectory_length_sum + epsilon) { - Eigen::Vector2d line_start_point, line_end_point, interpolated_point; - line_start_point << input_trajectory.points.at(i).pose.position.x, - input_trajectory.points.at(i).pose.position.y; - line_end_point << input_trajectory.points.at(i + 1).pose.position.x, - input_trajectory.points.at(i + 1).pose.position.y; + const auto line_start_point = autoware_utils::fromMsg( + input_trajectory.points.at(i).pose.position).to_2d(); + const auto line_end_point = autoware_utils::fromMsg( + input_trajectory.points.at(i + 1).pose.position).to_2d(); + autoware_utils::Point2d interpolated_point; point_helper.getBackwardPointFromBasePoint( line_start_point, line_end_point, line_end_point, -1.0 * (trajectory_length_sum - next_length), interpolated_point); - autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; - trajectory_point = input_trajectory.points.at(i); - trajectory_point.pose.position.x = interpolated_point.x(); - trajectory_point.pose.position.y = interpolated_point.y(); + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point = input_trajectory.points.at(i); + trajectory_point.pose.position = autoware_utils::toMsg( + interpolated_point.to_3d(input_trajectory.points.at(i).pose.position.z)); output_trajectory.points.push_back(trajectory_point); index_map.insert(std::make_pair(output_trajectory.points.size() - 1, size_t(i))); next_length += step_length; continue; } - trajectory_length_sum += std::hypot( - input_trajectory.points.at(i).pose.position.x - input_trajectory.points.at( - i + 1).pose.position.x, - input_trajectory.points.at(i).pose.position.y - input_trajectory.points.at( - i + 1).pose.position.y); + + const auto p1 = autoware_utils::fromMsg(input_trajectory.points.at(i).pose.position); + const auto p2 = autoware_utils::fromMsg(input_trajectory.points.at(i + 1).pose.position); + trajectory_length_sum += boost::geometry::distance(p1.to_2d(), p2.to_2d()); } if (!input_trajectory.points.empty()) { output_trajectory.points.push_back(input_trajectory.points.back()); @@ -84,12 +81,13 @@ bool Trajectory::trimTrajectoryWithIndexFromSelfPose( size_t min_distance_index = 0; bool is_init = false; for (size_t i = 0; i < input_trajectory.points.size(); ++i) { - const double distance = std::hypot( - input_trajectory.points.at(i).pose.position.x - self_pose.position.x, - input_trajectory.points.at(i).pose.position.y - self_pose.position.y); - if (!is_init || distance < min_distance) { + const auto p1 = autoware_utils::fromMsg(input_trajectory.points.at(i).pose.position); + const auto p2 = autoware_utils::fromMsg(self_pose.position); + const double point_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + if (!is_init || point_distance < min_distance) { is_init = true; - min_distance = distance; + min_distance = point_distance; min_distance_index = i; } } From f1bfb63d77522d3265b63adb5e730fb74446b1cc Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Fri, 5 Mar 2021 14:00:44 +0900 Subject: [PATCH 28/62] remove jsonDumpsPose and related code --- .../include/obstacle_stop_planner/node.hpp | 4 +-- .../include/obstacle_stop_planner/util.hpp | 25 ------------------- obstacle_stop_planner_refine/src/node.cpp | 9 ++----- 3 files changed, 3 insertions(+), 35 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 002cfd22..8d3f6ed2 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -65,7 +65,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr expand_stop_range_sub_; rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; std::shared_ptr debug_ptr_; tf2_ros::Buffer tf_buffer_; @@ -120,8 +119,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_collision_point, const PointHelper & point_helper, - autoware_planning_msgs::msg::Trajectory & output_msg, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag); + autoware_planning_msgs::msg::Trajectory & output_msg); void insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 8531fb59..4bba922c 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -39,31 +39,6 @@ inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) return yaw; } -inline std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} - -inline diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; - stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stop_reason_diag.name = "stop_reason"; - stop_reason_diag.message = stop_reason; - stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); - stop_reason_diag.values.push_back(stop_reason_diag_kv); - return stop_reason_diag; -} - inline cv::Point2d calcCentroid(const std::vector & pointcloud) { cv::Point2d centroid; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 6c355a8d..44af0c5f 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -114,7 +114,6 @@ void ObstacleStopPlannerNode::pathCallback( const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; /* * trim trajectory from self pose @@ -266,8 +265,7 @@ void ObstacleStopPlannerNode::pathCallback( base_path, nearest_collision_point, point_helper, - output_msg, - stop_reason_diag); + output_msg); } /* @@ -284,7 +282,6 @@ void ObstacleStopPlannerNode::pathCallback( output_msg); } path_pub_->publish(output_msg); - stop_reason_diag_pub_->publish(stop_reason_diag); debug_ptr_->publish(); } @@ -391,8 +388,7 @@ void ObstacleStopPlannerNode::insertStopPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_collision_point, const PointHelper & point_helper, - autoware_planning_msgs::msg::Trajectory & output_msg, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag) + autoware_planning_msgs::msg::Trajectory & output_msg) { for (size_t i = search_start_index; i < base_path.points.size(); ++i) { Eigen::Vector2d trajectory_vec; @@ -414,7 +410,6 @@ void ObstacleStopPlannerNode::insertStopPoint( if (stop_point.index <= output_msg.points.size()) { const auto trajectory_point = point_helper.insertStopPoint(stop_point, base_path, output_msg); - stop_reason_diag = makeStopReasonDiag("obstacle", trajectory_point.pose); debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); } break; From 43ff1a346e7e301a78619fdc09ecbe38c6b9725e Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Fri, 5 Mar 2021 15:28:48 +0900 Subject: [PATCH 29/62] replace VehicleInfo to Param --- .../include/obstacle_stop_planner/node.hpp | 3 +- .../obstacle_point_cloud.hpp | 6 +- .../one_step_polygon.hpp | 1 - .../include/obstacle_stop_planner/param.hpp | 52 ++++++++ .../obstacle_stop_planner/point_helper.hpp | 11 +- .../obstacle_stop_planner/trajectory.hpp | 8 +- .../include/obstacle_stop_planner/util.hpp | 16 +++ .../util/create_vehicle_footprint.hpp | 13 +- .../include/obstacle_stop_planner/vehicle.hpp | 123 ------------------ obstacle_stop_planner_refine/src/node.cpp | 115 +++++++++++----- .../src/obstacle_point_cloud.cpp | 14 +- .../src/point_helper.cpp | 6 +- .../src/trajectory.cpp | 16 +-- 13 files changed, 188 insertions(+), 196 deletions(-) create mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp delete mode 100644 obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 8d3f6ed2..bdff252c 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -40,7 +40,6 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/obstacle_point_cloud.hpp" -#include "obstacle_stop_planner/vehicle.hpp" #include "obstacle_stop_planner/trajectory.hpp" #include "obstacle_stop_planner/one_step_polygon.hpp" #include "obstacle_stop_planner/point_helper.hpp" @@ -71,8 +70,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // ObstacleStopPlanner impl_; ObstaclePointCloud obstacle_pointcloud_; - VehicleInfo vehicle_info_; Trajectory trajectory_; + Param param_; /* * Parameter diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 982a0e28..bf1ed52e 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -25,7 +25,7 @@ #include "rclcpp/logger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" -#include "obstacle_stop_planner/vehicle.hpp" +#include "obstacle_stop_planner/param.hpp" namespace obstacle_stop_planner { @@ -41,13 +41,13 @@ class ObstaclePointCloud pcl::PointCloud::Ptr searchCandidateObstacle( const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory, - const VehicleInfo & vehicle_info); + const Param & param); private: bool searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, - const VehicleInfo & vehicle_info, + const Param & param, pcl::PointCloud::Ptr output_pointcloud_ptr); sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 872b67ea..45b91ad7 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -25,7 +25,6 @@ #include "boost/geometry/geometries/linestring.hpp" #include "boost/geometry/geometries/point_xy.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "obstacle_stop_planner/vehicle.hpp" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp new file mode 100644 index 00000000..34af69d3 --- /dev/null +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/param.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 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 OBSTACLE_STOP_PLANNER__PARAM_HPP_ +#define OBSTACLE_STOP_PLANNER__PARAM_HPP_ + +#include "autoware_utils/autoware_utils.hpp" + +namespace obstacle_stop_planner +{ + +struct Param +{ + VehicleInfo vehicle_info; + double stop_margin; + double min_behavior_stop_margin; + double step_length; + double extend_distance; + double expand_stop_range; + double slow_down_margin; + double expand_slow_down_range; + double max_slow_down_vel; + double min_slow_down_vel; + double max_deceleration; + bool enable_slow_down; + double stop_search_radius; + double slow_down_search_radius; +}; + +inline double getSearchRadius(const Param & param) +{ + if (param.enable_slow_down) { + return param.slow_down_search_radius; + } else { + return param.stop_search_radius; + } +} + +} // namespace obstacle_stop_planner + +#endif // OBSTACLE_STOP_PLANNER__PARAM_HPP_ diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index b61ed508..149c7e6b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "obstacle_stop_planner/util.hpp" -#include "obstacle_stop_planner/vehicle.hpp" +#include "obstacle_stop_planner/param.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" @@ -49,11 +49,8 @@ struct SlowDownPoint class PointHelper { public: - explicit PointHelper(const VehicleInfo & vehicle_info) - { - vehicle_info_ = std::make_shared(vehicle_info); - } - explicit PointHelper(std::shared_ptr & vehicle_info) {vehicle_info_ = vehicle_info;} + explicit PointHelper(const Param & param) + : param_(param) {} bool getBackwardPointFromBasePoint( const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, @@ -94,7 +91,7 @@ class PointHelper double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const; private: - std::shared_ptr vehicle_info_; + const Param param_; }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index f760eddd..6b1fb9bd 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -20,8 +20,8 @@ #include "geometry_msgs/msg/pose.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" -#include "obstacle_stop_planner/vehicle.hpp" #include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/param.hpp" namespace obstacle_stop_planner { @@ -31,11 +31,11 @@ class Trajectory public: bool decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const VehicleInfo & vehicle_info, + const Param & param, autoware_planning_msgs::msg::Trajectory & output_trajectory); bool decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const VehicleInfo & vehicle_info, + const Param & param, autoware_planning_msgs::msg::Trajectory & output_trajectory, std::map & index_map); bool trimTrajectoryFromSelfPose( @@ -49,7 +49,7 @@ class Trajectory size_t & index); bool extendTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const VehicleInfo & vehicle_info, + const Param & param, autoware_planning_msgs::msg::Trajectory & output_trajectory); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 4bba922c..c8b6f983 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -39,6 +39,22 @@ inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) return yaw; } +inline geometry_msgs::msg::Pose getVehicleCenterFromBase( + const geometry_msgs::msg::Pose & base_pose, + const double vehicle_length, + const double rear_overhang) +{ + geometry_msgs::msg::Pose center_pose; + const double yaw = getYawFromQuaternion(base_pose.orientation); + center_pose.position.x = + base_pose.position.x + (vehicle_length / 2.0 - rear_overhang) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (vehicle_length / 2.0 - rear_overhang) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} + inline cv::Point2d calcCentroid(const std::vector & pointcloud) { cv::Point2d centroid; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp index 634b2565..b3a92a82 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util/create_vehicle_footprint.hpp @@ -15,13 +15,12 @@ #ifndef OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #define OBSTACLE_STOP_PLANNER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#include -#include +#include "autoware_utils/autoware_utils.hpp" namespace obstacle_stop_planner { inline autoware_utils::LinearRing2d createVehicleFootprint( - const vehicle_info_util::VehicleInfo & vehicle_info, const double top_margin = 0.0, + const VehicleInfo & vehicle_info, const double top_margin = 0.0, const double side_margin = 0.0) { using autoware_utils::LinearRing2d; @@ -29,10 +28,10 @@ inline autoware_utils::LinearRing2d createVehicleFootprint( const auto & i = vehicle_info; - const double x_front = i.front_overhang_m_ + i.wheel_base_m_ + top_margin; - const double x_rear = -(i.rear_overhang_m_ + top_margin); - const double y_left = i.wheel_tread_m_ / 2.0 + i.left_overhang_m_ + side_margin; - const double y_right = -(i.wheel_tread_m_ / 2.0 + i.right_overhang_m_ + side_margin); + const double x_front = i.front_overhang + i.wheel_base + top_margin; + const double x_rear = -(i.rear_overhang + top_margin); + const double y_left = i.wheel_tread / 2.0 + i.left_overhang + side_margin; + const double y_right = -(i.wheel_tread / 2.0 + i.right_overhang + side_margin); LinearRing2d footprint; footprint.push_back(Point2d{x_front, y_left}); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp deleted file mode 100644 index 4927ad43..00000000 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/vehicle.hpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2021 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 OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ -#define OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "vehicle_info_util/vehicle_info.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "obstacle_stop_planner/util.hpp" - -namespace obstacle_stop_planner -{ - -class VehicleInfo : public vehicle_info_util::VehicleInfo -{ -public: - VehicleInfo( - vehicle_info_util::VehicleInfo super, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter) - : vehicle_info_util::VehicleInfo(super) - { - // Parameters - stop_margin_ = - parameter->declare_parameter( - "stop_planner.stop_margin", - rclcpp::ParameterValue(5.0)).get(); - min_behavior_stop_margin_ = parameter->declare_parameter( - "stop_planner.min_behavior_stop_margin", rclcpp::ParameterValue(2.0)).get(); - step_length_ = - parameter->declare_parameter( - "stop_planner.step_length", - rclcpp::ParameterValue(1.0)).get(); - extend_distance_ = parameter->declare_parameter( - "stop_planner.extend_distance", rclcpp::ParameterValue( - 0.0)).get(); - expand_stop_range_ = parameter->declare_parameter( - "stop_planner.expand_stop_range", rclcpp::ParameterValue( - 0.0)).get(); - - slow_down_margin_ = parameter->declare_parameter( - "slow_down_planner.slow_down_margin", rclcpp::ParameterValue( - 5.0)).get(); - expand_slow_down_range_ = parameter->declare_parameter( - "slow_down_planner.expand_slow_down_range", rclcpp::ParameterValue(1.0)).get(); - max_slow_down_vel_ = parameter->declare_parameter( - "slow_down_planner.max_slow_down_vel", rclcpp::ParameterValue( - 4.0)).get(); - min_slow_down_vel_ = parameter->declare_parameter( - "slow_down_planner.min_slow_down_vel", rclcpp::ParameterValue( - 2.0)).get(); - max_deceleration_ = parameter->declare_parameter( - "slow_down_planner.max_deceleration", rclcpp::ParameterValue( - 2.0)).get(); - enable_slow_down_ = - parameter->declare_parameter("enable_slow_down", rclcpp::ParameterValue(false)).get(); - - stop_margin_ += wheel_base_m_ + front_overhang_m_; - min_behavior_stop_margin_ += wheel_base_m_ + front_overhang_m_; - slow_down_margin_ += wheel_base_m_ + front_overhang_m_; - stop_search_radius_ = - step_length_ + - std::hypot(vehicle_width_m_ / 2.0 + expand_stop_range_, vehicle_length_m_ / 2.0); - slow_down_search_radius_ = - step_length_ + - std::hypot(vehicle_width_m_ / 2.0 + expand_slow_down_range_, vehicle_length_m_ / 2.0); - } - - geometry_msgs::msg::Pose getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose) const - { - geometry_msgs::msg::Pose center_pose; - const double yaw = getYawFromQuaternion(base_pose.orientation); - center_pose.position.x = - base_pose.position.x + (vehicle_length_m_ / 2.0 - rear_overhang_m_) * std::cos(yaw); - center_pose.position.y = - base_pose.position.y + (vehicle_length_m_ / 2.0 - rear_overhang_m_) * std::sin(yaw); - center_pose.position.z = base_pose.position.z; - center_pose.orientation = base_pose.orientation; - return center_pose; - } - - double getSearchRadius() const - { - if (enable_slow_down_) { - return slow_down_search_radius_; - } else { - return stop_search_radius_; - } - } - - double stop_margin_; - double min_behavior_stop_margin_; - double step_length_; - double extend_distance_; - double expand_stop_range_; - double slow_down_margin_; - double expand_slow_down_range_; - double max_slow_down_vel_; - double min_slow_down_vel_; - double max_deceleration_; - bool enable_slow_down_; - double stop_search_radius_; - double slow_down_search_radius_; - -private: -}; - -} // namespace obstacle_stop_planner - -#endif // OBSTACLE_STOP_PLANNER__VEHICLE_HPP_ diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 44af0c5f..636f63cc 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -33,6 +33,7 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" +#include "vehicle_info_util/vehicle_info.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" @@ -44,19 +45,63 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode() : Node("obstacle_stop_planner"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), - obstacle_pointcloud_(this->get_logger()), - vehicle_info_(vehicle_info_util::VehicleInfo::create(*this), - this->get_node_parameters_interface()) + obstacle_pointcloud_(this->get_logger()) { + // Vehicle Info + auto i = vehicle_info_util::VehicleInfo::create(*this); + param_.vehicle_info.wheel_radius = i.wheel_radius_m_; + param_.vehicle_info.wheel_width = i.wheel_width_m_; + param_.vehicle_info.wheel_base = i.wheel_base_m_; + param_.vehicle_info.wheel_tread = i.wheel_tread_m_; + param_.vehicle_info.front_overhang = i.front_overhang_m_; + param_.vehicle_info.rear_overhang = i.rear_overhang_m_; + param_.vehicle_info.left_overhang = i.left_overhang_m_; + param_.vehicle_info.right_overhang = i.right_overhang_m_; + param_.vehicle_info.vehicle_height = i.vehicle_height_m_; + param_.vehicle_info.vehicle_length = i.vehicle_length_m_; + param_.vehicle_info.vehicle_width = i.vehicle_width_m_; + param_.vehicle_info.min_longitudinal_offset = i.min_longitudinal_offset_m_; + param_.vehicle_info.max_longitudinal_offset = i.max_longitudinal_offset_m_; + param_.vehicle_info.min_lateral_offset = i.min_lateral_offset_m_; + param_.vehicle_info.max_lateral_offset = i.max_lateral_offset_m_; + param_.vehicle_info.min_height_offset = i.min_height_offset_m_; + param_.vehicle_info.max_height_offset = i.max_height_offset_m_; + + // Parameters + param_.stop_margin = declare_parameter("stop_planner.stop_margin", 5.0); + param_.min_behavior_stop_margin = declare_parameter("stop_planner.min_behavior_stop_margin", 2.0); + param_.step_length = declare_parameter("stop_planner.step_length", 1.0); + param_.extend_distance = declare_parameter("stop_planner.extend_distance", 0.0); + param_.expand_stop_range = declare_parameter("stop_planner.expand_stop_range", 0.0); + + param_.slow_down_margin = declare_parameter("slow_down_planner.slow_down_margin", 5.0); + param_.expand_slow_down_range = + declare_parameter("slow_down_planner.expand_slow_down_range", 1.0); + param_.max_slow_down_vel = declare_parameter("slow_down_planner.max_slow_down_vel", 4.0); + param_.min_slow_down_vel = declare_parameter("slow_down_planner.min_slow_down_vel", 2.0); + param_.max_deceleration = declare_parameter("slow_down_planner.max_deceleration", 2.0); + param_.enable_slow_down = declare_parameter("enable_slow_down", false); + + param_.stop_margin += param_.vehicle_info.wheel_base + param_.vehicle_info.front_overhang; + param_.min_behavior_stop_margin += + param_.vehicle_info.wheel_base + param_.vehicle_info.front_overhang; + param_.slow_down_margin += param_.vehicle_info.wheel_base + param_.vehicle_info.front_overhang; + param_.stop_search_radius = param_.step_length + std::hypot( + param_.vehicle_info.vehicle_width / 2.0 + param_.expand_stop_range, + param_.vehicle_info.vehicle_length / 2.0); + param_.slow_down_search_radius = param_.step_length + std::hypot( + param_.vehicle_info.vehicle_width / 2.0 + param_.expand_slow_down_range, + param_.vehicle_info.vehicle_length / 2.0); + debug_ptr_ = std::make_shared( this, - vehicle_info_.wheel_base_m_ + - vehicle_info_.front_overhang_m_); + param_.vehicle_info.wheel_base + + param_.vehicle_info.front_overhang); // Initializer acc_controller_ = std::make_unique( - this, vehicle_info_.vehicle_width_m_, vehicle_info_.vehicle_length_m_, - vehicle_info_.wheel_base_m_, vehicle_info_.front_overhang_m_); + this, param_.vehicle_info.vehicle_width, param_.vehicle_info.vehicle_length, + param_.vehicle_info.wheel_base, param_.vehicle_info.front_overhang); // Publishers path_pub_ = @@ -99,7 +144,7 @@ void ObstacleStopPlannerNode::pathCallback( return; } - if (!current_velocity_ptr_ && vehicle_info_.enable_slow_down_) { + if (!current_velocity_ptr_ && param_.enable_slow_down) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for current velocity..."); @@ -110,7 +155,7 @@ void ObstacleStopPlannerNode::pathCallback( * extend trajectory to consider obstacles after the goal */ autoware_planning_msgs::msg::Trajectory extended_trajectory; - trajectory_.extendTrajectory(*input_msg, vehicle_info_, extended_trajectory); + trajectory_.extendTrajectory(*input_msg, param_, extended_trajectory); const autoware_planning_msgs::msg::Trajectory base_path = extended_trajectory; autoware_planning_msgs::msg::Trajectory output_msg = *input_msg; @@ -132,7 +177,7 @@ void ObstacleStopPlannerNode::pathCallback( autoware_planning_msgs::msg::Trajectory decimate_trajectory; std::map decimate_trajectory_index_map; trajectory_.decimateTrajectory( - trim_trajectory, vehicle_info_.step_length_, vehicle_info_, decimate_trajectory, + trim_trajectory, param_.step_length, param_, decimate_trajectory, decimate_trajectory_index_map); autoware_planning_msgs::msg::Trajectory & trajectory = decimate_trajectory; @@ -144,7 +189,7 @@ void ObstacleStopPlannerNode::pathCallback( new pcl::PointCloud); // search obstacle candidate pointcloud to reduce calculation cost - obstacle_pointcloud_.searchCandidateObstacle(tf_buffer_, trajectory, vehicle_info_); + obstacle_pointcloud_.searchCandidateObstacle(tf_buffer_, trajectory, param_); /* * check collision, slow_down @@ -162,20 +207,24 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointXYZ lateral_nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; - PointHelper point_helper {vehicle_info_}; + PointHelper point_helper {param_}; for (int i = 0; i < static_cast(trajectory.points.size()) - 1; ++i) { /* * create one step circle center for vehicle */ - const auto prev_center_pose = vehicle_info_.getVehicleCenterFromBase( + const auto prev_center_pose = getVehicleCenterFromBase( trajectory.points.at( - i).pose); + i).pose, + param_.vehicle_info.vehicle_length, + param_.vehicle_info.rear_overhang); autoware_utils::Point2d prev_center_point( prev_center_pose.position.x, prev_center_pose.position.y); - const auto next_center_pose = vehicle_info_.getVehicleCenterFromBase( - trajectory.points.at(i + 1).pose); + const auto next_center_pose = getVehicleCenterFromBase( + trajectory.points.at(i + 1).pose, + param_.vehicle_info.vehicle_length, + param_.vehicle_info.rear_overhang); autoware_utils::Point2d next_center_point( next_center_pose.position.x, next_center_pose.position.y); @@ -184,20 +233,20 @@ void ObstacleStopPlannerNode::pathCallback( */ const auto move_vehicle_polygon = createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - vehicle_info_.expand_stop_range_, vehicle_info_); + param_.expand_stop_range, param_.vehicle_info); debug_ptr_->pushPolygon( move_vehicle_polygon, trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); autoware_utils::Polygon2d move_slow_down_range_polygon; - if (vehicle_info_.enable_slow_down_) { + if (param_.enable_slow_down) { /* * create one step polygon for slow_down range */ move_slow_down_range_polygon = createOneStepPolygon( trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - vehicle_info_.expand_slow_down_range_, vehicle_info_); + param_.expand_slow_down_range, param_.vehicle_info); debug_ptr_->pushPolygon( move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDownRange); @@ -208,14 +257,14 @@ void ObstacleStopPlannerNode::pathCallback( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; getSlowDownPointcloud( - is_slow_down, vehicle_info_.enable_slow_down_, + is_slow_down, param_.enable_slow_down, obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, - vehicle_info_.slow_down_search_radius_, + param_.slow_down_search_radius, move_slow_down_range_polygon, slow_down_pointcloud_ptr, candidate_slow_down); getCollisionPointcloud( slow_down_pointcloud_ptr, prev_center_point, next_center_point, - vehicle_info_.stop_search_radius_, move_vehicle_polygon, trajectory.points.at( + param_.stop_search_radius, move_vehicle_polygon, trajectory.points.at( i), collision_pointcloud_ptr, is_collision); if (candidate_slow_down && !is_collision && !is_slow_down) { @@ -278,7 +327,7 @@ void ObstacleStopPlannerNode::pathCallback( nearest_slow_down_point, point_helper, calcSlowDownTargetVel(lateral_deviation), - vehicle_info_.slow_down_margin_, + param_.slow_down_margin, output_msg); } path_pub_->publish(output_msg); @@ -420,11 +469,11 @@ void ObstacleStopPlannerNode::insertStopPoint( void ObstacleStopPlannerNode::externalExpandStopRangeCallback( const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg) { - vehicle_info_.expand_stop_range_ = input_msg->expand_stop_range; - vehicle_info_.stop_search_radius_ = - vehicle_info_.step_length_ + std::hypot( - vehicle_info_.vehicle_width_m_ / 2.0 + vehicle_info_.expand_stop_range_, - vehicle_info_.vehicle_length_m_ / 2.0); + param_.expand_stop_range = input_msg->expand_stop_range; + param_.stop_search_radius = + param_.step_length + std::hypot( + param_.vehicle_info.vehicle_width / 2.0 + param_.expand_stop_range, + param_.vehicle_info.vehicle_length / 2.0); } void ObstacleStopPlannerNode::insertSlowDownVelocity( @@ -444,7 +493,7 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( slow_down_target_vel, std::sqrt( std::max( - slow_down_vel * slow_down_vel - 2 * vehicle_info_.max_deceleration_ * dist, + slow_down_vel * slow_down_vel - 2 * param_.max_deceleration * dist, 0.0))); if (!is_slow_down_end && slow_down_vel <= slow_down_target_vel) { slow_down_end_trajectory_point = output_path.points.at(j + 1); @@ -460,10 +509,10 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) { - return vehicle_info_.min_slow_down_vel_ + - (vehicle_info_.max_slow_down_vel_ - vehicle_info_.min_slow_down_vel_) * - std::max(lateral_deviation - vehicle_info_.vehicle_width_m_ / 2, 0.0) / - vehicle_info_.expand_slow_down_range_; + return param_.min_slow_down_vel + + (param_.max_slow_down_vel - param_.min_slow_down_vel) * + std::max(lateral_deviation - param_.vehicle_info.vehicle_width / 2, 0.0) / + param_.expand_slow_down_range; } void ObstacleStopPlannerNode::dynamicObjectCallback( diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 3f603cde..9dbd98a3 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -14,6 +14,7 @@ #include #include "obstacle_stop_planner/obstacle_point_cloud.hpp" +#include "obstacle_stop_planner/util.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/utils.h" #include "tf2_eigen/tf2_eigen.h" @@ -54,7 +55,7 @@ bool ObstaclePointCloud::isDataReceived() pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( const tf2_ros::Buffer & tf_buffer, const autoware_planning_msgs::msg::Trajectory & trajectory, - const VehicleInfo & vehicle_info) + const Param & param) { pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( new pcl::PointCloud); @@ -88,7 +89,7 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( // search obstacle candidate pointcloud to reduce calculation cost searchPointcloudNearTrajectory( trajectory, transformed_obstacle_pointcloud_ptr, - vehicle_info, + param, obstacle_candidate_pointcloud_ptr); obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; return obstacle_candidate_pointcloud_ptr; @@ -97,12 +98,15 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( bool ObstaclePointCloud::searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, - const VehicleInfo & vehicle_info, + const Param & param, pcl::PointCloud::Ptr output_pointcloud_ptr) { - const double squared_radius = vehicle_info.getSearchRadius() * vehicle_info.getSearchRadius(); + const double squared_radius = getSearchRadius(param) * getSearchRadius(param); for (const auto & trajectory_point : trajectory.points) { - const auto center_pose = vehicle_info.getVehicleCenterFromBase(trajectory_point.pose); + const auto center_pose = getVehicleCenterFromBase( + trajectory_point.pose, + param.vehicle_info.vehicle_length, + param.vehicle_info.rear_overhang); for (const auto & point : input_pointcloud_ptr->points) { const double x = center_pose.position.x - point.x; const double y = center_pose.position.y - point.y; diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index d187efde..d9894431 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -98,10 +98,10 @@ StopPoint PointHelper::searchInsertPoint( { const auto max_dist_stop_point = createTargetPoint( - idx, vehicle_info_->stop_margin_, trajectory_vec, collision_point_vec, + idx, param_.stop_margin, trajectory_vec, collision_point_vec, base_path); const auto min_dist_stop_point = createTargetPoint( - idx, vehicle_info_->min_behavior_stop_margin_, trajectory_vec, collision_point_vec, base_path); + idx, param_.min_behavior_stop_margin, trajectory_vec, collision_point_vec, base_path); // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; @@ -188,7 +188,7 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( slow_down_point.velocity = std::max( std::sqrt( - slow_down_target_vel * slow_down_target_vel + 2 * vehicle_info_->max_deceleration_ * + slow_down_target_vel * slow_down_target_vel + 2 * param_.max_deceleration * length_sum), current_velocity_x); return slow_down_point; diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 0ae6ebf6..5479e948 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -20,18 +20,18 @@ namespace obstacle_stop_planner { bool Trajectory::decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const VehicleInfo & vehicle_info, + const Param & param, autoware_planning_msgs::msg::Trajectory & output_trajectory) { std::map index_map; return decimateTrajectory( - input_trajectory, step_length, vehicle_info, output_trajectory, + input_trajectory, step_length, param, output_trajectory, index_map); } bool Trajectory::decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const VehicleInfo & vehicle_info, + const Param & param, autoware_planning_msgs::msg::Trajectory & output_trajectory, std::map & index_map) { @@ -39,7 +39,7 @@ bool Trajectory::decimateTrajectory( double trajectory_length_sum = 0.0; double next_length = 0.0; const double epsilon = 0.001; - PointHelper point_helper {vehicle_info}; + PointHelper point_helper {param}; for (int i = 0; i < static_cast(input_trajectory.points.size()) - 1; ++i) { if (next_length <= trajectory_length_sum + epsilon) { @@ -111,16 +111,16 @@ bool Trajectory::trimTrajectoryFromSelfPose( bool Trajectory::extendTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const VehicleInfo & vehicle_info, + const Param & param, autoware_planning_msgs::msg::Trajectory & output_trajectory) { output_trajectory = input_trajectory; const auto goal_point = input_trajectory.points.back(); double interpolation_distance = 0.1; - PointHelper point_helper {vehicle_info}; + PointHelper point_helper {param}; double extend_sum = 0.0; - while (extend_sum <= (vehicle_info.extend_distance_ - interpolation_distance)) { + while (extend_sum <= (param.extend_distance - interpolation_distance)) { const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( extend_sum, goal_point); @@ -128,7 +128,7 @@ bool Trajectory::extendTrajectory( extend_sum += interpolation_distance; } const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( - vehicle_info.extend_distance_, + param.extend_distance, goal_point); output_trajectory.points.push_back(extend_trajectory_point); From b9d20470a08596919c483c1f98e03a9e826e8972 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Fri, 5 Mar 2021 15:34:00 +0900 Subject: [PATCH 30/62] change logger variable to const reference --- .../include/obstacle_stop_planner/obstacle_point_cloud.hpp | 4 ++-- obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index bf1ed52e..0f6aa60f 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -33,7 +33,7 @@ namespace obstacle_stop_planner class ObstaclePointCloud { public: - explicit ObstaclePointCloud(rclcpp::Logger logger); + explicit ObstaclePointCloud(const rclcpp::Logger & logger); void updatePointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); @@ -51,7 +51,7 @@ class ObstaclePointCloud pcl::PointCloud::Ptr output_pointcloud_ptr); sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; - rclcpp::Logger logger_; + const rclcpp::Logger logger_; }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 9dbd98a3..df2cc7d8 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -21,7 +21,7 @@ namespace obstacle_stop_planner { -ObstaclePointCloud::ObstaclePointCloud(rclcpp::Logger logger) +ObstaclePointCloud::ObstaclePointCloud(const rclcpp::Logger & logger) : logger_(logger) { } From 04fbdbfbbbb1b50d5a161d1372229761a7610962 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 8 Mar 2021 12:04:22 +0900 Subject: [PATCH 31/62] Use using to simplify the code --- .../obstacle_stop_planner/debug_marker.hpp | 13 ++++++----- .../include/obstacle_stop_planner/node.hpp | 13 ++++++----- .../one_step_polygon.hpp | 7 +++--- .../include/obstacle_stop_planner/util.hpp | 15 ++++++++----- .../src/adaptive_cruise_control.cpp | 2 +- .../src/debug_marker.cpp | 6 ++--- obstacle_stop_planner_refine/src/node.cpp | 22 +++++++++---------- .../src/trajectory.cpp | 2 +- 8 files changed, 44 insertions(+), 36 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp index fed06f2c..aaffb94a 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp @@ -29,6 +29,7 @@ #include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "autoware_utils/autoware_utils.hpp" +#include "obstacle_stop_planner/util.hpp" #define EIGEN_MPL2_ONLY #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -46,8 +47,8 @@ class ObstacleStopPlannerDebugNode explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front); ~ObstacleStopPlannerDebugNode() {} bool pushPolygon( - const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); - bool pushPolygon(const autoware_utils::Polygon3d & polygon, const PolygonType & type); + const Polygon2d & polygon, const double z, const PolygonType & type); + bool pushPolygon(const Polygon3d & polygon, 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); bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); @@ -67,10 +68,10 @@ class ObstacleStopPlannerDebugNode std::shared_ptr slow_down_end_pose_ptr_; std::shared_ptr stop_obstacle_point_ptr_; std::shared_ptr slow_down_obstacle_point_ptr_; - std::vector vehicle_polygons_; - std::vector slow_down_range_polygons_; - std::vector collision_polygons_; - std::vector slow_down_polygons_; + std::vector vehicle_polygons_; + std::vector slow_down_range_polygons_; + std::vector collision_polygons_; + std::vector slow_down_polygons_; }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index bdff252c..87b31104 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -43,6 +43,7 @@ #include "obstacle_stop_planner/trajectory.hpp" #include "obstacle_stop_planner/one_step_polygon.hpp" #include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/util.hpp" namespace obstacle_stop_planner { @@ -100,16 +101,16 @@ class ObstacleStopPlannerNode : public rclcpp::Node void getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, - const autoware_utils::Point2d & prev_center_point, - const autoware_utils::Point2d & next_center_point, + const Point2d & prev_center_point, + const Point2d & next_center_point, const double search_radius, - const autoware_utils::Polygon2d & one_step_polygon, + const Polygon2d & one_step_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down); void getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, - const autoware_utils::Point2d & prev_center_point, - const autoware_utils::Point2d & next_center_point, - const double search_radius, const autoware_utils::Polygon2d & one_step_polygon, + const Point2d & prev_center_point, + const Point2d & next_center_point, + const double search_radius, const Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool & is_collision); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 45b91ad7..4319e742 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -26,14 +26,15 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" +#include "obstacle_stop_planner/util.hpp" namespace obstacle_stop_planner { -inline autoware_utils::Polygon2d createOneStepPolygon( +inline Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, const double expand_width, const VehicleInfo & vehicle_info) { - autoware_utils::Polygon2d one_step_move_vehicle_corner_points; + Polygon2d one_step_move_vehicle_corner_points; const auto footprint = createVehicleFootprint(vehicle_info, 0.0, expand_width); // start step @@ -66,7 +67,7 @@ inline autoware_utils::Polygon2d createOneStepPolygon( } } - autoware_utils::Polygon2d polygon; + Polygon2d polygon; boost::geometry::convex_hull(one_step_move_vehicle_corner_points, polygon); return polygon; } diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index c8b6f983..174c21ea 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -28,6 +28,11 @@ #include "diagnostic_msgs/msg/key_value.hpp" #include "autoware_utils/autoware_utils.hpp" +using autoware_utils::Point2d; +using autoware_utils::Point3d; +using autoware_utils::Polygon2d; +using autoware_utils::Polygon3d; + namespace { inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) @@ -82,18 +87,18 @@ inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quatern return rpy; } -inline autoware_utils::Polygon2d getPolygon( +inline Polygon2d getPolygon( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) { - autoware_utils::Polygon2d obj_poly; + Polygon2d obj_poly; geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation); double l = size.x * std::cos(obj_rpy.y) + l_margin; double w = size.y * std::cos(obj_rpy.x) + w_margin; double co = center_offset; boost::geometry::exterior_ring(obj_poly) = - boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( + boost::assign::list_of(l / 2.0 + co, w / 2.0)(-l / 2.0 + co, w / 2.0)( -l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0); // rotate polygon @@ -101,13 +106,13 @@ inline autoware_utils::Polygon2d getPolygon( boost::geometry::strategy::transform::rotate_transformer< boost::geometry::radian, double, 2, 2> rotate(-obj_rpy.z); // rotation - autoware_utils::Polygon2d rotate_obj_poly; + Polygon2d rotate_obj_poly; boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); // translate polygon boost::geometry::strategy::transform::translate_transformer translate( pose.position.x, pose.position.y); - autoware_utils::Polygon2d translate_obj_poly; + Polygon2d translate_obj_poly; boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); return translate_obj_poly; } diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 8ae8974e..6838d42c 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -201,7 +201,7 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const auto self_poly = getPolygon(self_pose, self_size, self_offset); // get nearest point - autoware_utils::Point2d nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); + Point2d nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); if (nearest_point_idx <= 2) { // if too nearest collision point, return direct distance diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index 2a6a98e8..f1abf075 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -32,9 +32,9 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) + const Polygon2d & polygon, const double z, const PolygonType & type) { - autoware_utils::Polygon3d polygon3d; + Polygon3d polygon3d; for (const auto & point : polygon.outer()) { polygon3d.outer().emplace_back(point.to_3d(z)); } @@ -42,7 +42,7 @@ bool ObstacleStopPlannerDebugNode::pushPolygon( } bool ObstacleStopPlannerDebugNode::pushPolygon( - const autoware_utils::Polygon3d & polygon, const PolygonType & type) + const Polygon3d & polygon, const PolygonType & type) { switch (type) { case PolygonType::Vehicle: diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 636f63cc..5483de9c 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -218,14 +218,14 @@ void ObstacleStopPlannerNode::pathCallback( i).pose, param_.vehicle_info.vehicle_length, param_.vehicle_info.rear_overhang); - autoware_utils::Point2d prev_center_point( + Point2d prev_center_point( prev_center_pose.position.x, prev_center_pose.position.y); const auto next_center_pose = getVehicleCenterFromBase( trajectory.points.at(i + 1).pose, param_.vehicle_info.vehicle_length, param_.vehicle_info.rear_overhang); - autoware_utils::Point2d next_center_point( + Point2d next_center_point( next_center_pose.position.x, next_center_pose.position.y); /* @@ -239,7 +239,7 @@ void ObstacleStopPlannerNode::pathCallback( trajectory.points.at(i).pose.position.z, PolygonType::Vehicle); - autoware_utils::Polygon2d move_slow_down_range_polygon; + Polygon2d move_slow_down_range_polygon; if (param_.enable_slow_down) { /* * create one step polygon for slow_down range @@ -337,16 +337,16 @@ void ObstacleStopPlannerNode::pathCallback( // collision void ObstacleStopPlannerNode::getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, - const autoware_utils::Point2d & prev_center_point, - const autoware_utils::Point2d & next_center_point, + const Point2d & prev_center_point, + const Point2d & next_center_point, const double search_radius, - const autoware_utils::Polygon2d & one_step_polygon, + const Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, pcl::PointCloud::Ptr collision_pointcloud, bool & is_collision) { for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { - autoware_utils::Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); + Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); if ( boost::geometry::distance(prev_center_point, point) < search_radius || boost::geometry::distance(next_center_point, point) < search_radius) @@ -367,16 +367,16 @@ void ObstacleStopPlannerNode::getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, - const autoware_utils::Point2d & prev_center_point, - const autoware_utils::Point2d & next_center_point, + const Point2d & prev_center_point, + const Point2d & next_center_point, const double search_radius, - const autoware_utils::Polygon2d & boost_polygon, + const Polygon2d & boost_polygon, pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down) { if (!is_slow_down && enable_slow_down) { for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { - autoware_utils::Point2d point( + Point2d point( obstacle_candidate_pointcloud->at(j).x, obstacle_candidate_pointcloud->at(j).y); if ( boost::geometry::distance(prev_center_point, point) < search_radius || diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 5479e948..09787147 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -47,7 +47,7 @@ bool Trajectory::decimateTrajectory( input_trajectory.points.at(i).pose.position).to_2d(); const auto line_end_point = autoware_utils::fromMsg( input_trajectory.points.at(i + 1).pose.position).to_2d(); - autoware_utils::Point2d interpolated_point; + Point2d interpolated_point; point_helper.getBackwardPointFromBasePoint( line_start_point, line_end_point, line_end_point, -1.0 * (trajectory_length_sum - next_length), interpolated_point); From 6462d77e1c613444f18b84744d81c197e4b520d3 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 8 Mar 2021 12:44:00 +0900 Subject: [PATCH 32/62] cleanup include file --- .../obstacle_stop_planner/debug_marker.hpp | 11 +---------- .../include/obstacle_stop_planner/node.hpp | 6 ------ .../one_step_polygon.hpp | 5 ----- .../obstacle_stop_planner/trajectory.hpp | 3 --- .../include/obstacle_stop_planner/util.hpp | 19 ++++++++----------- .../src/debug_marker.cpp | 1 - obstacle_stop_planner_refine/src/node.cpp | 1 + .../src/trajectory.cpp | 3 +++ 8 files changed, 13 insertions(+), 36 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp index aaffb94a..d3ec9664 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp @@ -23,22 +23,13 @@ #include "geometry_msgs/msg/pose.hpp" #include "pcl/point_types.h" #include "rclcpp/rclcpp.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" -#include "autoware_utils/autoware_utils.hpp" #include "obstacle_stop_planner/util.hpp" -#define EIGEN_MPL2_ONLY -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" + namespace obstacle_stop_planner { enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown }; - enum class PointType : int8_t { Stop = 0, SlowDown }; - enum class PoseType : int8_t { Stop = 0, SlowDownStart, SlowDownEnd }; class ObstacleStopPlannerDebugNode diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 87b31104..e38f0c31 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -33,15 +33,10 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" -#include "visualization_msgs/msg/marker_array.hpp" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc.hpp" #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/obstacle_point_cloud.hpp" #include "obstacle_stop_planner/trajectory.hpp" -#include "obstacle_stop_planner/one_step_polygon.hpp" #include "obstacle_stop_planner/point_helper.hpp" #include "obstacle_stop_planner/util.hpp" @@ -69,7 +64,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::shared_ptr debug_ptr_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - // ObstacleStopPlanner impl_; ObstaclePointCloud obstacle_pointcloud_; Trajectory trajectory_; Param param_; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp index 4319e742..928f9dd4 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/one_step_polygon.hpp @@ -18,12 +18,7 @@ #include #include #include -#include "boost/assert.hpp" -#include "boost/assign/list_of.hpp" -#include "boost/format.hpp" #include "boost/geometry.hpp" -#include "boost/geometry/geometries/linestring.hpp" -#include "boost/geometry/geometries/point_xy.hpp" #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" #include "obstacle_stop_planner/util.hpp" diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index 6b1fb9bd..4e6c5e0a 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -18,9 +18,6 @@ #include #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "obstacle_stop_planner/point_helper.hpp" #include "obstacle_stop_planner/param.hpp" namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 174c21ea..7ea870a0 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -18,14 +18,11 @@ #include #include -#include "opencv2/core/core.hpp" #include "geometry_msgs/msg/point.hpp" #include "boost/geometry.hpp" #include "boost/format.hpp" #include "boost/assign/list_of.hpp" #include "tf2/utils.h" -#include "diagnostic_msgs/msg/diagnostic_status.hpp" -#include "diagnostic_msgs/msg/key_value.hpp" #include "autoware_utils/autoware_utils.hpp" using autoware_utils::Point2d; @@ -60,17 +57,17 @@ inline geometry_msgs::msg::Pose getVehicleCenterFromBase( return center_pose; } -inline cv::Point2d calcCentroid(const std::vector & pointcloud) +inline Point2d calcCentroid(const std::vector & pointcloud) { - cv::Point2d centroid; - centroid.x = 0; - centroid.y = 0; + Point2d centroid; + centroid.x() = 0; + centroid.y() = 0; for (const auto & point : pointcloud) { - centroid.x += point.x; - centroid.y += point.y; + centroid.x() += point.x(); + centroid.y() += point.y(); } - centroid.x = centroid.x / static_cast(pointcloud.size()); - centroid.y = centroid.y / static_cast(pointcloud.size()); + centroid.x() = centroid.x() / static_cast(pointcloud.size()); + centroid.y() = centroid.y() / static_cast(pointcloud.size()); return centroid; } diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index f1abf075..318855b4 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -16,7 +16,6 @@ #include #include "obstacle_stop_planner/debug_marker.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "autoware_utils/autoware_utils.hpp" namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 5483de9c..3cdba79e 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -33,6 +33,7 @@ #include "boost/geometry/geometries/point_xy.hpp" #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" +#include "obstacle_stop_planner/one_step_polygon.hpp" #include "vehicle_info_util/vehicle_info.hpp" #define EIGEN_MPL2_ONLY diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 09787147..4b178c53 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -14,7 +14,10 @@ #include #include +#include "boost/geometry.hpp" #include "obstacle_stop_planner/trajectory.hpp" +#include "obstacle_stop_planner/point_helper.hpp" +#include "obstacle_stop_planner/util.hpp" namespace obstacle_stop_planner { From 475bfae626a2e3c62b8975b59df2795a1803c9bb Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 8 Mar 2021 15:20:47 +0900 Subject: [PATCH 33/62] Refactor trajectory, util - Delete unused function - Redesign input/output --- .../obstacle_stop_planner/trajectory.hpp | 32 +++---- .../include/obstacle_stop_planner/util.hpp | 24 ------ .../src/adaptive_cruise_control.cpp | 9 +- obstacle_stop_planner_refine/src/node.cpp | 28 +++--- .../src/trajectory.cpp | 86 +++++-------------- 5 files changed, 54 insertions(+), 125 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index 4e6c5e0a..b3a25ea5 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ #include +#include #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include "obstacle_stop_planner/param.hpp" @@ -23,31 +24,22 @@ namespace obstacle_stop_planner { +struct DecimateTrajectoryMap +{ + autoware_planning_msgs::msg::Trajectory orig_trajectory; + autoware_planning_msgs::msg::Trajectory decimate_trajectory; + std::map index_map; +}; + class Trajectory { public: - bool decimateTrajectory( + DecimateTrajectoryMap decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & param, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - bool decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & param, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - std::map & index_map); - bool trimTrajectoryFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose & self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory); - bool trimTrajectoryWithIndexFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose & self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - size_t & index); - bool extendTrajectory( + const Param & param); + std::tuple trimTrajectoryWithIndexFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const Param & param, - autoware_planning_msgs::msg::Trajectory & output_trajectory); + const geometry_msgs::msg::Pose & self_pose); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 7ea870a0..682b1b40 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -57,20 +57,6 @@ inline geometry_msgs::msg::Pose getVehicleCenterFromBase( return center_pose; } -inline Point2d calcCentroid(const std::vector & pointcloud) -{ - Point2d centroid; - centroid.x() = 0; - centroid.y() = 0; - for (const auto & point : pointcloud) { - centroid.x() += point.x(); - centroid.y() += point.y(); - } - centroid.x() = centroid.x() / static_cast(pointcloud.size()); - centroid.y() = centroid.y() / static_cast(pointcloud.size()); - return centroid; -} - inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) { tf2::Quaternion quat(q.x, q.y, q.z, q.w); @@ -113,16 +99,6 @@ inline Polygon2d getPolygon( boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); return translate_obj_poly; } - -inline double getDistanceFromTwoPoint( - const geometry_msgs::msg::Point & point1, - const geometry_msgs::msg::Point & point2) -{ - const double dx = point1.x - point2.x; - const double dy = point1.y - point2.y; - const double dist = std::hypot(dx, dy); - return dist; -} } // namespace #endif // OBSTACLE_STOP_PLANNER__UTIL_HPP_ diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 6838d42c..033b3dc3 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -219,8 +219,9 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx for (int i = 1; i < nearest_point_idx - 1; i++) { - dist_to_point += getDistanceFromTwoPoint( - trajectory.points.at(i).pose.position, trajectory.points.at(i + 1).pose.position); + dist_to_point += boost::geometry::distance( + autoware_utils::fromMsg(trajectory.points.at(i).pose.position).to_2d(), + autoware_utils::fromMsg(trajectory.points.at(i + 1).pose.position).to_2d()); } // add distance from nearest_collision_point to prev point of nearest_point_idx @@ -507,7 +508,9 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( // calc velocity of each point by gradient deceleration const auto current_p = output_trajectory.points[i]; const auto prev_p = output_trajectory.points[i - 1]; - const double p_dist = getDistanceFromTwoPoint(current_p.pose.position, prev_p.pose.position); + const auto p_dist = boost::geometry::distance( + autoware_utils::fromMsg(current_p.pose.position).to_2d(), + autoware_utils::fromMsg(prev_p.pose.position).to_2d()); total_dist += p_dist; if (current_p.twist.linear.x > target_vel && total_dist >= 0) { double next_pre_vel; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 3cdba79e..5940d9c3 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -168,20 +168,17 @@ void ObstacleStopPlannerNode::pathCallback( getSelfPose(input_msg->header, tf_buffer_, self_pose); autoware_planning_msgs::msg::Trajectory trim_trajectory; size_t trajectory_trim_index; - trajectory_.trimTrajectoryWithIndexFromSelfPose( - base_path, self_pose, trim_trajectory, - trajectory_trim_index); + std::tie(trim_trajectory, trajectory_trim_index) = + trajectory_.trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); /* * decimate trajectory for calculation cost */ - autoware_planning_msgs::msg::Trajectory decimate_trajectory; - std::map decimate_trajectory_index_map; - trajectory_.decimateTrajectory( - trim_trajectory, param_.step_length, param_, decimate_trajectory, - decimate_trajectory_index_map); + DecimateTrajectoryMap decimate_trajectory_map = trajectory_.decimateTrajectory( + trim_trajectory, param_.step_length, param_); - autoware_planning_msgs::msg::Trajectory & trajectory = decimate_trajectory; + autoware_planning_msgs::msg::Trajectory & trajectory = + decimate_trajectory_map.decimate_trajectory; /* * search candidate obstacle pointcloud @@ -302,8 +299,12 @@ void ObstacleStopPlannerNode::pathCallback( bool need_to_stop = is_collision; if (is_collision) { acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, decimate_trajectory_collision_index, self_pose, nearest_collision_point, - nearest_collision_point_time, object_ptr_, current_velocity_ptr_, need_to_stop, output_msg); + decimate_trajectory_map.decimate_trajectory, + decimate_trajectory_collision_index, + self_pose, nearest_collision_point, + nearest_collision_point_time, object_ptr_, + current_velocity_ptr_, + need_to_stop, output_msg); } /* @@ -311,7 +312,8 @@ void ObstacleStopPlannerNode::pathCallback( */ if (need_to_stop) { insertStopPoint( - decimate_trajectory_index_map.at(decimate_trajectory_collision_index) + trajectory_trim_index, + decimate_trajectory_map.index_map.at(decimate_trajectory_collision_index) + + trajectory_trim_index, base_path, nearest_collision_point, point_helper, @@ -323,7 +325,7 @@ void ObstacleStopPlannerNode::pathCallback( */ if (is_slow_down) { insertSlowDownPoint( - decimate_trajectory_index_map.at(decimate_trajectory_slow_down_index), + decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, point_helper, diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 4b178c53..9abbf234 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "boost/geometry.hpp" #include "obstacle_stop_planner/trajectory.hpp" #include "obstacle_stop_planner/point_helper.hpp" @@ -21,24 +22,13 @@ namespace obstacle_stop_planner { -bool Trajectory::decimateTrajectory( +DecimateTrajectoryMap Trajectory::decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & param, - autoware_planning_msgs::msg::Trajectory & output_trajectory) + const Param & param) { - std::map index_map; - return decimateTrajectory( - input_trajectory, step_length, param, output_trajectory, - index_map); -} - -bool Trajectory::decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & param, - autoware_planning_msgs::msg::Trajectory & output_trajectory, - std::map & index_map) -{ - output_trajectory.header = input_trajectory.header; + DecimateTrajectoryMap output; + output.orig_trajectory = input_trajectory; + output.decimate_trajectory.header = input_trajectory.header; double trajectory_length_sum = 0.0; double next_length = 0.0; const double epsilon = 0.001; @@ -57,8 +47,9 @@ bool Trajectory::decimateTrajectory( autoware_planning_msgs::msg::TrajectoryPoint trajectory_point = input_trajectory.points.at(i); trajectory_point.pose.position = autoware_utils::toMsg( interpolated_point.to_3d(input_trajectory.points.at(i).pose.position.z)); - output_trajectory.points.push_back(trajectory_point); - index_map.insert(std::make_pair(output_trajectory.points.size() - 1, size_t(i))); + output.decimate_trajectory.points.emplace_back(trajectory_point); + output.index_map.insert( + std::make_pair(output.decimate_trajectory.points.size() - 1, size_t(i))); next_length += step_length; continue; } @@ -68,18 +59,21 @@ bool Trajectory::decimateTrajectory( trajectory_length_sum += boost::geometry::distance(p1.to_2d(), p2.to_2d()); } if (!input_trajectory.points.empty()) { - output_trajectory.points.push_back(input_trajectory.points.back()); - index_map.insert( - std::make_pair(output_trajectory.points.size() - 1, input_trajectory.points.size() - 1)); + output.decimate_trajectory.points.emplace_back(input_trajectory.points.back()); + output.index_map.insert( + std::make_pair( + output.decimate_trajectory.points.size() - 1, + input_trajectory.points.size() - 1)); } - return true; + return output; } -bool Trajectory::trimTrajectoryWithIndexFromSelfPose( +std::tuple +Trajectory::trimTrajectoryWithIndexFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose & self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory, size_t & index) + const geometry_msgs::msg::Pose & self_pose) { + autoware_planning_msgs::msg::Trajectory output_trajectory; double min_distance = 0.0; size_t min_distance_index = 0; bool is_init = false; @@ -95,47 +89,9 @@ bool Trajectory::trimTrajectoryWithIndexFromSelfPose( } } for (size_t i = min_distance_index; i < input_trajectory.points.size(); ++i) { - output_trajectory.points.push_back(input_trajectory.points.at(i)); + output_trajectory.points.emplace_back(input_trajectory.points.at(i)); } output_trajectory.header = input_trajectory.header; - index = min_distance_index; - return true; -} - -bool Trajectory::trimTrajectoryFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose & self_pose, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - size_t index; - return trimTrajectoryWithIndexFromSelfPose( - output_trajectory, self_pose, output_trajectory, index); + return std::forward_as_tuple(output_trajectory, min_distance_index); } - -bool Trajectory::extendTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const Param & param, - autoware_planning_msgs::msg::Trajectory & output_trajectory) -{ - output_trajectory = input_trajectory; - const auto goal_point = input_trajectory.points.back(); - double interpolation_distance = 0.1; - PointHelper point_helper {param}; - - double extend_sum = 0.0; - while (extend_sum <= (param.extend_distance - interpolation_distance)) { - const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( - extend_sum, - goal_point); - output_trajectory.points.push_back(extend_trajectory_point); - extend_sum += interpolation_distance; - } - const auto extend_trajectory_point = point_helper.getExtendTrajectoryPoint( - param.extend_distance, - goal_point); - output_trajectory.points.push_back(extend_trajectory_point); - - return true; -} - } // namespace obstacle_stop_planner From 669698659a82b557d99ebc280c71e4aea753bfd1 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 8 Mar 2021 15:23:47 +0900 Subject: [PATCH 34/62] Delete Trajectory class --- .../include/obstacle_stop_planner/node.hpp | 2 -- .../include/obstacle_stop_planner/trajectory.hpp | 16 ++++++---------- obstacle_stop_planner_refine/src/node.cpp | 5 +++-- obstacle_stop_planner_refine/src/trajectory.cpp | 4 ++-- 4 files changed, 11 insertions(+), 16 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index e38f0c31..95165a0b 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -36,7 +36,6 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/obstacle_point_cloud.hpp" -#include "obstacle_stop_planner/trajectory.hpp" #include "obstacle_stop_planner/point_helper.hpp" #include "obstacle_stop_planner/util.hpp" @@ -65,7 +64,6 @@ class ObstacleStopPlannerNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; ObstaclePointCloud obstacle_pointcloud_; - Trajectory trajectory_; Param param_; /* diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp index b3a25ea5..228e7f87 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp @@ -31,16 +31,12 @@ struct DecimateTrajectoryMap std::map index_map; }; -class Trajectory -{ -public: - DecimateTrajectoryMap decimateTrajectory( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, - const Param & param); - std::tuple trimTrajectoryWithIndexFromSelfPose( - const autoware_planning_msgs::msg::Trajectory & input_trajectory, - const geometry_msgs::msg::Pose & self_pose); -}; +DecimateTrajectoryMap decimateTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, + const Param & param); +std::tuple trimTrajectoryWithIndexFromSelfPose( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const geometry_msgs::msg::Pose & self_pose); } // namespace obstacle_stop_planner #endif // OBSTACLE_STOP_PLANNER__TRAJECTORY_HPP_ diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 5940d9c3..3717e98c 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -34,6 +34,7 @@ #include "obstacle_stop_planner/node.hpp" #include "obstacle_stop_planner/util.hpp" #include "obstacle_stop_planner/one_step_polygon.hpp" +#include "obstacle_stop_planner/trajectory.hpp" #include "vehicle_info_util/vehicle_info.hpp" #define EIGEN_MPL2_ONLY @@ -169,12 +170,12 @@ void ObstacleStopPlannerNode::pathCallback( autoware_planning_msgs::msg::Trajectory trim_trajectory; size_t trajectory_trim_index; std::tie(trim_trajectory, trajectory_trim_index) = - trajectory_.trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); + trimTrajectoryWithIndexFromSelfPose(base_path, self_pose); /* * decimate trajectory for calculation cost */ - DecimateTrajectoryMap decimate_trajectory_map = trajectory_.decimateTrajectory( + DecimateTrajectoryMap decimate_trajectory_map = decimateTrajectory( trim_trajectory, param_.step_length, param_); autoware_planning_msgs::msg::Trajectory & trajectory = diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index 9abbf234..b85d6de3 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -22,7 +22,7 @@ namespace obstacle_stop_planner { -DecimateTrajectoryMap Trajectory::decimateTrajectory( +DecimateTrajectoryMap decimateTrajectory( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double step_length, const Param & param) { @@ -69,7 +69,7 @@ DecimateTrajectoryMap Trajectory::decimateTrajectory( } std::tuple -Trajectory::trimTrajectoryWithIndexFromSelfPose( +trimTrajectoryWithIndexFromSelfPose( const autoware_planning_msgs::msg::Trajectory & input_trajectory, const geometry_msgs::msg::Pose & self_pose) { From 1048d65a6d2a5c9afc5bcac32d427bf57f71b968 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 8 Mar 2021 17:03:14 +0900 Subject: [PATCH 35/62] Refactor point_helper --- .../include/obstacle_stop_planner/node.hpp | 5 +- .../obstacle_stop_planner/point_helper.hpp | 43 ++++--- obstacle_stop_planner_refine/src/node.cpp | 66 +++++------ .../src/point_helper.cpp | 106 +++++++++--------- .../src/trajectory.cpp | 5 +- 5 files changed, 117 insertions(+), 108 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 95165a0b..17cccd64 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -83,9 +83,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg); private: - bool getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, - geometry_msgs::msg::Pose & self_pose); + geometry_msgs::msg::Pose getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer); void insertSlowDownVelocity( const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, autoware_planning_msgs::msg::Trajectory & output_path); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index 149c7e6b..348f9d65 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -36,32 +36,43 @@ namespace obstacle_stop_planner struct StopPoint { size_t index; - Eigen::Vector2d point; + Point2d point; }; struct SlowDownPoint { size_t index; - Eigen::Vector2d point; + Point2d point; double velocity; }; +struct PointStamped +{ + rclcpp::Time time; + pcl::PointXYZ point; +}; + +struct PointDeviation +{ + double deviation; + pcl::PointXYZ point; +}; + class PointHelper { public: explicit PointHelper(const Param & param) : param_(param) {} - bool getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, - Eigen::Vector2d & output_point) const; - void getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) const; - void getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation) const; + Point2d getBackwardPointFromBasePoint( + const Point2d & line_point1, const Point2d & line_point2, + const Point2d & base_point, const double backward_length) const; + PointStamped getNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose) const; + PointDeviation getLateralNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose) const; autoware_planning_msgs::msg::TrajectoryPoint insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, @@ -69,16 +80,16 @@ class PointHelper StopPoint searchInsertPoint( const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) const; + const Point2d & trajectory_vec, const Point2d & collision_point_vec) const; StopPoint createTargetPoint( - const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, - const Eigen::Vector2d & collision_point_vec, + const int idx, const double margin, const Point2d & trajectory_vec, + const Point2d & collision_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path) const; SlowDownPoint createSlowDownStartPoint( const int idx, const double margin, const double slow_down_target_vel, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, + const Point2d & trajectory_vec, const Point2d & slow_down_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path, const double current_velocity_x) const; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 3717e98c..7de53b44 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -165,8 +165,7 @@ void ObstacleStopPlannerNode::pathCallback( /* * trim trajectory from self pose */ - geometry_msgs::msg::Pose self_pose; - getSelfPose(input_msg->header, tf_buffer_, self_pose); + auto self_pose = getSelfPose(input_msg->header, tf_buffer_); autoware_planning_msgs::msg::Trajectory trim_trajectory; size_t trajectory_trim_index; std::tie(trim_trajectory, trajectory_trim_index) = @@ -203,7 +202,6 @@ void ObstacleStopPlannerNode::pathCallback( bool is_slow_down = false; size_t decimate_trajectory_slow_down_index; pcl::PointXYZ nearest_slow_down_point; - pcl::PointXYZ lateral_nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; PointHelper point_helper {param_}; @@ -272,12 +270,13 @@ void ObstacleStopPlannerNode::pathCallback( debug_ptr_->pushPolygon( move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); - point_helper.getNearestPoint( - *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_slow_down_point, - &nearest_collision_point_time); - point_helper.getLateralNearestPoint( - *slow_down_pointcloud_ptr, trajectory.points.at(i).pose, &lateral_nearest_slow_down_point, - &lateral_deviation); + const auto nearest_slow_down_pointstamped = point_helper.getNearestPoint( + *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); + nearest_slow_down_point = nearest_slow_down_pointstamped.point; + nearest_collision_point_time = nearest_slow_down_pointstamped.time; + const auto lateral_nearest_slow_down_point = point_helper.getLateralNearestPoint( + *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); + lateral_deviation = lateral_nearest_slow_down_point.deviation; debug_ptr_->pushObstaclePoint(nearest_slow_down_point, PointType::SlowDown); } @@ -285,9 +284,10 @@ void ObstacleStopPlannerNode::pathCallback( * search nearest collision point by beginning of path */ if (is_collision) { - point_helper.getNearestPoint( - *collision_pointcloud_ptr, trajectory.points.at(i).pose, &nearest_collision_point, - &nearest_collision_point_time); + const auto nearest_collision_pointstamped = point_helper.getNearestPoint( + *collision_pointcloud_ptr, trajectory.points.at(i).pose); + nearest_collision_point = nearest_collision_pointstamped.point; + nearest_collision_point_time = nearest_collision_pointstamped.time; debug_ptr_->pushObstaclePoint(nearest_collision_point, PointType::Stop); decimate_trajectory_collision_index = i; break; @@ -405,15 +405,12 @@ void ObstacleStopPlannerNode::insertSlowDownPoint( autoware_planning_msgs::msg::Trajectory & output_msg) { for (size_t i = search_start_index; i < base_path.points.size(); ++i) { - Eigen::Vector2d trajectory_vec; - { - const double yaw = - getYawFromQuaternion(base_path.points.at(i).pose.orientation); - trajectory_vec << std::cos(yaw), std::sin(yaw); - } - Eigen::Vector2d slow_down_point_vec; - slow_down_point_vec << nearest_slow_down_point.x - base_path.points.at(i).pose.position.x, - nearest_slow_down_point.y - base_path.points.at(i).pose.position.y; + const double yaw = + getYawFromQuaternion(base_path.points.at(i).pose.orientation); + const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; + const Point2d slow_down_point_vec { + nearest_slow_down_point.x - base_path.points.at(i).pose.position.x, + nearest_slow_down_point.y - base_path.points.at(i).pose.position.y}; if ( trajectory_vec.dot(slow_down_point_vec) < 0.0 || @@ -444,15 +441,12 @@ void ObstacleStopPlannerNode::insertStopPoint( autoware_planning_msgs::msg::Trajectory & output_msg) { for (size_t i = search_start_index; i < base_path.points.size(); ++i) { - Eigen::Vector2d trajectory_vec; - { - const double yaw = - getYawFromQuaternion(base_path.points.at(i).pose.orientation); - trajectory_vec << std::cos(yaw), std::sin(yaw); - } - Eigen::Vector2d collision_point_vec; - collision_point_vec << nearest_collision_point.x - base_path.points.at(i).pose.position.x, - nearest_collision_point.y - base_path.points.at(i).pose.position.y; + const double yaw = + getYawFromQuaternion(base_path.points.at(i).pose.orientation); + const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; + const Point2d collision_point_vec { + nearest_collision_point.x - base_path.points.at(i).pose.position.x, + nearest_collision_point.y - base_path.points.at(i).pose.position.y}; if ( trajectory_vec.dot(collision_point_vec) < 0.0 || @@ -531,10 +525,10 @@ void ObstacleStopPlannerNode::currentVelocityCallback( current_velocity_ptr_ = input_msg; } -bool ObstacleStopPlannerNode::getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, - geometry_msgs::msg::Pose & self_pose) +geometry_msgs::msg::Pose ObstacleStopPlannerNode::getSelfPose( + const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer) { + geometry_msgs::msg::Pose self_pose; try { geometry_msgs::msg::TransformStamped transform; transform = @@ -548,9 +542,11 @@ bool ObstacleStopPlannerNode::getSelfPose( self_pose.orientation.y = transform.transform.rotation.y; self_pose.orientation.z = transform.transform.rotation.z; self_pose.orientation.w = transform.transform.rotation.w; - return true; } catch (tf2::TransformException & ex) { - return false; + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "could not get self pose from tf_buffer."); } + return self_pose; } } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index d9894431..386f524f 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -19,61 +19,63 @@ namespace obstacle_stop_planner { -bool PointHelper::getBackwardPointFromBasePoint( - const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, - const Eigen::Vector2d & base_point, const double backward_length, - Eigen::Vector2d & output_point) const +Point2d PointHelper::getBackwardPointFromBasePoint( + const Point2d & line_point1, const Point2d & line_point2, + const Point2d & base_point, const double backward_length) const { - Eigen::Vector2d line_vec = line_point2 - line_point1; - Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); - output_point = base_point + backward_vec; - return true; + const auto line_vec = Eigen::Vector2d(line_point2) - line_point1; + const auto backward_vec = backward_length * line_vec.normalized(); + const auto output_point = Eigen::Vector2d(base_point) + backward_vec; + return Point2d(output_point.x(), output_point.y()); } -void PointHelper::getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) const +PointStamped PointHelper::getNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose) const { double min_norm = 0.0; bool is_init = false; const double yaw = getYawFromQuaternion(base_pose.orientation); - Eigen::Vector2d base_pose_vec; - base_pose_vec << std::cos(yaw), std::sin(yaw); + Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; + PointStamped nearest_collision_point; for (size_t i = 0; i < pointcloud.size(); ++i) { - Eigen::Vector2d pointcloud_vec; - pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y; + Point2d pointcloud_vec { + pointcloud.at(i).x - base_pose.position.x, + pointcloud.at(i).y - base_pose.position.y}; double norm = base_pose_vec.dot(pointcloud_vec); if (norm < min_norm || !is_init) { min_norm = norm; - *nearest_collision_point = pointcloud.at(i); - *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; + nearest_collision_point.point = pointcloud.at(i); + nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; is_init = true; } } + return nearest_collision_point; } -void PointHelper::getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation) const +PointDeviation PointHelper::getLateralNearestPoint( + const pcl::PointCloud & pointcloud, + const geometry_msgs::msg::Pose & base_pose) const { double min_norm = std::numeric_limits::max(); const double yaw = getYawFromQuaternion(base_pose.orientation); - Eigen::Vector2d base_pose_vec; - base_pose_vec << std::cos(yaw), std::sin(yaw); + Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; + PointDeviation lateral_nearest_point; + for (size_t i = 0; i < pointcloud.size(); ++i) { - Eigen::Vector2d pointcloud_vec; - pointcloud_vec << pointcloud.at(i).x - base_pose.position.x, - pointcloud.at(i).y - base_pose.position.y; + Point2d pointcloud_vec { + pointcloud.at(i).x - base_pose.position.x, + pointcloud.at(i).y - base_pose.position.y}; double norm = std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); if (norm < min_norm) { min_norm = norm; - *lateral_nearest_point = pointcloud.at(i); + lateral_nearest_point.point = pointcloud.at(i); } } - *deviation = min_norm; + lateral_nearest_point.deviation = min_norm; + return lateral_nearest_point; } autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( @@ -94,7 +96,7 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( StopPoint PointHelper::searchInsertPoint( const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & collision_point_vec) const + const Point2d & trajectory_vec, const Point2d & collision_point_vec) const { const auto max_dist_stop_point = createTargetPoint( @@ -121,41 +123,41 @@ StopPoint PointHelper::searchInsertPoint( } StopPoint PointHelper::createTargetPoint( - const int idx, const double margin, const Eigen::Vector2d & trajectory_vec, - const Eigen::Vector2d & collision_point_vec, + const int idx, const double margin, const Point2d & trajectory_vec, + const Point2d & collision_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path) const { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(collision_point_vec); - Eigen::Vector2d line_start_point, line_end_point; - { - line_start_point << base_path.points.at(0).pose.position.x, - base_path.points.at(0).pose.position.y; - const double yaw = getYawFromQuaternion(base_path.points.at(0).pose.orientation); - line_end_point << std::cos(yaw), std::sin(yaw); - } + Point2d line_start_point { + base_path.points.at(0).pose.position.x, + base_path.points.at(0).pose.position.y}; + const double yaw = getYawFromQuaternion(base_path.points.at(0).pose.orientation); + Point2d line_end_point {std::cos(yaw), std::sin(yaw)}; - StopPoint stop_point{0, Eigen::Vector2d()}; + StopPoint stop_point{0, Point2d {0.0, 0.0}}; for (size_t j = idx; 0 < j; --j) { - line_start_point << base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y; - line_end_point << base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y; + line_start_point = { + base_path.points.at(j - 1).pose.position.x, + base_path.points.at(j - 1).pose.position.y}; + line_end_point = { + base_path.points.at(j).pose.position.x, + base_path.points.at(j).pose.position.y}; if (margin < length_sum) { stop_point.index = j; break; } length_sum += (line_end_point - line_start_point).norm(); } - getBackwardPointFromBasePoint( - line_start_point, line_end_point, line_start_point, length_sum - margin, stop_point.point); + stop_point.point = getBackwardPointFromBasePoint( + line_start_point, line_end_point, line_start_point, length_sum - margin); return stop_point; } SlowDownPoint PointHelper::createSlowDownStartPoint( const int idx, const double margin, const double slow_down_target_vel, - const Eigen::Vector2d & trajectory_vec, const Eigen::Vector2d & slow_down_point_vec, + const Point2d & trajectory_vec, const Point2d & slow_down_point_vec, const autoware_planning_msgs::msg::Trajectory & base_path, const double current_velocity_x) const { @@ -164,12 +166,14 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( Eigen::Vector2d line_start_point{}; Eigen::Vector2d line_end_point{}; - SlowDownPoint slow_down_point{0, Eigen::Vector2d(), 0.0}; + SlowDownPoint slow_down_point{0, Point2d {0.0, 0.0}, 0.0}; for (size_t j = idx; 0 < j; --j) { - line_start_point << base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y; - line_end_point << base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y; + line_start_point = { + base_path.points.at(j).pose.position.x, + base_path.points.at(j).pose.position.y}; + line_end_point = { + base_path.points.at(j - 1).pose.position.x, + base_path.points.at(j - 1).pose.position.y}; if (margin < length_sum) { slow_down_point.index = j; break; diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index b85d6de3..b1a0d4fd 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -40,10 +40,9 @@ DecimateTrajectoryMap decimateTrajectory( input_trajectory.points.at(i).pose.position).to_2d(); const auto line_end_point = autoware_utils::fromMsg( input_trajectory.points.at(i + 1).pose.position).to_2d(); - Point2d interpolated_point; - point_helper.getBackwardPointFromBasePoint( + Point2d interpolated_point = point_helper.getBackwardPointFromBasePoint( line_start_point, line_end_point, line_end_point, - -1.0 * (trajectory_length_sum - next_length), interpolated_point); + -1.0 * (trajectory_length_sum - next_length)); autoware_planning_msgs::msg::TrajectoryPoint trajectory_point = input_trajectory.points.at(i); trajectory_point.pose.position = autoware_utils::toMsg( interpolated_point.to_3d(input_trajectory.points.at(i).pose.position.z)); From 95c024cbae4379687943f0ec5857ebc15e3bf210 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Mon, 8 Mar 2021 20:45:10 +0900 Subject: [PATCH 36/62] Refactor AdaptiveCruiseControl class --- .../adaptive_cruise_control.hpp | 21 +++--- .../src/adaptive_cruise_control.cpp | 67 ++++++++++--------- obstacle_stop_planner_refine/src/node.cpp | 4 +- 3 files changed, 50 insertions(+), 42 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 3cb22a9e..296b51ba 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include +#include #include "geometry_msgs/msg/twist_stamped.hpp" #include "pcl/point_types.h" @@ -36,15 +37,14 @@ class AdaptiveCruiseController rclcpp::Node * node, const double vehicle_width, const double vehicle_length, const double wheel_base, const double front_overhang); - void insertAdaptiveCruiseVelocity( + std::tuple insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, - bool & need_to_stop, - autoware_planning_msgs::msg::Trajectory & output_trajectory); + const autoware_planning_msgs::msg::Trajectory & input_trajectory); private: rclcpp::Publisher::SharedPtr pub_debug_; @@ -180,14 +180,15 @@ class AdaptiveCruiseController const rclcpp::Time & nearest_collision_point_time); double calcTrajYaw( const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx); - bool estimatePointVelocityFromObject( + std::tuple estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const pcl::PointXYZ & nearest_collision_point, double * velocity); - bool estimatePointVelocityFromPcl( + const pcl::PointXYZ & nearest_collision_point, + const double old_velocity); + std::tuple estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * velocity); - double estimateRoughPointVelocity(double current_vel); + const rclcpp::Time & nearest_collision_point_time, const double old_velocity); + double estimateRoughPointVelocity(const double current_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel); double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel); @@ -197,10 +198,10 @@ class AdaptiveCruiseController double calcTargetVelocityByPID( const double current_vel, const double current_dist, const double obj_vel); - void insertMaxVelocityToPath( + autoware_planning_msgs::msg::Trajectory insertMaxVelocityToPath( const geometry_msgs::msg::Pose & self_pose, const double current_vel, const double target_vel, const double dist_to_collision_point, - autoware_planning_msgs::msg::Trajectory & output_trajectory); + const autoware_planning_msgs::msg::Trajectory & output_trajectory); void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); /* Debug */ diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 033b3dc3..e0e44cc8 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "boost/algorithm/clamp.hpp" #include "boost/assert.hpp" @@ -102,20 +103,23 @@ AdaptiveCruiseController::AdaptiveCruiseController( 1); } -void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( +std::tuple +AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, - const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, bool & need_to_stop, - autoware_planning_msgs::msg::Trajectory & output_trajectory) + const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, + const autoware_planning_msgs::msg::Trajectory & input_trajectory) { debug_values_.data.clear(); debug_values_.data.resize(num_debug_values_, 0.0); const double current_velocity = current_velocity_ptr->twist.linear.x; - double point_velocity; + double point_velocity = current_velocity; bool success_estm_vel = false; + auto output_trajectory = input_trajectory; + /* * calc distance to collision point */ @@ -132,13 +136,13 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( * estimate velocity of collision point */ if (param_.use_pcl_to_est_vel) { - success_estm_vel = estimatePointVelocityFromPcl( - traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity); + std::tie(success_estm_vel, point_velocity) = estimatePointVelocityFromPcl( + traj_yaw, nearest_collision_point, nearest_collision_point_time, point_velocity); } if (param_.use_object_to_est_vel) { - success_estm_vel = estimatePointVelocityFromObject( - object_ptr, traj_yaw, nearest_collision_point, &point_velocity); + std::tie(success_estm_vel, point_velocity) = estimatePointVelocityFromObject( + object_ptr, traj_yaw, nearest_collision_point, point_velocity); } if (param_.use_rough_est_vel && !success_estm_vel) { @@ -151,11 +155,10 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "Failed to estimate velocity of forward vehicle. Insert stop line."); - need_to_stop = true; prev_upper_velocity_ = current_velocity; // reset prev_upper_velocity prev_target_velocity_ = 0.0; pub_debug_->publish(debug_values_); - return; + return std::forward_as_tuple(true, output_trajectory); } // calculate max(target) velocity of self @@ -168,8 +171,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), "Upper velocity is too low. Insert stop line."); - need_to_stop = true; - return; + return std::forward_as_tuple(true, output_trajectory); } /* @@ -177,7 +179,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( */ insertMaxVelocityToPath( self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory); - need_to_stop = false; + return std::forward_as_tuple(false, output_trajectory); } double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( @@ -250,10 +252,11 @@ double AdaptiveCruiseController::calcTrajYaw( return tf2::getYaw(trajectory.points.at(collision_point_idx).pose.orientation); } -bool AdaptiveCruiseController::estimatePointVelocityFromObject( +std::tuple AdaptiveCruiseController::estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const pcl::PointXYZ & nearest_collision_point, double * velocity) + const pcl::PointXYZ & nearest_collision_point, + const double old_velocity) { geometry_msgs::msg::Point nearest_collision_p_ros; nearest_collision_p_ros.x = nearest_collision_point.x; @@ -278,17 +281,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( } if (get_obj) { - *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); - debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; - return true; + const auto velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = velocity; + return std::forward_as_tuple(true, velocity); } else { - return false; + return std::forward_as_tuple(false, old_velocity); } } -bool AdaptiveCruiseController::estimatePointVelocityFromPcl( +std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, - const rclcpp::Time & nearest_collision_point_time, double * velocity) + const rclcpp::Time & nearest_collision_point_time, const double old_velocity) { geometry_msgs::msg::Point nearest_collision_p_ros; nearest_collision_p_ros.x = nearest_collision_point.x; @@ -306,7 +309,7 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_time_ = nearest_collision_point_time; prev_collision_point_ = nearest_collision_point; prev_collision_point_valid_ = true; - return false; + return std::forward_as_tuple(false, old_velocity); } const double p_dx = nearest_collision_point.x - prev_collision_point_.x; const double p_dy = nearest_collision_point.y - prev_collision_point_.y; @@ -320,7 +323,7 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_ = nearest_collision_point; prev_collision_point_valid_ = true; est_vel_que_.clear(); - return false; + return std::forward_as_tuple(false, old_velocity); } // append new velocity and remove old velocity from que @@ -328,17 +331,17 @@ bool AdaptiveCruiseController::estimatePointVelocityFromPcl( } // calc average(median) velocity from que - *velocity = getMedianVel(est_vel_que_); - debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = *velocity; + const auto velocity = getMedianVel(est_vel_que_); + debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = velocity; prev_collision_point_time_ = nearest_collision_point_time; prev_collision_point_ = nearest_collision_point; - prev_target_velocity_ = *velocity; + prev_target_velocity_ = velocity; prev_collision_point_valid_ = true; - return true; + return std::forward_as_tuple(true, velocity); } -double AdaptiveCruiseController::estimateRoughPointVelocity(double current_vel) +double AdaptiveCruiseController::estimateRoughPointVelocity(const double current_vel) { const double p_dt = node_->now().seconds() - prev_collision_point_time_.seconds(); if (param_.valid_est_vel_diff_time >= p_dt) { @@ -482,11 +485,14 @@ double AdaptiveCruiseController::calcTargetVelocityByPID( return target_vel; } -void AdaptiveCruiseController::insertMaxVelocityToPath( +autoware_planning_msgs::msg::Trajectory +AdaptiveCruiseController::insertMaxVelocityToPath( const geometry_msgs::msg::Pose & self_pose, const double current_vel, const double target_vel, - const double dist_to_collision_point, autoware_planning_msgs::msg::Trajectory & output_trajectory) + const double dist_to_collision_point, + const autoware_planning_msgs::msg::Trajectory & input_trajectory) { // plus distance from self to next nearest point + auto output_trajectory = input_trajectory; double dist = dist_to_collision_point; double dist_to_first_point = 0.0; if (output_trajectory.points.size() > 1) { @@ -535,6 +541,7 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( pre_vel = next_pre_vel; } } + return output_trajectory; } void AdaptiveCruiseController::registerQueToVelocity( diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 7de53b44..e16aa857 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -299,13 +299,13 @@ void ObstacleStopPlannerNode::pathCallback( */ bool need_to_stop = is_collision; if (is_collision) { - acc_controller_->insertAdaptiveCruiseVelocity( + std::tie(need_to_stop, output_msg) = acc_controller_->insertAdaptiveCruiseVelocity( decimate_trajectory_map.decimate_trajectory, decimate_trajectory_collision_index, self_pose, nearest_collision_point, nearest_collision_point_time, object_ptr_, current_velocity_ptr_, - need_to_stop, output_msg); + output_msg); } /* From 0a648fa26f51e1463b30f8da3e5c9241e89cf62e Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 9 Mar 2021 14:51:12 +0900 Subject: [PATCH 37/62] Remove unused line --- .../include/obstacle_stop_planner/util.hpp | 6 +----- .../src/adaptive_cruise_control.cpp | 6 +----- obstacle_stop_planner_refine/src/node.cpp | 1 - 3 files changed, 2 insertions(+), 11 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 682b1b40..8fa1d8f9 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -61,12 +61,8 @@ inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quatern { tf2::Quaternion quat(q.x, q.y, q.z, q.w); tf2::Matrix3x3 mat(quat); - double roll, pitch, yaw; - mat.getRPY(roll, pitch, yaw); geometry_msgs::msg::Vector3 rpy; - rpy.x = roll; - rpy.y = pitch; - rpy.z = yaw; + mat.getRPY(rpy.x, rpy.y, rpy.z); return rpy; } diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index e0e44cc8..7ba512d0 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -454,8 +454,6 @@ double AdaptiveCruiseController::calcTargetVelocity_D( double add_vel_d = 0; add_vel_d = diff_vel; - if (add_vel_d >= 0) {diff_vel *= param_.d_coeff_pos;} - if (add_vel_d < 0) {diff_vel *= param_.d_coeff_neg;} add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm); // add buffer @@ -493,14 +491,12 @@ AdaptiveCruiseController::insertMaxVelocityToPath( { // plus distance from self to next nearest point auto output_trajectory = input_trajectory; - double dist = dist_to_collision_point; double dist_to_first_point = 0.0; if (output_trajectory.points.size() > 1) { dist_to_first_point = boost::geometry::distance( autoware_utils::fromMsg(self_pose.position).to_2d(), autoware_utils::fromMsg(output_trajectory.points.at(1).pose.position).to_2d()); } - dist += dist_to_first_point; double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; // accel = (v_after^2 - v_before^2 ) / 2x @@ -578,7 +574,7 @@ double AdaptiveCruiseController::getMedianVel( } std::vector raw_vel_que; - for (const auto vel : vel_que) { + for (const auto & vel : vel_que) { raw_vel_que.emplace_back(vel.twist.linear.x); } diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index e16aa857..be52bf7f 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -500,7 +500,6 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( } if (!is_slow_down_end) { slow_down_end_trajectory_point = output_path.points.back(); - is_slow_down_end = true; } debug_ptr_->pushPose(slow_down_end_trajectory_point.pose, PoseType::SlowDownEnd); } From d471f073df064e04385f32a8ebd683a8b25a590d Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 9 Mar 2021 18:17:00 +0900 Subject: [PATCH 38/62] refactor input/output --- .../include/obstacle_stop_planner/node.hpp | 28 ++++--- .../obstacle_stop_planner/point_helper.hpp | 11 ++- .../include/obstacle_stop_planner/util.hpp | 25 +++--- .../src/adaptive_cruise_control.cpp | 19 +---- obstacle_stop_planner_refine/src/node.cpp | 78 +++++++++++-------- .../src/obstacle_point_cloud.cpp | 4 +- .../src/point_helper.cpp | 37 ++++++--- .../src/trajectory.cpp | 18 +++-- 8 files changed, 121 insertions(+), 99 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 17cccd64..1f639823 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" @@ -85,39 +86,42 @@ class ObstacleStopPlannerNode : public rclcpp::Node private: geometry_msgs::msg::Pose getSelfPose( const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer); - void insertSlowDownVelocity( - const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, - autoware_planning_msgs::msg::Trajectory & output_path); + autoware_planning_msgs::msg::Trajectory insertSlowDownVelocity( + const size_t slow_down_start_point_idx, + const double slow_down_target_vel, + const double slow_down_vel, + const autoware_planning_msgs::msg::Trajectory & input_path); double calcSlowDownTargetVel(const double lateral_deviation); - void getSlowDownPointcloud( + std::tuple::Ptr> getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, const Point2d & prev_center_point, const Point2d & next_center_point, const double search_radius, const Polygon2d & one_step_polygon, - pcl::PointCloud::Ptr slow_down_pointcloud, bool & candidate_slow_down); - void getCollisionPointcloud( + const pcl::PointCloud::Ptr slow_down_pointcloud, + const bool candidate_slow_down); + std::tuple::Ptr> getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, const Point2d & prev_center_point, const Point2d & next_center_point, const double search_radius, const Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, - pcl::PointCloud::Ptr collision_pointcloud, - bool & is_collision); - void insertStopPoint( + const pcl::PointCloud::Ptr collision_pointcloud, + const bool is_collision); + autoware_planning_msgs::msg::Trajectory insertStopPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_collision_point, const PointHelper & point_helper, - autoware_planning_msgs::msg::Trajectory & output_msg); - void insertSlowDownPoint( + const autoware_planning_msgs::msg::Trajectory & input_msg); + autoware_planning_msgs::msg::Trajectory insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_slow_down_point, const PointHelper & point_helper, const double slow_down_target_vel, const double slow_down_margin, - autoware_planning_msgs::msg::Trajectory & output_msg); + const autoware_planning_msgs::msg::Trajectory & output_msg); }; } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index 348f9d65..ebdb603a 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -16,6 +16,7 @@ #define OBSTACLE_STOP_PLANNER__POINT_HELPER_HPP_ #include +#include #include "rclcpp/rclcpp.hpp" #include "pcl/point_cloud.h" @@ -74,9 +75,10 @@ class PointHelper const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose) const; - autoware_planning_msgs::msg::TrajectoryPoint insertStopPoint( + std::tuple + insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) const; + const autoware_planning_msgs::msg::Trajectory & input_path) const; StopPoint searchInsertPoint( const int idx, const autoware_planning_msgs::msg::Trajectory & base_path, @@ -93,10 +95,11 @@ class PointHelper const autoware_planning_msgs::msg::Trajectory & base_path, const double current_velocity_x) const; - autoware_planning_msgs::msg::TrajectoryPoint insertSlowDownStartPoint( + std::tuple + insertSlowDownStartPoint( const SlowDownPoint & slow_down_start_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) const; + const autoware_planning_msgs::msg::Trajectory & input_path) const; autoware_planning_msgs::msg::TrajectoryPoint getExtendTrajectoryPoint( double extend_distance, const autoware_planning_msgs::msg::TrajectoryPoint & goal_point) const; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp index 8fa1d8f9..f6a7b5cb 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/util.hpp @@ -32,13 +32,19 @@ using autoware_utils::Polygon3d; namespace { -inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) +inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) { - tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); - double roll, pitch, yaw; - tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw); + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + tf2::Matrix3x3 mat(quat); + geometry_msgs::msg::Vector3 rpy; + mat.getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} - return yaw; +inline double getYawFromQuaternion(const geometry_msgs::msg::Quaternion & quat) +{ + const auto rpy = rpyFromQuat(quat); + return rpy.z; } inline geometry_msgs::msg::Pose getVehicleCenterFromBase( @@ -57,15 +63,6 @@ inline geometry_msgs::msg::Pose getVehicleCenterFromBase( return center_pose; } -inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q) -{ - tf2::Quaternion quat(q.x, q.y, q.z, q.w); - tf2::Matrix3x3 mat(quat); - geometry_msgs::msg::Vector3 rpy; - mat.getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} - inline Polygon2d getPolygon( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0) diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 7ba512d0..8690ffef 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -258,16 +258,11 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromObje const pcl::PointXYZ & nearest_collision_point, const double old_velocity) { - geometry_msgs::msg::Point nearest_collision_p_ros; - nearest_collision_p_ros.x = nearest_collision_point.x; - nearest_collision_p_ros.y = nearest_collision_point.y; - nearest_collision_p_ros.z = nearest_collision_point.z; - /* get object velocity, and current yaw */ bool get_obj = false; double obj_vel; double obj_yaw; - const auto collision_point_2d = autoware_utils::fromMsg(nearest_collision_p_ros).to_2d(); + const Point2d collision_point_2d {nearest_collision_point.x, nearest_collision_point.y}; for (const auto & obj : object_ptr->objects) { const auto obj_poly = getPolygon( obj.state.pose_covariance.pose, obj.shape.dimensions, 0.0, @@ -293,11 +288,6 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, const double old_velocity) { - geometry_msgs::msg::Point nearest_collision_p_ros; - nearest_collision_p_ros.x = nearest_collision_point.x; - nearest_collision_p_ros.y = nearest_collision_point.y; - nearest_collision_p_ros.z = nearest_collision_point.z; - /* estimate velocity */ const double p_dt = nearest_collision_point_time.seconds() - prev_collision_point_time_.seconds(); @@ -491,13 +481,6 @@ AdaptiveCruiseController::insertMaxVelocityToPath( { // plus distance from self to next nearest point auto output_trajectory = input_trajectory; - double dist_to_first_point = 0.0; - if (output_trajectory.points.size() > 1) { - dist_to_first_point = boost::geometry::distance( - autoware_utils::fromMsg(self_pose.position).to_2d(), - autoware_utils::fromMsg(output_trajectory.points.at(1).pose.position).to_2d()); - } - double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; // accel = (v_after^2 - v_before^2 ) / 2x double target_acc = (std::pow(target_vel, 2) - std::pow(current_vel, 2)) / (2 * margin_to_insert); diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index be52bf7f..d58a78f3 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "autoware_utils/geometry/geometry.hpp" #include "diagnostic_msgs/msg/key_value.hpp" @@ -253,13 +254,13 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - getSlowDownPointcloud( + std::tie(candidate_slow_down, slow_down_pointcloud_ptr) = getSlowDownPointcloud( is_slow_down, param_.enable_slow_down, obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, param_.slow_down_search_radius, move_slow_down_range_polygon, slow_down_pointcloud_ptr, candidate_slow_down); - getCollisionPointcloud( + std::tie(is_collision, collision_pointcloud_ptr) = getCollisionPointcloud( slow_down_pointcloud_ptr, prev_center_point, next_center_point, param_.stop_search_radius, move_vehicle_polygon, trajectory.points.at( i), collision_pointcloud_ptr, is_collision); @@ -312,7 +313,7 @@ void ObstacleStopPlannerNode::pathCallback( * insert stop point */ if (need_to_stop) { - insertStopPoint( + output_msg = insertStopPoint( decimate_trajectory_map.index_map.at(decimate_trajectory_collision_index) + trajectory_trim_index, base_path, @@ -325,7 +326,7 @@ void ObstacleStopPlannerNode::pathCallback( * insert slow_down point */ if (is_slow_down) { - insertSlowDownPoint( + output_msg = insertSlowDownPoint( decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, @@ -339,16 +340,20 @@ void ObstacleStopPlannerNode::pathCallback( } // collision -void ObstacleStopPlannerNode::getCollisionPointcloud( +std::tuple::Ptr> +ObstacleStopPlannerNode::getCollisionPointcloud( const pcl::PointCloud::Ptr slow_down_pointcloud, const Point2d & prev_center_point, const Point2d & next_center_point, const double search_radius, const Polygon2d & one_step_polygon, const autoware_planning_msgs::msg::TrajectoryPoint & trajectory_point, - pcl::PointCloud::Ptr collision_pointcloud, - bool & is_collision) + const pcl::PointCloud::Ptr collision_pointcloud, + const bool is_collision) { + auto output_pointcloud = collision_pointcloud; + auto output_collision = is_collision; + for (size_t j = 0; j < slow_down_pointcloud->size(); ++j) { Point2d point(slow_down_pointcloud->at(j).x, slow_down_pointcloud->at(j).y); if ( @@ -356,18 +361,20 @@ void ObstacleStopPlannerNode::getCollisionPointcloud( boost::geometry::distance(next_center_point, point) < search_radius) { if (boost::geometry::within(point, one_step_polygon)) { - collision_pointcloud->push_back(slow_down_pointcloud->at(j)); - is_collision = true; + output_pointcloud->push_back(slow_down_pointcloud->at(j)); + output_collision = true; debug_ptr_->pushPolygon( one_step_polygon, trajectory_point.pose.position.z, PolygonType::Collision); } } } + return std::make_tuple(output_collision, output_pointcloud); } // slow down -void ObstacleStopPlannerNode::getSlowDownPointcloud( +std::tuple::Ptr> +ObstacleStopPlannerNode::getSlowDownPointcloud( const bool is_slow_down, const bool enable_slow_down, const pcl::PointCloud::Ptr obstacle_candidate_pointcloud, @@ -375,9 +382,12 @@ void ObstacleStopPlannerNode::getSlowDownPointcloud( const Point2d & next_center_point, const double search_radius, const Polygon2d & boost_polygon, - pcl::PointCloud::Ptr slow_down_pointcloud, - bool & candidate_slow_down) + const pcl::PointCloud::Ptr slow_down_pointcloud, + const bool candidate_slow_down) { + auto output_pointcloud = slow_down_pointcloud; + auto output_candidate = candidate_slow_down; + if (!is_slow_down && enable_slow_down) { for (size_t j = 0; j < obstacle_candidate_pointcloud->size(); ++j) { Point2d point( @@ -387,23 +397,26 @@ void ObstacleStopPlannerNode::getSlowDownPointcloud( boost::geometry::distance(next_center_point, point) < search_radius) { if (boost::geometry::within(point, boost_polygon)) { - slow_down_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); - candidate_slow_down = true; + output_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); + output_candidate = true; } } } } else { - slow_down_pointcloud = obstacle_candidate_pointcloud; + output_pointcloud = obstacle_candidate_pointcloud; } + return std::make_tuple(output_candidate, output_pointcloud); } -void ObstacleStopPlannerNode::insertSlowDownPoint( +autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_slow_down_point, const PointHelper & point_helper, const double slow_down_target_vel, const double slow_down_margin, - autoware_planning_msgs::msg::Trajectory & output_msg) + const autoware_planning_msgs::msg::Trajectory & input_msg) { + auto output_msg = input_msg; + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { const double yaw = getYawFromQuaternion(base_path.points.at(i).pose.orientation); @@ -421,25 +434,30 @@ void ObstacleStopPlannerNode::insertSlowDownPoint( base_path, current_velocity_ptr_->twist.linear.x); if (slow_down_start_point.index <= output_msg.points.size()) { - const auto slowdown_trajectory_point = point_helper.insertSlowDownStartPoint( + autoware_planning_msgs::msg::TrajectoryPoint slowdown_trajectory_point; + std::tie(slowdown_trajectory_point, output_msg) = + point_helper.insertSlowDownStartPoint( slow_down_start_point, base_path, output_msg); debug_ptr_->pushPose(slowdown_trajectory_point.pose, PoseType::SlowDownStart); - insertSlowDownVelocity( + output_msg = insertSlowDownVelocity( slow_down_start_point.index, slow_down_target_vel, slow_down_start_point.velocity, output_msg); } break; } } + return output_msg; } // stop -void ObstacleStopPlannerNode::insertStopPoint( +autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertStopPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_collision_point, const PointHelper & point_helper, - autoware_planning_msgs::msg::Trajectory & output_msg) + const autoware_planning_msgs::msg::Trajectory & input_msg) { + auto output_msg = input_msg; + for (size_t i = search_start_index; i < base_path.points.size(); ++i) { const double yaw = getYawFromQuaternion(base_path.points.at(i).pose.orientation); @@ -455,13 +473,15 @@ void ObstacleStopPlannerNode::insertStopPoint( const auto stop_point = point_helper.searchInsertPoint(i, base_path, trajectory_vec, collision_point_vec); if (stop_point.index <= output_msg.points.size()) { - const auto trajectory_point = + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point; + std::tie(trajectory_point, output_msg) = point_helper.insertStopPoint(stop_point, base_path, output_msg); debug_ptr_->pushPose(trajectory_point.pose, PoseType::Stop); } break; } } + return output_msg; } void ObstacleStopPlannerNode::externalExpandStopRangeCallback( @@ -474,11 +494,12 @@ void ObstacleStopPlannerNode::externalExpandStopRangeCallback( param_.vehicle_info.vehicle_length / 2.0); } -void ObstacleStopPlannerNode::insertSlowDownVelocity( +autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownVelocity( const size_t slow_down_start_point_idx, const double slow_down_target_vel, double slow_down_vel, - autoware_planning_msgs::msg::Trajectory & output_path) + const autoware_planning_msgs::msg::Trajectory & input_path) { autoware_planning_msgs::msg::TrajectoryPoint slow_down_end_trajectory_point; + auto output_path = input_path; bool is_slow_down_end = false; for (size_t j = slow_down_start_point_idx; j < output_path.points.size() - 1; ++j) { @@ -502,6 +523,7 @@ void ObstacleStopPlannerNode::insertSlowDownVelocity( slow_down_end_trajectory_point = output_path.points.back(); } debug_ptr_->pushPose(slow_down_end_trajectory_point.pose, PoseType::SlowDownEnd); + return output_path; } double ObstacleStopPlannerNode::calcSlowDownTargetVel(const double lateral_deviation) @@ -534,13 +556,7 @@ geometry_msgs::msg::Pose ObstacleStopPlannerNode::getSelfPose( tf_buffer.lookupTransform( header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds( 0.1)); - self_pose.position.x = transform.transform.translation.x; - self_pose.position.y = transform.transform.translation.y; - self_pose.position.z = transform.transform.translation.z; - self_pose.orientation.x = transform.transform.rotation.x; - self_pose.orientation.y = transform.transform.rotation.y; - self_pose.orientation.z = transform.transform.rotation.z; - self_pose.orientation.w = transform.transform.rotation.w; + self_pose = autoware_utils::transform2pose(transform.transform); } catch (tf2::TransformException & ex) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index df2cc7d8..41dfc051 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -83,7 +83,8 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( pcl::PointCloud::Ptr transformed_obstacle_pointcloud_ptr( new pcl::PointCloud); pcl::transformPointCloud( - *obstacle_pointcloud_pcl_ptr_, *transformed_obstacle_pointcloud_ptr, + *obstacle_pointcloud_pcl_ptr_, + *transformed_obstacle_pointcloud_ptr, affine_matrix); // search obstacle candidate pointcloud to reduce calculation cost @@ -107,6 +108,7 @@ bool ObstaclePointCloud::searchPointcloudNearTrajectory( trajectory_point.pose, param.vehicle_info.vehicle_length, param.vehicle_info.rear_overhang); + for (const auto & point : input_pointcloud_ptr->points) { const double x = center_pose.position.x - point.x; const double y = center_pose.position.y - point.y; diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 386f524f..852dabf0 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -14,6 +14,8 @@ #include #include +#include + #include "obstacle_stop_planner/point_helper.hpp" #include "pcl_conversions/pcl_conversions.h" @@ -33,22 +35,27 @@ PointStamped PointHelper::getNearestPoint( const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose) const { - double min_norm = 0.0; - bool is_init = false; const double yaw = getYawFromQuaternion(base_pose.orientation); Point2d base_pose_vec {std::cos(yaw), std::sin(yaw)}; + + Point2d pointcloud_vec { + pointcloud.at(0).x - base_pose.position.x, + pointcloud.at(0).y - base_pose.position.y}; + double min_norm = base_pose_vec.dot(pointcloud_vec); PointStamped nearest_collision_point; + nearest_collision_point.point = pointcloud.at(0); + nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; - for (size_t i = 0; i < pointcloud.size(); ++i) { + for (size_t i = 1; i < pointcloud.size(); ++i) { Point2d pointcloud_vec { pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y}; double norm = base_pose_vec.dot(pointcloud_vec); - if (norm < min_norm || !is_init) { + + if (norm < min_norm) { min_norm = norm; nearest_collision_point.point = pointcloud.at(i); nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; - is_init = true; } } return nearest_collision_point; @@ -67,6 +74,7 @@ PointDeviation PointHelper::getLateralNearestPoint( Point2d pointcloud_vec { pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y}; + double norm = std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); if (norm < min_norm) { @@ -78,10 +86,12 @@ PointDeviation PointHelper::getLateralNearestPoint( return lateral_nearest_point; } -autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( +std::tuple +PointHelper::insertStopPoint( const StopPoint & stop_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) const + const autoware_planning_msgs::msg::Trajectory & input_path) const { + auto output_path = input_path; autoware_planning_msgs::msg::TrajectoryPoint stop_trajectory_point = base_path.points.at(std::max(static_cast(stop_point.index) - 1, 0)); stop_trajectory_point.pose.position.x = stop_point.point.x(); @@ -91,7 +101,7 @@ autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertStopPoint( for (size_t j = stop_point.index; j < output_path.points.size(); ++j) { output_path.points.at(j).twist.linear.x = 0.0; } - return stop_trajectory_point; + return std::make_tuple(stop_trajectory_point, output_path); } StopPoint PointHelper::searchInsertPoint( @@ -198,15 +208,18 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( return slow_down_point; } -autoware_planning_msgs::msg::TrajectoryPoint PointHelper::insertSlowDownStartPoint( +std::tuple +PointHelper::insertSlowDownStartPoint( const SlowDownPoint & slow_down_start_point, const autoware_planning_msgs::msg::Trajectory & base_path, - autoware_planning_msgs::msg::Trajectory & output_path) const + const autoware_planning_msgs::msg::Trajectory & input_path) const { + auto output_path = input_path; + autoware_planning_msgs::msg::TrajectoryPoint slow_down_start_trajectory_point = base_path.points.at(std::max(static_cast(slow_down_start_point.index) - 1, 0)); - slow_down_start_trajectory_point.pose.position.x = slow_down_start_point.point.x(); - slow_down_start_trajectory_point.pose.position.y = slow_down_start_point.point.y(); + slow_down_start_trajectory_point.pose.position = + autoware_utils::toMsg(slow_down_start_point.point.to_3d()); slow_down_start_trajectory_point.twist.linear.x = slow_down_start_point.velocity; constexpr double epsilon = 0.001; const auto & insert_target_point = output_path.points.at(slow_down_start_point.index); diff --git a/obstacle_stop_planner_refine/src/trajectory.cpp b/obstacle_stop_planner_refine/src/trajectory.cpp index b1a0d4fd..740fee8c 100644 --- a/obstacle_stop_planner_refine/src/trajectory.cpp +++ b/obstacle_stop_planner_refine/src/trajectory.cpp @@ -43,10 +43,12 @@ DecimateTrajectoryMap decimateTrajectory( Point2d interpolated_point = point_helper.getBackwardPointFromBasePoint( line_start_point, line_end_point, line_end_point, -1.0 * (trajectory_length_sum - next_length)); + autoware_planning_msgs::msg::TrajectoryPoint trajectory_point = input_trajectory.points.at(i); trajectory_point.pose.position = autoware_utils::toMsg( interpolated_point.to_3d(input_trajectory.points.at(i).pose.position.z)); output.decimate_trajectory.points.emplace_back(trajectory_point); + output.index_map.insert( std::make_pair(output.decimate_trajectory.points.size() - 1, size_t(i))); next_length += step_length; @@ -73,20 +75,22 @@ trimTrajectoryWithIndexFromSelfPose( const geometry_msgs::msg::Pose & self_pose) { autoware_planning_msgs::msg::Trajectory output_trajectory; - double min_distance = 0.0; + size_t min_distance_index = 0; - bool is_init = false; - for (size_t i = 0; i < input_trajectory.points.size(); ++i) { + const auto self_point = autoware_utils::fromMsg(self_pose.position); + const auto input_point = autoware_utils::fromMsg(input_trajectory.points.at(0).pose.position); + double min_distance = boost::geometry::distance(input_point.to_2d(), self_point.to_2d()); + + for (size_t i = 1; i < input_trajectory.points.size(); ++i) { const auto p1 = autoware_utils::fromMsg(input_trajectory.points.at(i).pose.position); - const auto p2 = autoware_utils::fromMsg(self_pose.position); - const double point_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + const double point_distance = boost::geometry::distance(p1.to_2d(), self_point.to_2d()); - if (!is_init || point_distance < min_distance) { - is_init = true; + if (point_distance < min_distance) { min_distance = point_distance; min_distance_index = i; } } + for (size_t i = min_distance_index; i < input_trajectory.points.size(); ++i) { output_trajectory.points.emplace_back(input_trajectory.points.at(i)); } From 6887e2f922d53501b08c53f3f2a79fffa471c95b Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 9 Mar 2021 18:17:09 +0900 Subject: [PATCH 39/62] Add test --- ...st_one_step_polygon.cpp => gtest_main.cpp} | 17 ---------- .../src/test_create_vehicle_footprint.cpp | 34 +++++++++++++++++++ .../test/src/test_util.cpp | 26 ++++++++++++++ 3 files changed, 60 insertions(+), 17 deletions(-) rename obstacle_stop_planner_refine/test/src/{test_one_step_polygon.cpp => gtest_main.cpp} (75%) create mode 100644 obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp create mode 100644 obstacle_stop_planner_refine/test/src/test_util.cpp diff --git a/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp b/obstacle_stop_planner_refine/test/src/gtest_main.cpp similarity index 75% rename from obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp rename to obstacle_stop_planner_refine/test/src/gtest_main.cpp index 9151315a..e45b3785 100644 --- a/obstacle_stop_planner_refine/test/src/test_one_step_polygon.cpp +++ b/obstacle_stop_planner_refine/test/src/gtest_main.cpp @@ -13,23 +13,6 @@ // limitations under the License. #include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" - -class TestSuite : public ::testing::Test -{ -protected: - void SetUp() - { - rclcpp::init(0, nullptr); - } - void TearDown() - { - (void)rclcpp::shutdown(); - } -}; // sanity_check - -// write test here - int32_t main(int32_t argc, char ** argv) { diff --git a/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp b/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp new file mode 100644 index 00000000..ae371120 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_create_vehicle_footprint.cpp @@ -0,0 +1,34 @@ +// Copyright 2021 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 "gtest/gtest.h" +#include "obstacle_stop_planner/util/create_vehicle_footprint.hpp" + +namespace obstacle_stop_planner +{ +TEST(createVehicleFootprint, returnValue) { + VehicleInfo vehicle_info; + vehicle_info.front_overhang = 0.5; + vehicle_info.rear_overhang = 0.3; + vehicle_info.wheel_base = 2.0; + vehicle_info.wheel_tread = 0.1; + vehicle_info.left_overhang = 0.2; + vehicle_info.right_overhang = 0.2; + + const auto ring2d = createVehicleFootprint(vehicle_info); + EXPECT_EQ(ring2d.size(), 5); + EXPECT_EQ(ring2d.at(0).x(), 2.5); + EXPECT_EQ(ring2d.at(0).y(), 0.25); +} +} // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/test/src/test_util.cpp b/obstacle_stop_planner_refine/test/src/test_util.cpp new file mode 100644 index 00000000..5cb11c15 --- /dev/null +++ b/obstacle_stop_planner_refine/test/src/test_util.cpp @@ -0,0 +1,26 @@ +// Copyright 2021 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 "gtest/gtest.h" +#include "obstacle_stop_planner/util.hpp" + +namespace obstacle_stop_planner +{ +TEST(getYawFromQuaternion, returnValue) { + geometry_msgs::msg::Quaternion quat; + EXPECT_EQ(0.0, getYawFromQuaternion(quat)); +} + + +} // namespace obstacle_stop_planner From 3852167824ef784b72c05c7a04b91630ee23385b Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Tue, 9 Mar 2021 19:28:34 +0900 Subject: [PATCH 40/62] Refactoring --- .../include/obstacle_stop_planner/node.hpp | 3 +- obstacle_stop_planner_refine/src/node.cpp | 38 ++++++++++--------- .../src/point_helper.cpp | 20 +++------- 3 files changed, 26 insertions(+), 35 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index 1f639823..d2bc4632 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -113,13 +113,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_collision_point, - const PointHelper & point_helper, const autoware_planning_msgs::msg::Trajectory & input_msg); autoware_planning_msgs::msg::Trajectory insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, const pcl::PointXYZ & nearest_slow_down_point, - const PointHelper & point_helper, const double slow_down_target_vel, + const double slow_down_target_vel, const double slow_down_margin, const autoware_planning_msgs::msg::Trajectory & output_msg); }; diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index d58a78f3..8cdc57d0 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -212,26 +212,25 @@ void ObstacleStopPlannerNode::pathCallback( * create one step circle center for vehicle */ const auto prev_center_pose = getVehicleCenterFromBase( - trajectory.points.at( - i).pose, + trajectory.points.at(i).pose, param_.vehicle_info.vehicle_length, param_.vehicle_info.rear_overhang); - Point2d prev_center_point( - prev_center_pose.position.x, - prev_center_pose.position.y); const auto next_center_pose = getVehicleCenterFromBase( trajectory.points.at(i + 1).pose, param_.vehicle_info.vehicle_length, param_.vehicle_info.rear_overhang); - Point2d next_center_point( - next_center_pose.position.x, - next_center_pose.position.y); + + Point2d prev_center_point = autoware_utils::fromMsg(prev_center_pose.position).to_2d(); + Point2d next_center_point = autoware_utils::fromMsg(next_center_pose.position).to_2d(); + /* * create one step polygon for vehicle */ const auto move_vehicle_polygon = createOneStepPolygon( - trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose, - param_.expand_stop_range, param_.vehicle_info); + trajectory.points.at(i).pose, + trajectory.points.at(i + 1).pose, + param_.expand_stop_range, + param_.vehicle_info); debug_ptr_->pushPolygon( move_vehicle_polygon, trajectory.points.at(i).pose.position.z, @@ -254,6 +253,7 @@ void ObstacleStopPlannerNode::pathCallback( pcl::PointCloud::Ptr collision_pointcloud_ptr( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + std::tie(candidate_slow_down, slow_down_pointcloud_ptr) = getSlowDownPointcloud( is_slow_down, param_.enable_slow_down, obstacle_candidate_pointcloud_ptr, prev_center_point, next_center_point, @@ -262,8 +262,8 @@ void ObstacleStopPlannerNode::pathCallback( std::tie(is_collision, collision_pointcloud_ptr) = getCollisionPointcloud( slow_down_pointcloud_ptr, prev_center_point, next_center_point, - param_.stop_search_radius, move_vehicle_polygon, trajectory.points.at( - i), collision_pointcloud_ptr, is_collision); + param_.stop_search_radius, move_vehicle_polygon, + trajectory.points.at(i), collision_pointcloud_ptr, is_collision); if (candidate_slow_down && !is_collision && !is_slow_down) { is_slow_down = true; @@ -271,10 +271,12 @@ void ObstacleStopPlannerNode::pathCallback( debug_ptr_->pushPolygon( move_slow_down_range_polygon, trajectory.points.at(i).pose.position.z, PolygonType::SlowDown); + const auto nearest_slow_down_pointstamped = point_helper.getNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); nearest_slow_down_point = nearest_slow_down_pointstamped.point; nearest_collision_point_time = nearest_slow_down_pointstamped.time; + const auto lateral_nearest_slow_down_point = point_helper.getLateralNearestPoint( *slow_down_pointcloud_ptr, trajectory.points.at(i).pose); lateral_deviation = lateral_nearest_slow_down_point.deviation; @@ -318,7 +320,6 @@ void ObstacleStopPlannerNode::pathCallback( trajectory_trim_index, base_path, nearest_collision_point, - point_helper, output_msg); } @@ -330,7 +331,6 @@ void ObstacleStopPlannerNode::pathCallback( decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), base_path, nearest_slow_down_point, - point_helper, calcSlowDownTargetVel(lateral_deviation), param_.slow_down_margin, output_msg); @@ -381,7 +381,7 @@ ObstacleStopPlannerNode::getSlowDownPointcloud( const Point2d & prev_center_point, const Point2d & next_center_point, const double search_radius, - const Polygon2d & boost_polygon, + const Polygon2d & one_step_polygon, const pcl::PointCloud::Ptr slow_down_pointcloud, const bool candidate_slow_down) { @@ -396,7 +396,7 @@ ObstacleStopPlannerNode::getSlowDownPointcloud( boost::geometry::distance(prev_center_point, point) < search_radius || boost::geometry::distance(next_center_point, point) < search_radius) { - if (boost::geometry::within(point, boost_polygon)) { + if (boost::geometry::within(point, one_step_polygon)) { output_pointcloud->push_back(obstacle_candidate_pointcloud->at(j)); output_candidate = true; } @@ -411,11 +411,12 @@ ObstacleStopPlannerNode::getSlowDownPointcloud( autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, - const pcl::PointXYZ & nearest_slow_down_point, const PointHelper & point_helper, + const pcl::PointXYZ & nearest_slow_down_point, const double slow_down_target_vel, const double slow_down_margin, const autoware_planning_msgs::msg::Trajectory & input_msg) { auto output_msg = input_msg; + PointHelper point_helper {param_}; for (size_t i = search_start_index; i < base_path.points.size(); ++i) { const double yaw = @@ -453,10 +454,11 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownP autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertStopPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, - const pcl::PointXYZ & nearest_collision_point, const PointHelper & point_helper, + const pcl::PointXYZ & nearest_collision_point, const autoware_planning_msgs::msg::Trajectory & input_msg) { auto output_msg = input_msg; + PointHelper point_helper {param_}; for (size_t i = search_start_index; i < base_path.points.size(); ++i) { const double yaw = diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 852dabf0..35cf5172 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -139,20 +139,14 @@ StopPoint PointHelper::createTargetPoint( { double length_sum = 0.0; length_sum += trajectory_vec.normalized().dot(collision_point_vec); - Point2d line_start_point { - base_path.points.at(0).pose.position.x, - base_path.points.at(0).pose.position.y}; + Point2d line_start_point = autoware_utils::fromMsg(base_path.points.at(0).pose.position).to_2d(); const double yaw = getYawFromQuaternion(base_path.points.at(0).pose.orientation); Point2d line_end_point {std::cos(yaw), std::sin(yaw)}; StopPoint stop_point{0, Point2d {0.0, 0.0}}; for (size_t j = idx; 0 < j; --j) { - line_start_point = { - base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y}; - line_end_point = { - base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y}; + line_start_point = autoware_utils::fromMsg(base_path.points.at(j - 1).pose.position).to_2d(); + line_end_point = autoware_utils::fromMsg(base_path.points.at(j).pose.position).to_2d(); if (margin < length_sum) { stop_point.index = j; break; @@ -178,12 +172,8 @@ SlowDownPoint PointHelper::createSlowDownStartPoint( SlowDownPoint slow_down_point{0, Point2d {0.0, 0.0}, 0.0}; for (size_t j = idx; 0 < j; --j) { - line_start_point = { - base_path.points.at(j).pose.position.x, - base_path.points.at(j).pose.position.y}; - line_end_point = { - base_path.points.at(j - 1).pose.position.x, - base_path.points.at(j - 1).pose.position.y}; + line_start_point = autoware_utils::fromMsg(base_path.points.at(j).pose.position).to_2d(); + line_end_point = autoware_utils::fromMsg(base_path.points.at(j - 1).pose.position).to_2d(); if (margin < length_sum) { slow_down_point.index = j; break; From c2838611ebd1168f8141afb247ffe02e604eda76 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 10 Mar 2021 14:02:26 +0900 Subject: [PATCH 41/62] Replace pcl::PointXYZ with Point3d --- .../adaptive_cruise_control.hpp | 12 ++++------ .../obstacle_stop_planner/debug_marker.hpp | 3 +-- .../include/obstacle_stop_planner/node.hpp | 4 ++-- .../obstacle_point_cloud.hpp | 5 ++-- .../obstacle_stop_planner/point_helper.hpp | 2 +- .../src/adaptive_cruise_control.cpp | 24 ++++++++----------- .../src/debug_marker.cpp | 8 ++----- obstacle_stop_planner_refine/src/node.cpp | 22 ++++++++--------- .../src/obstacle_point_cloud.cpp | 17 ++++++------- .../src/point_helper.cpp | 9 +++++-- 10 files changed, 48 insertions(+), 58 deletions(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 296b51ba..8b63c9df 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -19,8 +19,6 @@ #include #include "geometry_msgs/msg/twist_stamped.hpp" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" #include "rclcpp/rclcpp.hpp" #include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "tf2/utils.h" @@ -40,7 +38,7 @@ class AdaptiveCruiseController std::tuple insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, - const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const geometry_msgs::msg::Pose self_pose, const Point2d & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, @@ -59,7 +57,7 @@ class AdaptiveCruiseController double front_overhang_; rclcpp::Time prev_collision_point_time_; - pcl::PointXYZ prev_collision_point_; + Point2d prev_collision_point_; double prev_target_vehicle_time_ = 0.0; double prev_target_vehicle_dist_ = 0.0; double prev_target_velocity_ = 0.0; @@ -176,17 +174,17 @@ class AdaptiveCruiseController double lowpass_filter(const double current_value, const double prev_value, const double gain); double calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, - const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, + const geometry_msgs::msg::Pose & self_pose, const Point2d & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time); double calcTrajYaw( const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx); std::tuple estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const pcl::PointXYZ & nearest_collision_point, + const Point2d & nearest_collision_point, const double old_velocity); std::tuple estimatePointVelocityFromPcl( - const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, + const double traj_yaw, const Point2d & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, const double old_velocity); double estimateRoughPointVelocity(const double current_vel); double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp index d3ec9664..70694f62 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp @@ -21,7 +21,6 @@ #include "autoware_planning_msgs/msg/stop_reason_array.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "pcl/point_types.h" #include "rclcpp/rclcpp.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "obstacle_stop_planner/util.hpp" @@ -42,7 +41,7 @@ class ObstacleStopPlannerDebugNode bool pushPolygon(const Polygon3d & polygon, 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); - bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); + bool pushObstaclePoint(const Point3d & obstacle_point, const PointType & type); visualization_msgs::msg::MarkerArray makeVisualizationMarker(); autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray(); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp index d2bc4632..6e0ecefc 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp @@ -112,12 +112,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node autoware_planning_msgs::msg::Trajectory insertStopPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, - const pcl::PointXYZ & nearest_collision_point, + const Point2d & nearest_collision_point, const autoware_planning_msgs::msg::Trajectory & input_msg); autoware_planning_msgs::msg::Trajectory insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, - const pcl::PointXYZ & nearest_slow_down_point, + const Point2d & nearest_slow_down_point, const double slow_down_target_vel, const double slow_down_margin, const autoware_planning_msgs::msg::Trajectory & output_msg); diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp index 0f6aa60f..07d62a25 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp @@ -44,11 +44,10 @@ class ObstaclePointCloud const Param & param); private: - bool searchPointcloudNearTrajectory( + pcl::PointCloud::Ptr searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, - const Param & param, - pcl::PointCloud::Ptr output_pointcloud_ptr); + const Param & param); sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_; const rclcpp::Logger logger_; diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp index ebdb603a..43ead397 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/point_helper.hpp @@ -50,7 +50,7 @@ struct SlowDownPoint struct PointStamped { rclcpp::Time time; - pcl::PointXYZ point; + Point3d point; }; struct PointDeviation diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 8690ffef..71e9f738 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -106,7 +106,7 @@ AdaptiveCruiseController::AdaptiveCruiseController( std::tuple AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_collision_point_idx, - const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, + const geometry_msgs::msg::Pose self_pose, const Point2d & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr, @@ -184,7 +184,7 @@ AdaptiveCruiseController::insertAdaptiveCruiseVelocity( double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const autoware_planning_msgs::msg::Trajectory & trajectory, const int nearest_point_idx, - const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point, + const geometry_msgs::msg::Pose & self_pose, const Point2d & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time) { double distance; @@ -203,11 +203,9 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( const auto self_poly = getPolygon(self_pose, self_size, self_offset); // get nearest point - Point2d nearest_point2d(nearest_collision_point.x, nearest_collision_point.y); - if (nearest_point_idx <= 2) { // if too nearest collision point, return direct distance - distance = boost::geometry::distance(self_poly, nearest_point2d); + distance = boost::geometry::distance(self_poly, nearest_collision_point); debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = distance; return distance; } @@ -228,7 +226,7 @@ double AdaptiveCruiseController::calcDistanceToNearestPointOnPath( // add distance from nearest_collision_point to prev point of nearest_point_idx dist_to_point += boost::geometry::distance( - nearest_point2d, + nearest_collision_point, autoware_utils::fromMsg(trajectory.points.at(nearest_point_idx - 1).pose.position).to_2d()); // subtract base_link to front @@ -255,19 +253,18 @@ double AdaptiveCruiseController::calcTrajYaw( std::tuple AdaptiveCruiseController::estimatePointVelocityFromObject( const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr, const double traj_yaw, - const pcl::PointXYZ & nearest_collision_point, + const Point2d & nearest_collision_point, const double old_velocity) { /* get object velocity, and current yaw */ bool get_obj = false; double obj_vel; double obj_yaw; - const Point2d collision_point_2d {nearest_collision_point.x, nearest_collision_point.y}; for (const auto & obj : object_ptr->objects) { const auto obj_poly = getPolygon( obj.state.pose_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); - if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { + if (boost::geometry::distance(obj_poly, nearest_collision_point) <= 0) { obj_vel = obj.state.twist_covariance.twist.linear.x; obj_yaw = tf2::getYaw(obj.state.pose_covariance.pose.orientation); get_obj = true; @@ -285,7 +282,7 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromObje } std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( - const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, + const double traj_yaw, const Point2d & nearest_collision_point, const rclcpp::Time & nearest_collision_point_time, const double old_velocity) { /* estimate velocity */ @@ -301,10 +298,9 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_valid_ = true; return std::forward_as_tuple(false, old_velocity); } - const double p_dx = nearest_collision_point.x - prev_collision_point_.x; - const double p_dy = nearest_collision_point.y - prev_collision_point_.y; - const double p_dist = std::hypot(p_dx, p_dy); - const double p_yaw = std::atan2(p_dy, p_dx); + const double p_dist = autoware_utils::calcDistance2d(nearest_collision_point, prev_collision_point_); + const auto p_diff = nearest_collision_point - prev_collision_point_; + const double p_yaw = std::atan2(p_diff.x(), p_diff.y()); const double p_vel = p_dist / p_dt; const double est_velocity = p_vel * std::cos(p_yaw - traj_yaw); // valid velocity check diff --git a/obstacle_stop_planner_refine/src/debug_marker.cpp b/obstacle_stop_planner_refine/src/debug_marker.cpp index 318855b4..a290e817 100644 --- a/obstacle_stop_planner_refine/src/debug_marker.cpp +++ b/obstacle_stop_planner_refine/src/debug_marker.cpp @@ -96,13 +96,9 @@ bool ObstacleStopPlannerDebugNode::pushObstaclePoint( } bool ObstacleStopPlannerDebugNode::pushObstaclePoint( - const pcl::PointXYZ & obstacle_point, const PointType & type) + const Point3d & obstacle_point, const PointType & type) { - geometry_msgs::msg::Point ros_point; - ros_point.x = obstacle_point.x; - ros_point.y = obstacle_point.y; - ros_point.z = obstacle_point.z; - return pushObstaclePoint(ros_point, type); + return pushObstaclePoint(autoware_utils::toMsg(obstacle_point), type); } void ObstacleStopPlannerDebugNode::publish() diff --git a/obstacle_stop_planner_refine/src/node.cpp b/obstacle_stop_planner_refine/src/node.cpp index 8cdc57d0..c2de9c4e 100644 --- a/obstacle_stop_planner_refine/src/node.cpp +++ b/obstacle_stop_planner_refine/src/node.cpp @@ -196,13 +196,13 @@ void ObstacleStopPlannerNode::pathCallback( // for collision bool is_collision = false; size_t decimate_trajectory_collision_index; - pcl::PointXYZ nearest_collision_point; + Point3d nearest_collision_point; rclcpp::Time nearest_collision_point_time; // for slow down bool candidate_slow_down = false; bool is_slow_down = false; size_t decimate_trajectory_slow_down_index; - pcl::PointXYZ nearest_slow_down_point; + Point3d nearest_slow_down_point; pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); double lateral_deviation = 0.0; PointHelper point_helper {param_}; @@ -305,7 +305,7 @@ void ObstacleStopPlannerNode::pathCallback( std::tie(need_to_stop, output_msg) = acc_controller_->insertAdaptiveCruiseVelocity( decimate_trajectory_map.decimate_trajectory, decimate_trajectory_collision_index, - self_pose, nearest_collision_point, + self_pose, nearest_collision_point.to_2d(), nearest_collision_point_time, object_ptr_, current_velocity_ptr_, output_msg); @@ -319,7 +319,7 @@ void ObstacleStopPlannerNode::pathCallback( decimate_trajectory_map.index_map.at(decimate_trajectory_collision_index) + trajectory_trim_index, base_path, - nearest_collision_point, + nearest_collision_point.to_2d(), output_msg); } @@ -330,7 +330,7 @@ void ObstacleStopPlannerNode::pathCallback( output_msg = insertSlowDownPoint( decimate_trajectory_map.index_map.at(decimate_trajectory_slow_down_index), base_path, - nearest_slow_down_point, + nearest_slow_down_point.to_2d(), calcSlowDownTargetVel(lateral_deviation), param_.slow_down_margin, output_msg); @@ -411,7 +411,7 @@ ObstacleStopPlannerNode::getSlowDownPointcloud( autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, - const pcl::PointXYZ & nearest_slow_down_point, + const Point2d & nearest_slow_down_point, const double slow_down_target_vel, const double slow_down_margin, const autoware_planning_msgs::msg::Trajectory & input_msg) { @@ -423,8 +423,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownP getYawFromQuaternion(base_path.points.at(i).pose.orientation); const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; const Point2d slow_down_point_vec { - nearest_slow_down_point.x - base_path.points.at(i).pose.position.x, - nearest_slow_down_point.y - base_path.points.at(i).pose.position.y}; + nearest_slow_down_point.x() - base_path.points.at(i).pose.position.x, + nearest_slow_down_point.y() - base_path.points.at(i).pose.position.y}; if ( trajectory_vec.dot(slow_down_point_vec) < 0.0 || @@ -454,7 +454,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertSlowDownP autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertStopPoint( const size_t search_start_index, const autoware_planning_msgs::msg::Trajectory & base_path, - const pcl::PointXYZ & nearest_collision_point, + const Point2d & nearest_collision_point, const autoware_planning_msgs::msg::Trajectory & input_msg) { auto output_msg = input_msg; @@ -465,8 +465,8 @@ autoware_planning_msgs::msg::Trajectory ObstacleStopPlannerNode::insertStopPoint getYawFromQuaternion(base_path.points.at(i).pose.orientation); const Point2d trajectory_vec {std::cos(yaw), std::sin(yaw)}; const Point2d collision_point_vec { - nearest_collision_point.x - base_path.points.at(i).pose.position.x, - nearest_collision_point.y - base_path.points.at(i).pose.position.y}; + nearest_collision_point.x() - base_path.points.at(i).pose.position.x, + nearest_collision_point.y() - base_path.points.at(i).pose.position.y}; if ( trajectory_vec.dot(collision_point_vec) < 0.0 || diff --git a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp index 41dfc051..a51a13a5 100644 --- a/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp +++ b/obstacle_stop_planner_refine/src/obstacle_point_cloud.cpp @@ -57,9 +57,6 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param) { - pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( - new pcl::PointCloud); - // transform pointcloud geometry_msgs::msg::TransformStamped transform_stamped; try { @@ -88,20 +85,20 @@ pcl::PointCloud::Ptr ObstaclePointCloud::searchCandidateObstacle( affine_matrix); // search obstacle candidate pointcloud to reduce calculation cost - searchPointcloudNearTrajectory( + auto obstacle_candidate_pointcloud_ptr = searchPointcloudNearTrajectory( trajectory, transformed_obstacle_pointcloud_ptr, - param, - obstacle_candidate_pointcloud_ptr); + param); obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header; return obstacle_candidate_pointcloud_ptr; } -bool ObstaclePointCloud::searchPointcloudNearTrajectory( +pcl::PointCloud::Ptr ObstaclePointCloud::searchPointcloudNearTrajectory( const autoware_planning_msgs::msg::Trajectory & trajectory, const pcl::PointCloud::Ptr input_pointcloud_ptr, - const Param & param, - pcl::PointCloud::Ptr output_pointcloud_ptr) + const Param & param) { + pcl::PointCloud::Ptr output_pointcloud_ptr( + new pcl::PointCloud); const double squared_radius = getSearchRadius(param) * getSearchRadius(param); for (const auto & trajectory_point : trajectory.points) { const auto center_pose = getVehicleCenterFromBase( @@ -116,7 +113,7 @@ bool ObstaclePointCloud::searchPointcloudNearTrajectory( if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);} } } - return true; + return output_pointcloud_ptr; } } // namespace obstacle_stop_planner diff --git a/obstacle_stop_planner_refine/src/point_helper.cpp b/obstacle_stop_planner_refine/src/point_helper.cpp index 35cf5172..11cbb6b7 100644 --- a/obstacle_stop_planner_refine/src/point_helper.cpp +++ b/obstacle_stop_planner_refine/src/point_helper.cpp @@ -21,6 +21,11 @@ namespace obstacle_stop_planner { +const Point3d pointXYZtoPoint3d(pcl::PointXYZ point) +{ + return Point3d(point.x, point.y, point.z); +} + Point2d PointHelper::getBackwardPointFromBasePoint( const Point2d & line_point1, const Point2d & line_point2, const Point2d & base_point, const double backward_length) const @@ -43,7 +48,7 @@ PointStamped PointHelper::getNearestPoint( pointcloud.at(0).y - base_pose.position.y}; double min_norm = base_pose_vec.dot(pointcloud_vec); PointStamped nearest_collision_point; - nearest_collision_point.point = pointcloud.at(0); + nearest_collision_point.point = pointXYZtoPoint3d(pointcloud.at(0)); nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; for (size_t i = 1; i < pointcloud.size(); ++i) { @@ -54,7 +59,7 @@ PointStamped PointHelper::getNearestPoint( if (norm < min_norm) { min_norm = norm; - nearest_collision_point.point = pointcloud.at(i); + nearest_collision_point.point = pointXYZtoPoint3d(pointcloud.at(i)); nearest_collision_point.time = pcl_conversions::fromPCL(pointcloud.header).stamp; } } From 8807c156dc1c95d30d96b3d1c970375e89768323 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 17 Mar 2021 19:12:55 +0900 Subject: [PATCH 42/62] add include file --- .../include/obstacle_stop_planner/adaptive_cruise_control.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 8b63c9df..e8dd747c 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -25,6 +25,7 @@ #include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_utils/autoware_utils.hpp" namespace obstacle_stop_planner { From 4aca1e3491087afe52a36d18acd7721067c3a1e1 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 17 Mar 2021 19:39:54 +0900 Subject: [PATCH 43/62] fix build error --- .../obstacle_stop_planner/adaptive_cruise_control.hpp | 5 +++++ obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp index e8dd747c..8d93f517 100644 --- a/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -29,6 +29,11 @@ namespace obstacle_stop_planner { +using autoware_utils::Point2d; +using autoware_utils::Point3d; +using autoware_utils::Polygon2d; +using autoware_utils::Polygon3d; + class AdaptiveCruiseController { public: diff --git a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp index 71e9f738..813e5fee 100644 --- a/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp +++ b/obstacle_stop_planner_refine/src/adaptive_cruise_control.cpp @@ -298,7 +298,7 @@ std::tuple AdaptiveCruiseController::estimatePointVelocityFromPcl( prev_collision_point_valid_ = true; return std::forward_as_tuple(false, old_velocity); } - const double p_dist = autoware_utils::calcDistance2d(nearest_collision_point, prev_collision_point_); + const double p_dist = boost::geometry::distance(nearest_collision_point, prev_collision_point_); const auto p_diff = nearest_collision_point - prev_collision_point_; const double p_yaw = std::atan2(p_diff.x(), p_diff.y()); const double p_vel = p_dist / p_dt; From e5bbbaa7933c88b7a6ad2fbd3f5b80061c48324a Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Fri, 19 Mar 2021 13:46:34 +0900 Subject: [PATCH 44/62] Apply clang-tidy format --- .../design/class_diagram.pu | 43 ------------- obstacle_stop_planner_refine/design/design.md | 11 ---- .../design/img/class_diagram_orig.png | Bin 14639 -> 0 bytes .../design/img/class_diagram_refine-1.png | Bin 42496 -> 0 bytes .../adaptive_cruise_control.hpp | 22 ++++--- .../include/obstacle_stop_planner/node.hpp | 6 +- .../obstacle_point_cloud.hpp | 2 +- .../obstacle_stop_planner/point_helper.hpp | 34 ++++++----- .../src/adaptive_cruise_control.cpp | 54 +++++++++-------- .../src/debug_marker.cpp | 57 ++++++++---------- obstacle_stop_planner_refine/src/node.cpp | 18 +++--- .../src/obstacle_point_cloud.cpp | 2 +- .../src/point_helper.cpp | 36 +++++------ .../src/trajectory.cpp | 5 +- 14 files changed, 119 insertions(+), 171 deletions(-) delete mode 100644 obstacle_stop_planner_refine/design/class_diagram.pu delete mode 100644 obstacle_stop_planner_refine/design/design.md delete mode 100644 obstacle_stop_planner_refine/design/img/class_diagram_orig.png delete mode 100644 obstacle_stop_planner_refine/design/img/class_diagram_refine-1.png diff --git a/obstacle_stop_planner_refine/design/class_diagram.pu b/obstacle_stop_planner_refine/design/class_diagram.pu deleted file mode 100644 index 45701623..00000000 --- a/obstacle_stop_planner_refine/design/class_diagram.pu +++ /dev/null @@ -1,43 +0,0 @@ -@startuml - -namespace rclcpp { - class Node -} - -namespace motion_planning { -rclcpp.Node <|-- ObstacleStopPlannerNode -ObstacleStopPlannerNode *-- AdaptiveCruiseController -ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode -} - -@enduml - -@startuml - -namespace rclcpp { - class Node - class Logger -} - -namespace motion_planning { -rclcpp.Node <|-- ObstacleStopPlannerNode -ObstacleStopPlannerNode *-- AdaptiveCruiseController -ObstacleStopPlannerNode *-- ObstacleStopPlannerDebugNode -ObstacleStopPlannerNode *-- ObstaclePointCloud -ObstacleStopPlannerNode *-- VehicleInfo -ObstaclePointCloud *-- VehicleInfo -ObstaclePointCloud *-- rclcpp.Logger -ObstacleStopPlannerNode *-- Trajectory -AdaptiveCruiseController *-- rclcpp.Node -ObstacleStopPlannerDebugNode *-- rclcpp.Node -OneStepPolygon *-- VehicleInfo -PointHelper *-- VehicleInfo - - -vehicle_info_util.VehicleInfo <|-- VehicleInfo -} -namespace vehicle_info_util { -class VehicleInfo -} - -@enduml diff --git a/obstacle_stop_planner_refine/design/design.md b/obstacle_stop_planner_refine/design/design.md deleted file mode 100644 index 1b41ce64..00000000 --- a/obstacle_stop_planner_refine/design/design.md +++ /dev/null @@ -1,11 +0,0 @@ -class ObstacleStopPlannerNode - -Key input: pathCallback(input/trajectory) - -Class diagram - -![img](img/class_diagram_orig.png) - -Proposal class diagram - -![img](img/class_diagram_refine-1.png) diff --git a/obstacle_stop_planner_refine/design/img/class_diagram_orig.png b/obstacle_stop_planner_refine/design/img/class_diagram_orig.png deleted file mode 100644 index 689bc42e6f6ee2a40e7db4938b8e3a34655c43a6..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 14639 zcmcJ$Wmwf~xF@^_NeStOrF3_9Nl14K(j`cDhm@3bcb7C0f(R(xU4nE;_xu;Q&))l- zdFP#(>kJ=6*Lvda_&vAp73E)|AQ2#eKp+%pDRE^G2pSm#f>K9-20k(GmSzBc(Kt$I zI=*>p=VonU>IiyiVr%lw(9y)0%*c(*+|kjFmx;;F+R)a~$;O)T&0Cvi%sfOO5R8R| zs;1*#pM#))V_eh9ljdv*SkQwHG**e}PbrJV#nJN^wk5a=6h+OY-svRki7o8T>Qu^M z)Ono`W?(3zvZ+e=ho5eIIqM(1Kggg;Al%8RT2=eJreBgoNK(BdBpbuy z!cpYVjH2Z2SbbI79bJ0q;-|hCe|_%xJ}5;Qo!ZIZ8&P>;#4BXEhQHs{WLz#Sl5N95HvC<8u)&arVK!WoECNQh$ut_F zjMHPCp^6h@L8&99=W<)11;1mL5~+`MLDr$+!IBVqJ_zj=sF%eCxZVI9gR4s!R&3m&o|tUtY-| zYBCnWGRLW=5ngG3#E{5~)XkTRk_m{qQdj&=_H%(hUhs+c84S0MD>PBRC)AS{d+mqt z+)@f1&4^^9$ibn6 z_%)`)c_D*H)CDc)8h)fJU=#8EAOo`eqLeJ{*=WP=^9KA_t*iO_soZnQ37@FCK4Yx~ zQc5w-H>&(kFk-$AizXk52}R8eh4mz5%6Lv9(k&iLY37ZY{^o$gEgYS~=-6OWP)_r@ zQ7ac6lg%O%(~OHMq@jLAV$}okK#rS*`e0a@W0m$wJo{@l^R<}`{?7X*or>~XqATw+ z{cc$(4-XzwIzx=6X>X2bd+McTwPw$GR@c{k6}k}IyR0}ioYg&%hH zkRN|7mVv|ezY9ZQe8F=&%A`NIg|-|(2@(g6Va$;L%X=xmyw-C6 zv3HH=dn^q0)$?=<3dP3}lc)_&J*#NTtQcH$yeXLm`5pjtwS?jXk>TX)5#7mTm_O+_ zhpZQm5hda?kZW;YUlgl`0{OrMft<6|5E70Y27l3O)sWk&5)Iuoyj{}2|9*9Gaq;ZM z%YFl9p>{HZy`dI(mmmG#)3|)}t#z%9jEtO}IS;Tue;k=#JpQ>>wnz0A$r8QD`%2PVEK9N+9lXG3;C+)0S2Aog z`kkDuJgsYEe|S-yvliJIbo2D_@{_Z(lFwMpY-DeAbW&$~73svKxSFe9hpT3H+yCrI zwpbspbG$P*`L)Q_@PrTK4Tl7^+bIQBWrPWQbz4RgMKV zW@aWK(LL*Ar4=_Tls7kOBD=G*bNOkrriPWR+>(}X)*=Kgv*pF#+hLXLON|z;oG>Zn z@mSt{{d{>GvFMDlpOBo)bKcZcZC^cBpa3Xgt<}E)iNB@k^;rdS4mPMA`caFi%orq+ zhw6TSF7CK_N}F0=T7xJs@rrPeRes0X$h#^0J7m(B$CKeABwk1vl1gnzDy8< zv3mvX{88{z!>5_IrL|N6*zNPMyI3@SkzjgHC3E~e3g`k2V&>Fpp?AW|xJ(=5g$5Xe zphHrK!gvx7;zWt;ZtsgH6P|c5-R)kEnN`kDan7h}^9A2vAc7d70Ac&r`e8wLzoL0x z&&RNg36a|-LE5{eFP}?}vE@e8!fe?`ZTTF~?9CNl$^zehgpd#~zTton@`WCX3u3W; z^JZ}U7upm)jtIoQpN0Im=7o9b75?E&*hR0SEwjX1?b}Kygqb ziMZyukSvHB5n_DS`)}?--sK)7-nsH{U$A*@WTSV`LFn)OCHRZ!qy9p05V&^`10)Yo z066uUK2M{;&=8^K6RdCCpJm8f6E>Q7{=4*0%j6LLYMPjg-BDoUk+J1c*(&4~wNJ*I zb>9`Vt5PA)6mzTw68YoIXhC;KL*!`eU~qV%?b>QDYPk|Ph2&zQ=ILITUy&adagw-g z4Cbqg{V+YC<&P$U`;RS|O743HXx9ARPHW#Bv=X<2C^^Pu1lOeRj8awbP{98pZs9~` z%X?)p{CJU;?U;l8H(25Khj^(?R515HSpZg@!^WQsn0fvt_t~c4jx|-lO;wq$+V~Q@*vU zbB{~VFfp0M3Ui@%cX#XQ>0Lg#%$59rF^{nAVQ5}ZpkJD4kRBJ8TVFIQhB-=#1jP^c zs8h_*onH}FEuUzEo2hpUw1%)&(_I!<`<+rwuwJ2?})n+`L( z$;imya%saSB^6@vey(O7Gt=iVcV_dZCsOX!tA6g7FSjs$!tU7TtQ>~4Dxobd2S{?z^E$;7b`@NStpz6}ENqOFp^H4O^*VmVq+x#quY;is?a{iZC+I2nZR}7~V zaZSGVK6?cQ!gC-c)V8dZqK>BbTT4qBDXFafL58mdPe<1SkCc~TCYbZm&@<;D<%)-J#nLM`k zc%8%jbmR1j0$L%f^#!wf67k=HW^+I@yi=jf^+3oeEw%dAkMDHqWV#P7DJjuxt*ME< z9!lf*R8bK%F#-5W50FM%q6fT0-6y?mbMLMq!ZORF3uQ*@J1v>e z($T#^VzE3an*Th1cIKShpa*=odMZp-Wj7*fyS2Z+SpBBw>&1K3qD~>L?X9huW~ZHL z`!`#ZKfl6LH3~WT+)rPBG93zw4A-l&6A=-C`Xu%*nr{VRs~x7~DKWO6^KN;Oc0vJ{ zzxfjaz^w#cg(=(~XS?6mKEC-Dl`VVWXRts`^4lWT=rG+99^m`~)MEgt-I~~RN%nuU zbif)U{+}Ghag5G;4%-*uPpkp{c_#!5dIk%GU{TI<{Ctg@4?LjfPabU?IYa-*rv%E5 zYSRwF7q@A{JOl+)>rUG4@b7{DP}63%}ILNjn{GYYa;bUFnn z?a%32QkAw{o#@79h#)FRW&sq1Gg>-V#SHro3nx7mO45+{NX-EMt`uNj_OC#B;0KVL z0ZA1Aoh0_qU>i`xI&us$T}?nHXw~(#B34f|ctQ`v zKyUx(KB>KYom9Eze(hG~s?-MQ4It+s(4$$*@_Jn2B3sj1$?UYp+K1d(rgmzYldVoi z#QdeB&Y&iyf-3M`C(IQcr#LDukd%lZqHS1jYp!S$y8qc)kl}qL>r~An+GvviQiP=4 zmHPRHJ$0ScZpo5igOc!pXPq`c;v)F7&8M%FZ)R8;`#*0gnRh8|Jtr(I@qmGG0=}#X zm5=FA12%#nmRI_2iEfINvGK%Mp-3J_&Eu=x1U(cPgpHMw`SY$1R>y=b7tbg9ax6dK zRisdWU?EOAA9pS2Hhruy!x50lED#K&wwCWW`15SCAoGVZMqyvaDL*I-KlixNECDnO zae$TBc{9hBtk&vK1jb|vkmJKT>3!tiy&|sM&z|YB%7QlktixTaqgZ)T7`hy*1ucvV z;f`yqZP~y}(MS5Za-(4ah#4AE#jLl9ao)FwESt*0gYV7|9Gq0h>vHMn4YH5=lhpl>tIc_doC zCYCWvNrgP{k=qN@2m+!uy-$$^*Ks9TZwS8x9S8)(%>L=)^Raz?jBZU8d|!e9_H+JEaIF)E>t9TT-JU8@(=ks=ffgr1l3 zU~2r^H_DV@b25t?=iOpYo<2}T_t-b#hhr4~N8^+Y4F{p?19nhWyAez%QQuag_t86f z9}p>f@*m2ThKd!oH?!gwJ4IL?*lg4Q8wGbOJ}Zw}?kMPLeE-kp>Ez+22I@f0HrFz!Cw}u$O3hT|;~kw~xA$xl#FaoWMFf%j*M3k5$L5&V(%Vj&;+saI5r>9CJKBiyU z`r}Miek78%H{H*eWsW>wz$u#Q+SU$FZp-w!%ws|)Vn|`77drCiyJRj z_}re=-k!Ouf>}`0y*$S;X8A4dEDXn@*ZOEoL1<#371AkY7h9X-?g|6$ExqiaJEu2ul$ja>IFYLF~)hkO&bsD_&JU#h~ zi9e}G{KZ;N6d_^mjN0P&Vyms2k?;MjX12)AkO0=fzoLY|7H(R@Uuwl*P*BO(|0P0n zI2~!?1F_KL*qE5K?n2%|Sl#4u1|_zC(qp?R1~}$xk_{};i@Z>;sxBcKBaO|nLFOfm zRF*m%s9&#tsT4FS*{dFGZ|As6NCY)Yw_sb9e-W4|e09S{ovV->3fw+s1H14l6h-*K z&bbTGcP-e~uK;D9*=gR^x{a<$uW6~NnPgW&tQ)#tq*YX#o{mnXq2>xGzTdzZOj=r4 zNNat}u^qMT*0!=!ILmXF7)6>LB2y@N|2^25mp5bx3eUUp!qCc%AhGa#dTIs5s4R9udQQn^)?Y z=dZG4qb{;Jg~c2Dcj^uvAErTV4?4|4SL%4He=)HBBax4f?_jYtHJ<%YQ`D;`_ZQ%# zj=oXpl{kbQxDVl?2iu>oEDen zEo}`7c(^Z3bWOKS0m&Y8UpTv9@?|kx$m8y0hVHyhjyexr`vp>%kBn8%PtpsPfHcu) z&S=rBEb+JCpkV?f#TykFV?o$h6@+H`d=#SoQeK0Rz|~o0$K05xXZv7B;e+a0pTNFCvG0y+L{i~}V5Wz79xOSBsk-eR|j4gKJFTA+WvEzHG z?5G2AU-vwDLJ_g<`9$8K(h%$?2L244ax!C&~`~>2skw5b@>7WU8Y(veEU)E@<@r*F7=H#mD6p*c{x)b0g%p?(RCi{nU7A z%b(~}tZ%L?2Fb^3O!ZLrhFJ1svyaCLpT`$Cw%Z+ayS@FGe7TKz8BKL5M(%6N4Tbjl zflc3i8o7V{0@{udKqUK%YJU!4@=(6I81D{_gGHsW7wG(YdhwllvyywHM z^vlZ2*Uz$}P(|kSfMESvi7A*oPYA2C^gr(VS0|kuWlI(o9^TU2{2TfA%SaN6#%JT_ zox{6|N=hu=ofn;*oj~W*yqD$PU5+$~#kx=w^9~1LhRa0l`~E^pYD$VZm8`6+24$u3 zSu}A#S4%@}?dQ`;ogWEIn7gQAb9%Qo+i7V;brKUKDbaU|2$e>UQnizLJJ?vyw_^{t zQRZVJpc3k`=oUigFeoU^PV~^cPF5OZ#>9NV{Xi-tZ}rAW8_Xf6kS3ovJ-0NyICp*F zP+n70<958!{jGa2gYEhxd8|JU_m{)@{z9GAR1`7S*$m52+sSX)c>m zG_T@9w(Ti1E2i^oS2C9@q-?j0$Kls7IlX?(#o6B6mp#d!&+k77UYY-n@9PRidlNox z0<^rd0_+3?Znkr6k!Q>-EG$38C3}+tMP2{ce4vW^=#E5U3CRWxDur$WHBBz!A4EVA z!@}HrIb1c9(~F>V3`qro2#9`!fk$KZxXHT!y4d~weLk1{&&?}p`~9>LO%5BQ=_m^W z{O%{4kz9+e45u8X3PNca@$ss9Dht!&jW%RaZx6ZgR5Te(alGiwn#3 zZ|ol~yVFW~dILB-os!ZX7pPR_uV&kQgj~jm5e^3T!3m5yw9}E2BB)mS+A10vFO$!g zvjeuHnw@j)Q7$UAj+ei{v`P7>sHiM?90=4&u#n99!N8B7MYACW9n#`paAS2MBNU!6 zO=Z})PIHV)0EHB+3c_AcJ?J!_qljTV(IA>OV5;QBo0m1bi8kF6tU<}dtVtget2(_2}_^Z5XRS@d#na8QtwA6`wC>sU}YGPDC&G&uj= zCZpd0vrW)J)rjzPo>I2eA+Gl|^cGITr!az3>Rvyrs0nGI%_wl=g1PRRfrdfPwsj1j zqUi=sx2rh-hlmZ5|IfrR+gbqpoLh4Nm1_T{QRrLF1lFt zDhb2f?T@7{)8=H&GFe4{RiX3#mUz)J(%1{ma(D2(9!sCh5bzks$<0PQi`MzuZ2ITL zlJ4WBJ&!#C z7tRK`Vry4z`+&gTqQgjzZgI;l6Q{xBA5~lxbsHr z>?Qp8+9oEw45Hw_LU$)k`8AQhP_g&M-O#4dh)-a#5R?`83Z3&Gs7$lahYEi|P86^nHQ8zabd@Us<1@PD3{Z1E$ zD|vb3tlNSi-0mmlGBQZzAHGIEw98yM82f)1T2Llu@zPw_LbJM1XP0LQUz^x z8>{07E`NqOYzz)~+1gs`?gGgE_a8r!k9uQX?2h5pYwGI0O02GxZ*F%3aHILfxw-4> z+9iby*5tf(08dJaX&CiFroCnqmm@GOR0v7n;T=f(-$Fqd4L z&v`cqPbdaS?V^WC66P5OJJlL>1Z&Hqe!jm>3<}2yCL)M5*_kY7XKe1vx_sIsK^>^^ zC!ulYJGG;U?9SJ*NuHij(dEP~o`Y|E_f;=2|G5SRZmF1ZFhk%6Noh^u(Zo`eDV|s1 zEYLV|{~e>x;yJJM3YIl5^e6#HV%fY4(G}(~xjHIX+|<)m1^+8eM&6@AmHi_(zIK%P z>C+E|j2DC%>P$}#sUi@(Wq#hP!qE7KV20}d`N&NM{(p*5ov)nbXCYG$D?c0o!z$iEN05aO$_JW2mb(f>yU)c^YW_1BDTXnq*M@6eY*4yh@A zKh#{)_U{*e2gL7Il9XP_d?U{GJawHwQ#Y}?)l)WKYIE0}-|$-yRF`-$di7GnPt+Cd zQRxsk>c{Sg=H0DPm(TTP1?R-wg=5|1jN1Car;@ss+kv=qb8+pBOG6Q;VCPI%F;rK~+3R!MFD~QiWGz_5L!T1*KzA=cBL=w@*Ws21*#Mwr~jf#ibq28`w&LYk>DUap18V;6AOOMTohz>?qS= z$CCH@E9a0sfXZE)St?i5x*kg+iep1u1)5t%%;mm4qcqcQMdYQUfaL`S``Oc{Ztu2W zVkN6lK=2QdqJu}!YVUGqH)Toxbm57UnUAmd5$@&bfPkwr%JX1IsSZLtQu5lEQcx>g za=1pbn1)>tqQ5V@Bo(YE-|aXc_UnD9kbN;f>yKRR^>24lG6^MecGc_HEcKmS9J?)R zKQLXDxaj+PM;Q`pRE7V*mvvrs;r$ZQ_REj+Z;v+< zNy?Ol$8XE~w_DXljy^r_V0&=O*SH#U?(Htkl$SDW`YR*vZj}y}GAi7TEW?4fa+}8` z$XX~cL-}L-%d9ie!TWdjA2kwPtDl`~bl^V_gCN9b+u%j_iMOFz)*2*caey4b*hH!_ zJ&g1bboK^PnhJAoAyXqWa@;QeoYmQLEgZwL{&q)GSU(HD#0($vkUo5?M&`#961#@5 zR>Cw>mKLmU!^^gZw2u@*d?O#IKqs-~-@a{OtNS5nbv96G5t!U?Q0H^2dfJ0_I6_o~ z{J?T&NAk7a61Undj{efkyET5iB*`W3J)-Hz2hkfO=zX+V;#W%#2w$?ZYpUB|`eezu z=t|8AI?R7mAFmgb$x$IpJBQ=(vkcjg$dti0(v9)Kmhr%$9bwnUHP0CYZo7~PGZ-yH z6MU$NC2G&$g0VRxXq%Q$KY~^sJP$-SEZ4P^S*+*3Fs?|1SUa%f3UV*kr&JL{va!a`ba6>vG8GFh8{xgUvuHDOp->ML9H zE|mk`tl1xq2n-fT)W4z0kbN2ZJ<2aP+OY{A9!m2uWT01VjJD9(pAxvGxL@77czfLh z!RuXQ5(<+%$k+dfRP95XeaTh77k{ENavn+^dLE|SMI)NjLkr?72*_{CY{18*;0!K9 zw&iKFmsB(Vq1xGZR3ZX0zXv7o0ylI4Om|ziOq`|nZ`a^_euCWpxLr!ZgGn!w@d~RQ zdCfU$(%BPGZ(p!H@Gk$5TVU}-2bUeT^jsNQ?>i{tg@VK_9+;8>2&5 z9AB|G*?%@Uj*l+lhSIeOeXhzzp@n){1!T_EQ@~KZ%W6|?$h6(h6$6tgUE959guVw4Hgjik0MKpCpLj;Jbw1nfcTa@M{-7?Q zQBuVOpUInBHjXJkCb?-%OQ)wQ6K|KSa0z`sJnU`Dt>a>@xwuntW!!UuL>Is>0QbNq z^fh=qdsW@z08OYO-KQ9l>GOr(_F$XO_hLOe#7eL)6vRIRKlEDdUa6a&5;h#^&b}OG zc(K3~#R#$1Ljj0vI+=D`jyHD-OW2MG1@8j(aWR=V?_~A$#JB`CPBUI{j8tMWBF|HM@P30pWWA#GyWs{f8fi(&H z2gd4eTY_+LJVPEp4oX0FTgped?)-^xysFMmZ}6tVehl0;Hl1KTP-z7VmnnVcsoTD5~kd*-0H687;Rg zBaY=iSQ_NPH0V*<-;XN*Jmd6{n&obcMB?xbFF?p4ZjK>>h0re!-dqhIWIQg!aRXo> z3_{8Q5NOY%(C4|-^AG7|15RP)h? zhlT&Tu^eQ3k&)f5y5VedPA0{J9Jo6NU=9{z1;EhR$WRm|B0U40$pb88W&yzOOcid` z_4NsJA-ue>bIFC7Zteh%y)#t_OqmsIW`lKGWOg903lPBSYef4l&5P6y>=5i#K;suv zNFmMC>+9>g!lNGv^6M)qDsKF5lyYAp+QOc;GCMLd8np3Sozc@KR_dzHO(Z3o>(RTc zsj8~JO5z3!3AMMj-U3(+R&GuRN}yHlWbn{mjBwCf-1R8n8E9%s9V?s@sa7_seM!mt zCH`HD%y=nYjRqLju(hmkzJ2TD zbUOF8T`uJJ)#*;om||yLX>G>DP~~8s+dD7H7E`vnn!<=jO$^{qIhzRx>TpA5P~^KG zU@xai&teG3Anhe^x_AFFy#Mqb7hz9QlD#M8mj4wBK*ngN8wS!FRhp^h%659drXcsxdw5qFA-5bBw zon2g(0JLrbrIIywSCZyKQK1QB%15w@jj6o2v^2Mu&vkPd7%gnNl{Om?gx5+{WSLbk z3<{&v-tC&K%Da5~H!Mgfk+Oe;{cDNZeGILTih@FIN|g&hgy-rb@x|ot9$`HZjY;d6 z7uM{(N|TRZUTw88dl;EWkre=rpsRM;nI~)}L`?*gfiH#wyKB8HhVFk|!UvQAc+H#^ z1qa6|@HC0pj5koCw8}cGCV|-pj*(jq>q0vEQD;U-;*8>?BpE^UjLntSlF1Sl0Lc89 zMxPKb(0}3`*q!1?;`@c9F!a_L5 zal`R;ILD}RGtp~et!xQa+5lx;HMQ>fOH;a3SA!mi2Mkn8ncgMs@c1w`Eo+4$*sUk3iJf4?tqU=k9_gl#V)sqtv60A1ayeB}Y8|24tC2Jb8`t^Yi0#x71yM*VrE# z^k&Y6xgt?D`@*$7@xTj6E;EuS1iyB?y}r8I-1l2E1h7)}p)#6Y0>=qJjwUjSh1?OL3|czG>1 zFgyKyxB*$-)6QSP(cYdV|I|#9Uo0^M5+J+_r4YGI-Nj?8+8`4BrI$Wm@ z3}@1AC#IMO0b_hQ*I%9><6>c9F}JW@Sn^+6b^?PPDMB>QR+D~T0Pp5ArSw`eTC)rp zd@fPyqNu>I1|2YIDmfFW3_V-@2L3%fcFEzjmZ8~2&d4D?c&Z?SapAAH0~j4#r2Syp zmjki>WEIVeH398hsWE{E;{(crgM%?|A-C_wu{d|AIzSjIOREd7 zjPoJ)iF>@l2fqY6KD7X%^rVpK;V1eOqUN}tmBe%+Q(1iMxpDEo$WKpIQ`omfHHz5T z+ep-v;2P@eMmnQES;jX;IIE3_zmtL#*;S&OZLabF2pmT&>+~$0ohc&(dBFy1BL>Cg zhq#1hGyZ}bC{Oq%IgYJ2gS7@x*IpA04QHTFv8kLW&>2STd8BnKry9JUsL=)8xj@ky z++vdRoPOb{JU$2T`ebcwSyoz^JIC5wf}Xl3eir|j8?y24LGpISKA@LKfi1Gbk7a3R zl_MLs$v9xuj=%#MFcB{$C%3NfDP4rH?+awxF;e#OOLjk~<`ECRPV8-wi zLy$#hfc;?10NGR~N*z!cqg!M?n`UqGdorhRay^#qHlJ6kcfUbH+*7{S91$1H6<7tU zB+@H|11apkVnc(X-i)kH2ZO4xXQUG3mXWZa8{vC$x~oy3UendY&;#`;HLM-lzgrog z`}y*jwj?VHmI;`M%i_iC7O1p0SAnte# zi8sO`i29p3`u|-W6EFtmVMnh`G=~nVe@@s3hyj>PIbYVS1=<9VwFGCZ$wNQ3-0gtH zCULfI`y-x=UWs4HSRoms54M6k7oD2=SNNq}!_!WTZrSdehl1P;3z{h-H;JZ($wddw z@cnA!bB{k9Fbjc}&Hn$Ixc?oJfC;|+FV*Ju)?%RPFl+K3kMc+yE-rut zZhjBk9540orLYK#L4Qb#33>vxKJ70Be4yK!E=RXXZ{6THILyg{rNLq|!V4k{qzFs= z)1x}m!2iEVmdsBhQ!Yt)FhJu4JTU7A0(t%Oe=2RF$t5Nl9r~bW1k~NOyO4cf+0Tz0bMs z^V~n+em|(sTFYl@>umeSr!C1A`_e`W^xUgTx2}1FM7#2R;$S5YhpE zQQLh`vD3G-ayBzGvV##Zv@o>MwKFsz(Q_vGXlG}|$-rP`rfXqmZ*E4fZ)yINk%JHh z27cB=LB;MrpTod{*El5>#?4sFyuAe-uNqXODdiQ=AQw zp`la3`I4*w{(9}UYr?{H_%a9y>L*f^iG97_*=rgr)=hMxN_VRqT&JEU!K>0_xlXx{ zG`MkucxmurYpj z9ATe+6jlkYaK@BmU|Pm=CLhHzQZVBr7vrnc5lP)RwvtPzDsKwbkHDqE~+P1Jxcg$J7>SxnE6yBdiDgBBJPbT&iGJm|g zdXx4}t&0l%IYhi!oP)y454t@2jXrH0G&=_k8s`g60cxMm1F_=%&n~Ah7R}aB ztV#|MpM5RA?Q#SM#xY$(&PrHgAzv|Z~aMrf5+=*Qv0jpy}t zgM=pE#T_8jO>MKO|1MP2_mT=U3}&XyQYr%6bbZ<)G4^NATfiyU>? zG0VH>6~<;%G0tBXieDxWL@SNWheo(<_<^3#kNzQ zw10Fhhh7~`wB%o$Q1?h$EX_ysCPw$nPMqxM{G3RfTVI^R8q!aJq`htpJ3~CQ`(zR# z-RzC@Eg*sJFw50}f~ef)vAiw@yTF+T+ABfq7WiTP3&)HyzR%RiAJONsJABcR25A%L znkQdfVyM4g;Wc}kU<3o>1ta!eK*33CckWS~!q^<&;p$mm0=L+F+xyaXs-PfBIc#hx zrajAJqg)9F+CIehPaij^$YRQpypStReJX(YHQ28)snAT9z;KGwEMeZkh2eq4HnzvA zCu**yjK#JoWrO=>mg8d9rGJ)dqpHfS9RmQ#Ur#u{wk{a|p9dKQmij;c7NtUf{^#E$ zX_(=^k?i;5Yb5z6~!&qy2U!>X_~U-4mw&sH9RvBy&o+ORk^v_G_o3^3`uFc(s1d;&C^h(liBfep$R**P6D@q;F3s0neQemP@%L}MqCSHqNEq;& zB2$cYzhCK*XNE9rWD2h%|MUH*H~3!v5j1B~N>^^_bHMYj_+0Btei+Ki$_Fz|2lx0F z#-TkLm2Rraj<}lDm6b88;ck&J^y=cg{{6nvY4K09GbLhTqUp6u-G*1izGX{#oYxrR zpdxB|g?uMx;Ikac_|o~LtgP%YDk`4-R#=MrIsVll@|F(`S)D;O@Hs`5h8TFP}`XG#TSX-+^_+@ zhx#r9PU~d^e61e_D2hKSD;?T8pRQ&=G8J+a47}V7+Mt_L%r@!q4ohT@xi~q^*k&5t zZ#p}0Xq7d-<34_i@>Zxqt-)>za$H(k%4GLO(`jIyM{`b7I-BM7>wr~DPkq;qOpR;8 zENtH+4GK3EDYk;zWf+yhju%6xgBX$w4E`}AC?*2bi$I${FFQNn2^V4LVA#trV=$A9 zj;~qb5P_7eEXwVb8rzM|4#S>Uzy5$1MjP*0%x!I1&W4Q2BEs~4g{JY^q*JCDiz0P) zMCU8{hc+l&zbc-PI5zG}{^0(kY4+nUVfn}coGVeb_-m*0UDf`v8SQzYyetWG!&Ugd zcL<95_awSkj(k1r>gLjjbC-Ip;|<~(7eY|$!X+KV9Hd!Ch993@x$V|4$s*#hBr@BJ zZ>pH82Ku9%r`*IF1f>0@X1(23f$x|@IbGvFKY2ghBxycPSebw2xT zeFT#H!PYZG_OXQS;Lp3KZzY2~)EvD0lUn(40@q_2tXDcBbOTxi{3)=+HmS}bNNj9w zV%sr1*Z#bX>gRz;ACj6hqO(~iy22~{{B9!WDojai7j3ZTp9~G@y^bUPzR*-OYEC|#<0P}(}JQW41x z{(9Q_``Br5Q)5r(MOdwWhN6B1T#zl!D~bF)9IUd5K;@!fG+wjqZA$%(=HjE>Ngc*q+(t z+=w5=#@k&v(f|8UCrPrgm!%+u3;$lPt9adkMtp$p9}g>eE)shIznT!hljuYY4bqCL zf}5!KVNvC|d72{I$jYLm|9vv$uNj@(`O)AY;p4ww_twe(*jt(+DcT^TGFU4VX7Z+`ZT0DCQR7hyB_KV?4u~hmJkaxDT;1kc0|Hh2jc#F{PCrwD;$tgm4(;cE3skbYYLAuKEmi6-f17;E2k1Q@}8 zZ5=c}@F_ydfHv(=lU>>*q zCRpso*qm3$Wg2GYPZT7xIuBPLtvqK?)lgK#;f#rTE#H31o%{7Ee(xJyax(!r@yfFx zt+uzu?5|?zNhqx_Ld(X9%z{$8hs9{!L1O~bE1w8F!%hVQwR6{tyuAG9+~qo_ zaYJB_vQDOKk;$@tK6#owwq>tgWzpB&{hiVJ=l0wDo$Z~R+dyo8k%Js+`E2g0eGWfC zfemdfMuAS76qQ>9ve&2j@1ORNks$u>F@o{al+shK#n;w8+=hgNDCighmM}Iph8~cy zY|ws*vVz}^%xu25)`A@Lp4onSw7Xk8e@snL@q<4FF8OFq;82Jx6OK{fyS?wPC+bi7 zxm+5O3k)p=sEs!Ye&U*BBhW~b25)1&vH;1jF19RoxAUrP*r zD&VvwyTf&L2TL}ch07)fn;XV=AJ_kezUJ9PYJ^D_p`b0B`Lx~UAnEl=i_fF3*&JPF zA?9uG-A7rHNk8sF_ADG9q&}aWoqc(!8O1?=(t?Qb{5cMUV(WeM*52M+iEisE^|#m3 z->dJh&X9b(yDyx$>-Z*lKAi*3^Tws-RDn0ea-q?^Y@_%0(e^MjnN`i?-hwDMWyWS2 z_e(3MXU%12uc|XK+gR!~{3@Adq^*!(E4xyD^c%m3fhrs*zlXs6XT-;y(kk@^*2c!h zmu`)FuhzndxY+d9zdj`sktOaTx83M7EGAvZiij+>^h_$yb#OROYYlj{F9t|3Kk{a% zHy#OqXe$H+7%#i_K$6)9)3Nyd`x0dH@m*B~jjXDwSf*faNr`pL?<}tUW)*5up5(2z zFXM#JzTp?CmI-6OM>^k~6IwH#OHou)+P33#b#c*}y?vz__agO;Ms-PP>Clpu%9|MW zd8d_V^J3YnoAU$p8k`jIXB;--7fXfYYr#b)oZPfE1UP|K$34R>OQ4t7(IV~bVp z*Apze4VbsdLP`hchTrnd0o!jtu+IXUS^$wPy-yCHQ6~do=;K^Ozy3|P+ ziw|t3PSiP5TntT~cni*`EWZEf#PS6`a$i{W>!%=>A^jhKBKvH0@ePqa?Be(p69Tle z+yFg|W2rdYhBIlD5kD1Pac^&LXS_Ip*}i+$+4hsE>E_Q-n9Pa_`>}1sZb zrDQ8;)#?R)>odwAK5tW@S}=U?M({I`cxcnDv2gJ*_tDykIj*)dAd(JIkW8rUC0oc7 zjp_2DqCkoF{HQR#UjGK-ugGv4EYQt>@1Dn9uk9!|g!-li!c8qLQ#j2{6%`d1>0HOW zt9I?}>|`^TKQp*zB+Hzx%F7H7(5Y2f5XEmYBX$W z+*{1OL?(b-oi^$^>2g0v!PX;CmRE5@8tlp`qsVA#n5Joqdo&l=GSvFL~W8F24EnU-PnkThzabLSzvYM z7mw<;q{t9o(Leov+kB$sRDN;s05THvmkge^nv!XS8m+WaxUMi0-vr>*-(JhmtCai| z9V`tEDUwp2x^c`UK3bPdX1+R`(9R})tEi>bco^Xa$RI9HZBA)vL^rq9B8>efg0oUU z6X$P%1P4g%^>Poxrd!p_-F?vo@Q(<4&+#R)fLuwJf`JV(zYV^3DZRI)9mHg`!fO*_ z6NOr3-_3?bP2}0lCQ~7~Ic~oMlcYBiEE_p^K5>c}cs0g`SPJ1Ck*KwSGbR7$Og*h4 zJ5rpum$M?wo%bpxIvsxd)>O*V*g&smFvzfgNu%sdP714v*=ROyDDj=2| zUfqsVDXyh#HNB@&5EDZ=L)Zr?7f2?G@)~!?xj##O$U<`ttW+T(`2gtV|C*V7ALT@d z^N(V}ZPKmaRvte3@T%`~9!38l0cAXlY%Wml_+ZH%LQ*(n(V@vc6d0Yue^IO4o}Cju z=f;C3j7NfUX+f(6X50Cu8-f2Y4i^S@d`0sAat@F||L+v!h@M~2As#=}azFc!UHo#p zW?kh#W+IyL6|z?G!BG<0m-+m#bx!KP_)rFhjzBGtOaAvAcwaguOzgjn>ewX>a*AHR zH=3K%CpD2zuy+4M!ZT(THs%htjM!HApzbvRX+>)P#0PZF#_CBMT__pgb+6Omb+?smq#g?6N z>1T_41((#uCVY7lMS0?}pSh>P+3}`V3N*tAjpzz_!(6$=DA92{urOPQe;NTMc0d(h zjzZpJyJWuTN3X=8j#Af?1C^&8k-`djVga`gia9xr*J!xJho^-Z4PWPP%GK!Jc8&BQ z** z=q^cnf#q=Iuz%WYVB*ZiYt0cO+f*(omqO@4trXTKhdj}GM9bi?`3G+A?8A|8IiYEBPj7s?i?Ysrrii?2o(=2Fqi$3!Qn4U<;8?O)Ii(1IL*TRkB9FVy2v-;1RsqT3j1C!WWDX<4qS zyDr{NZ(1aZY5s0EWrdI1-27)|LZX>VnC$Q+!R`g$x0xJ^?H{)8c3-z*1h4w8yuUCM zy_*%{Os~R0@neD!6_n@dbN)b+e_YUt@Ep^ia zmMEv%-52b;SpNPeTD28acqNo3B87ul8-MWXzIEK|fPjE@*q4>s+SK!@KFhv}9d}hk3T+Bo> zu|>nQ&wVy!a2(r+K^;Pd9Pac{`Y8v){D5o!s3-hN<6@?|C6eV9#nzIJC~ZEQ6*Qfp z0UD}IuKA6in)GP=i?WH_#R>h#Q9q=zP=o^>Ge)FWsxOQq{X1dxKK~I3y~y96?R49P z!rRQKw=@!N#f9WT7Uc7w^ui=ORa`z{y4@CoJl1ShXDv;2bi_Sy6GW^Dn#`qI){8aV z&Hl)6ZBM^{8;lBVMhl`_zt1n{MQ=P>oZG)oz|7To&haF2WxhBVr}gowGGYqxMaZI| zs={GX<4Cv*<(zuU>keWC1U(gd|K^P4XxtS3N8#Lm0w1G-=tR!=;;o?Wj7LlTwcEn( z_5Dsl5jTD2zT1_&^Wt)rUpD+vR??hz2bS^$;!PfaW*r{`y((%UdMW$FBqo*IX|Yd9^ABoJI(923H| z7Llfr0~r`h1vHR=0#yDxmqh?xoXk`knzwsk7eB#9@Poka0x@ObluyJZU`@w<~(?NwoC9v9W8=h&#nM`^-DXIQnbLNew)&r7_L-KfUs zsg)5s6&3Z7bnUV7i^AK|oR`k86DM}cUYgh1>0hMtl}W~^Z?QVgZLUyuOhlgLyF4Rn zc)+ed+)jq9Ey(0`O=h-K3DtWtS&SLAP$)N}{RzCKkt%aFrZ?(I>o`*G*ru#aZaAY~aOV7jFFJpH^bsePt7{<+X8_#8g|s-(F$mK*1T;GLUT(MSrF0e?AQo4> z+Q8jbYFR)awzF?vO16B7reSurVKdhDqR*!#b!)6qjWgq zbD2fGd`QxYt>4}P7~$v0HFXPZp+9bClY7C%PRk7ZgG$kN*$n5)&i9Bq# zKIH9xW&>?qOJ{HKG>OFuWs=z?3x;m%MUVJI;4WsRcrx89+xd?_p-JE3a*q9$V%k1R z_ZdxGKX0Y;^A31~ld~5ai-`IB%%a66Y2~q>xk#+Dd%m@nkA-CP^*+{Gh_g2Vyk+ON zyW000c{GQtwA*m`KEeFe;zlCXG+Tep;eGnVK1V^hxjDO3@z}hzpulxFIQ#IW5c6LH ztCf`B9xL-^5+guKLPc$B-KAP+L$>|0&n7my8to-r+LyG3PhO)iQH!!c>azP6xwC#8 zY;QF+n`%_7gGgvntIadYQqO;%Z8KJ`wijvZ{;a=-jcg>?O% z^w+pG&!0wlBkHD#avw2F@ZzHi^5V*jKymcdQm`^B$Db zzH1h1&X_qB&p=X39aUzltDXME6muok4ha<|lP%b8Gaz?K`orpv*I0JSSJ~}?#&zw; z;1KWr?f$i5wrVPww_1SKgrJ6qEx}2O@+B|YE50rdTJc>I*ARR6*=$99&Q zTL^X8+d}$c!qgMb&4QK8eNXhDgm5dmBXfa+@=hw~7$B!6PmC=kuB|Z4R%XXK^_b@K zoNGDn+x!3*4+s^pYOa=@@PkdjmaSh%Vk2F3a`59nppc}CqoI*GN#C~4+N{F=^wdKO zsxp`}GpBD-RvrIApluti|2GEJy}yD4U~$uzB5U?z5w;p&h(~k2t)%U1fFGF!U@N zslU}&00@!Pj%pLt$fkVWvACu{pce8Fp}KP@s1D4N@E-w{AM5z~cFbLRxPm|~_kKaY zp@~+p=;Jn-I|nx^>N{$}WZJNcrh{Dq5tk-+NzOOQY)t|W#lmNMpHMNwKUX7%SVmGh z$~W!;$1L-Ybr0WEH=gz3C(4QFfcRFZj@rh(zM>_3=K~ z?|d652HW`={TB(aeX=}(gr>e)UbQk){@$!N-y^#D7PCOnsILtEtg zc{8LC;u(Syl0IA&b~;<1iKQ$mpiy!FL*(`-o)G3sMJV0gFO;|6^KIZR?)q{@8XZ~` zEUnO8=3%INS5DXyG_|SBR%e*Xq;C?i*4V?&BZ*M}dH`Q+m@&^C=DHbPn~L~@;dzmg zR=Cb@vl=VpHMw#h+C|55N^UUlUOXak8DJ%7v@U-z;t{^~2tAFUq2TD)quZm*=7@ks zI=CxxvB(c9%>L>Xn|S{saxjeiQ$@Aoh062f@dW^#p&{{OFKo;Twrqp9&naMdvGQ+9 zhR9++teoY7gW4!_F7_$bFS|LfVhl z-7U!vSMQm3`~0=J6+2%hc!&*pDNr@^NQ8#de!uR7{Z_s)q__>XQoOqFTq-^@Eq_C8{z7F6{c*h3t(B*-9^$7`GtQP7RsQRL*XZ;+j80a47e-6=iAjD z{gh<{Ro4SP<{jvZzA=s;v|$pK)KL#B(ChJ6&L3H~)jOa~^m3C8yk&Fl;^{II3X?^i zIpS9t8e2HH2P*Z;vI8L;5C<^-4CVMe_UNyZgU~TQ6Q1up>DT0Wek?%-!1Hd!+Y$L|@lb)~+m{OD z2l~^0n%~c@A2pynazLYW%cxzMmm78~X%GVw7}~l+HWw6cKYj2rHZ%`%uwRNdr!_3W z2Q8=LE+p0>X=2ho2VA090`7sdiA_1rirl1wOiM#+!LzjJaUCEz4(Me$&3HTfndQ!v z5?D4a?l_zs>x&Yi0c!{r&hvw&Z-$or++hf7&663PRT_Usi*Iq%(E3J8pj@arV@49+ zAcmbH_(yf1Eop3JW=#J|q9*wE*!A>lqr!&H?`h6#49L%Hyxjyg;G*rfokU;ezw!L5 zQP~a%XIqKiBF|nSyH~2tp8CnzU0Zwa9+d_0rj=#Lk*}E?7Rn&RPAF&DntgnGzXuJz z4lVe^^vr~iO~VJZ`Ge5~2tJ(?27m@nYfPaz|LDqQU7ZzZ6cM^C6!>#qa?$fwhbDIy zVG^&$KcZ8cJB_0}QOTUI+5CF{U1O@juv?jeQg!s9`?**KS5A3pD?cA~RFgwX+Z!6j zbJOth8lItiSODC<<3}1_mKfIZ9(cxYCQ!%xl~%-fUIwB!=Z|dPs~Bmz6di!1+LCDR zcX$aZyT-^IcaRV~=>71=T%IKNt;`G=HkPRzmAz``n7L1%Z2#6-FwMim{abNgs5cMs464@E*J^@UaW z?^SiZL_vG{@X+T06Uv#67T@BsCQL2+W3>}#6O{7ZGe+EgQC-q{{pl1wL;5ELybJ7a zL^yMWZ3RKTtK-GZ`|va~#4e$jn?7kGmA1n?IYP2!qDeF_M_)8)fF6 z7ECz~ZesC5TLL?f;Cd*Kc;?7m1oh^12sOg|+^%#!KUeFzSObf>_&nj>^onT=AHg}| z&+?j%&Wn&DlZt6wj$y#utG^;5N$T12&6!;aB=2Gvc6&Oi_f`+EZ~_+`97>@@znF!P zJeZ^{Cy2Oe3ZBONyuOZ@2T^oZvEX)mNCGxD;juxlO*le-0$?nRA%)|=lYoU->WKFuN-3nrLc;jKfrS7p2o!k)&Z%OJY0gpJ9%YZ1h9$7cyEk~kx z4U#5$We4NZ$by-w&@<*90cKL|X1&6dt`nr;mPQ&8&V1>*s)_E4UobGKTN?8ZodqCp zS#-yuVQAT0=FXbp%~AyXiTaz07aStWx!^dP#!Pn4xF)Q6XH_@yg0ylTnf(t>k3O5 zOOeT96gM?ttOe-2| z>(OEsd@sUTl0%rhgm^5ornDZodJ;b?%xQ#LZ_D!bY?2$~$|8sicX)ES9{D4ZHgSG1 zUYS$7E@^#Emhs1~9htV<@CTTImoq4TByyNYSETkr$snvJVKs;cYG#i$0Ez|97=&)_ ziX@H~daI|`_mCo&qV{IOhyu1se6WY*E8n{njE*jOzs;<(ZFb^bu~T?&q)sgVWcQ-# z+)H+>qa&joTi~HjQqAT`Uv2n6^v8hBVx||8IYYD7g5Ci^%`l1Ew$k}ykuP_*Puw*t zs~dtvS6`ZrO63h%Iev^B`^vdjz)8n~3&GqHkKHC*C2;?gcKeHaiY)ug*_2EB5dq9c zwd{*M`V;ML#nr^iO?>y7{uZh~s}DM^O&!c{M6TWPpC+LNqL|P>!eKiua|QHSF$hH_ zB`9MLw-;nj&{_HlS9ib>8wZUHLcJ8mjjSKaT2Db5bP+1C!M2G`YwpyC$p2oG?c$61 zFD_}SPU_>D=RTwJo_idBa^tA>l?3_Xx-yj^gP$zfq(}fe!R*XG952Ms@I`!Z(?e>@ zlGO)3GHxQ(^{EdSv((j(0UWk*`mzBUKC2UdgA>TCo%JBOIiRc^oVlKU{n~&^JECg`9}1cm@9E>%Yek zsI3F}m+AH4txceKRbLs1P{7qdHhG97ycF(n>-(aQEPG^?FeG+AD7aKlzs zs#A%aqu>sIJG@nTt$q#NGziwf?Jg!v2&{Rip+tXo!N9x{aY6pnPvD^hZRT4ed4ZMh zDv+_CN4O>5hZHi>Lx^TwQ8{%ufvU*apsoY5D@v`}*UXT*jWzUgK0ZMPr(z4N- zF{g=G>f9e9<2=G&6DVfV4?_==reU7^maRZ7uB~_PaedSs!x~M};|RpS)UjocR4ou> zYKz)aMSPylzh>S!gsb6fNcR9{0pewd(^fK75_?^;30t_5`wdDvFf9uL`XU5m9mkGD zt%VyNPz$Brr;}gL-6J)Gh2VfLix78EW{$ABxs{hT`8Dy;>Jggngpu_?&In*9GowbV zwIejVk>xMG7J=weULW&*>2NlY;X%Q|FcfBpYJ}r?(vWr%+1;nVxClxZ-y`dlozMt| zUm}p5fiKTIsg;*ZsYUI7ucr|-;y~GtZ#;n*RBal9Jw&zmy2=&MW{jQ)3k&w;Bvq6Q z2bTa`sBNb|YSkG2;UK4+t+RVTG@A`&?Qyl8mQNAf!6rf}>H-lN-w1D%8c<>vl+%9Z zn~~c8?r8c+3=2y_=j?V5BBA8_46hV$rnOUtcPqnV8#h)yTCXCsuC-|u1}gF5$cB4x zQD-nt44Jtsra$u|oP%Q-2!7V|F6}b5%{O4gd-DAV5n0o$!-a|k;ISDH%`YE$p{bK_ z5XyuEH;V8`(i)zh4!EDvL1PX7_xPDq0{Ki~R@8SK^}NFz^_0rzIvGW{N1uINSIa&0 zLWG}a_XAUuWe_fQ$2L^MQoHV)x=}fRRNr{vDJ;=Hfjt#$=vTpR5T%M+{ zUO2>Fg<_)~aYHt|UFOQw3RNnfr|a9f0;L}afPGiTXobSQ?WB9{(?oz#ujX6ZhuU4$ z@YY8mLE8D@AjB~aWek%7;s&*j=0n#q32|fC%5Rxof_=^0i$$8B$+TO%;9*?L+p9{E zy{!MH62GFC`ih0<13WS~W;~OE8*Q3|J1<}&r$9*#gs@%>Oxogsrm`6u zcdf;)HJaI<9HW4af)pUGM$bmlu!&S=++0ZB7r9*e08n6{);BT8dr(y@OH+u#TW^Kd zxZeZy5~wxZS-cqpxLAV20{k@7UBCNQQ625=nMB%9Tn&FBKm%Vpy#4By_J;L!+9tBhwodVZ>`7{#NZ)v!99}cFGL}LaBeFVkV8N= z(=qx4o=m^lr`WhI>W%<{9`*3MxC~f`z-*m9kfG}&qCEL>EpSKFnMu0PXh)FGMlfpr^SzsO%aKPzIyYRSaot}NVjb2d~ZgCw@9VtmED`Sv%}Mo9Q3y9 z1@WMr3Toi$db8e_FxCE(ArzZyxwVS->(6@~R#VWEED`fYE-#8EI`~O=*pfcPzy{(U z(W>IT*@1Hgg=nOyLn6s8QJBER%cOh>G*l@jLe^{Q+RH zuCCU((}tZEH5Ibb=o+AtUanJaMnoyEc;K741^c#!(^qsT(o(HH58D6;-XOeWa%%7) z3z^BnQkP9}v)S(E3}u{N^P_{}CQFB5q}#FeRqz@(czTU$ew_WC9WvsLz7!F6u-?AD zzEP2cK4IQad@f3`)OwdbQ)WqR92}Z*mCJT(JrTFd@c_9vm0C0EtI5&P_wL+|`-E53 zpmVwwhJc#UW>i!dT^})>SYnsqfOKpq^Z6=!z7;rEJewEU7t{I*ch)&yfS29l4C+AJfU0M@6dY9t;*w zt}d4uM%{8qaN+Cl7M9p$_h|8y^yRYu>t&868w(6tLLCL?Fl5k>h6BRN8y-0Xrgb~*o?k*Iyo)18pPdF zv}VhsWC_cW68N<6!FGs(S>3!U~$;u5kGn!j@n>2u;(Ds1h-on6<@6U zt*tlYkpn;mm+)yG5K6ijY=FmjKUTWfq&Z7!4@R|XjU3p2*#ksX-uEc2F4;Y|C$A9C zv7SWst+?ioxT#0nR%LdBw9x4b8F`woeMc~BC^1gw#@j^*H69$8;gwq>z+wQ=B4I=n zo@EjVr}t|Yb<4k4CLT7Lcnbp!5Ru+V;U*T^ zeL3tivElha??nvR+_Z(@1TBiEuwyOV6(_dj8t;&Q*Puj(&iEpcBxdN`gERjex!6kCj0%-fGvH<%(xQ z^2ZEFmWBFqvo+jLheX&Cq-+EP8#2O^H||*WU$@T9&DmWY=8J4W)Wt%8E9i#evG&&@ zc_~cjpMadRolLVmYihT)>doPyO%TonCk0vjoUMNHZpn{_AD@s1R!+ilA>SIYZ4sLJ z$M6$JK2?A%0JZdT>BLv0)Z~p^N(J_})%?6w0>*YL>M$r+0bWtk*#sP>2EvE1Vg~Q* z6h3|0sXf~PouAAuy_>r=Qx9%iDRhN&qu|^qByT_12>h@f`DHSa9fHqg1TMR5K1yT> z_EROyW|T14D0w_OK2ERR_{LU)QcNO-p2L1yVN-?8Pw)u9zH~CP^D;CD>SkhMvdL|0 zYYW;vH`FQ4hfT1xIj%rK4)| ztA~>}p^S=8TOepi@*h7>46RezxKLeji{RUSL2?)FaZpjiT(2g|Elr^57)u4vvCe!` zm7;hTNmHpXCjM~*5E%e9vKgzcLs*tvIs^?zbwDMl5(udKsEdrI>$n>@>;Tg9TqB>J zFP=u;C7`t~KVRfY8=VIrh=?IM<|o1Qe%$mHk#ZSp*(MhmqJhXG}aCTh9+JQZtx3n7GOzA@Z$OxY7vry2`~aL$=8tBM5^NlBv7 z>vD|Zp7-JMMPG)om4669e#ZqU zgh&ukg6m|kgmTj!+FA8?zRP5}ILhnZR*{8V4x%P?Y|51tbG}L#*3Z=W^Qy&!8gaml zj4oVw>fB*{E&`7WJKZj4w6h@tf?f4;)5u+LTfisH(YeW0BUm{TO5xYaQmJMuL;aHL zJ2S{RXF)c0!PMxomrQVC;3WByT(W5T9;oIZwF0N-Lm`{o7DzU9nt=@TYw^xyD{yqzzUIe<+7k=A07zTMU)e4a-x{J- z-hi`I(Qf(E2vyNU!H_#3aRBFduB&Do zi0j|&6JpVM)Amp;QABsVCoje9OQE4agT0p^)TrWpuE=1F4e`EodgviBf`j3D=rabRH>a@ixQP>IvoK4GF@a@7SXA+gi?EuXXsT^hY94G^Jlmuk_7le+Qa=y0q7kz1q5!XF!)m?VT@N zSh!CdRLQYmWV;d6zrYF~8u=yv^={BW^Y|^Y0hFd<>){k_Heo6*?W&v?O0mc z;l;U0q2IkAS#I&Op$F(s8dhj-EG43wXp{)slB6(`qIN=6c^Xyiy+y?BRSx;^0-wV{ zwkWmipA}!hG>6|9O!P%S zbR{V0dMM}hcHk6aiUm7#fTjZwI*$i`l5!%8W+pg|$B13i>`G)F(w%QrzaJIOnCUtpNzQ9u}U9JC#Ha^>a?E**<4M%T`j zVykt47MwlY!w0;;C$PD8*u|W|6K*g%AMIzjfs7Sb6Fw_xu>e{-crv@`3Fyrw6bC>v z9HdC_%{%$;mg0`B0H5wy&@=`#Nf!qn06H%Zcv_NKUd+LlLyeymR4CH4Z~LJmy@N*Ko@4UGLaJ2PSf-GW@wwre&$OhRkxrZ%f2_UcT-0 zrNNV&c9j;|_8UcaeKxFv^?hwo-a5fUH}kkVqv2c6vySifyKtggQRxreqivH!5`xmj zmeLLTK%3DxLUZ${B>B+yi2B-29BYp^%#!E>AMxlGiTtS|w@yw5RO<8iJveL7(P~t5 zI5>}#XrZmy+%M@*{!u6sDpmG02TKmoM-p!AODFbs!}?){WE#SArPc}F=-Ecj6rShQ z=8#kEc$GSRu}_*`mOwHRJ>NwL%UBMZiy;A^9gEs+T78;_T%<^4!-tc|D!3x}(G&Y9 zUuxKjrXRV#+hSOhojzJ_MPUufQh#J~Ls9gK@INtZxiVy z<>M=+Voqr$K>b?Rl@ff<8eTFUS%L_QOfw!s_{s527ii)GJbz)6@H25{crS23htmE0 z%W=8WlOTH@@0b$J*b+p6Q!H6(mhJMJOeL^KSS2Rmj41hizI0S;s0 zB?*i``*fuKUMNM=iP6L%+|~CJB<@whkI14Fbt)H%ZaY-hS8cV){1THz5|f`vTu^eN zO2@vFb(2=}bHXMdp?oWumYnk!3tcnJv=H zf6sy!U!t4wwS@fA1&;b(OZU~588yLMa?~XB)5hHXDC%1rRSjzYLppX$wtl!CBZKo0 ziOcD)8HbOtY|BuZjF+iyKeuThHa~ByXp_4{Oj8xiLtcoS(%TEo#4H|oxKzEHqY$sa zW}+^YDDsM$*qN~@G2D$gzGb2Ll_P8L23*2(`CyevywJFey}%jl!RrP#A|%jqqMc~r zv#f|vU@>HQvjfp4XsoXn=jB3HT0`cm#_6wIjfiPehkNnWE9#heFq4;R60=8i$bDFl z$J!F>*uY}exM%b(RU<$X8@R^GB$n>=j6;n3pWA<~tBrqO{xk73U7A(aZL11RPr=x5 zNte!OsoC*ut|1v68eA@jx~!#F&a!&a5}$1Qd)d#7%~8E=1wOKQB=e9V^dqLBdSM%X zdJ4DkdGHPCnpNd09Cw*}`|FYS_xa#@e-*R}lGc%&7%SJ_-yZ;nTRtzzc znxx~X2l$k`<2CC5lw&`9 z_dPoLt?n|yanFyU#?Wr#Vlm0(`Cudl*oud~j0QH0{5;>+cCIU*%yzq419KUL+DumF z=O+>K^7C(bsU9>(5q9P#(BHbyi zAl=>FEz;c}jevA_Bi$g~pdc;Xxuv_i@7#Xh`Q3IdtnbhI#OGU&%LGRm7YFDou4?TkT+8F@0Q zb-I?~vg9gOF&(P`MAtIS){uUpHQvbDH!tn&M#cH}z4yf{xK5%eAD&v-=;LZh*J3K^ z2Lz3G&n=ODqKn@12Yg;dSve|wWOi2Kk&kjEM14fh3+jCuAR2(oU(wD z()b#AkMpXA@ExK|GCg=CTf&$zhYulrAN-O>`e;^`$Y5P7315VA0 zOv!sWk-VfTvr|*ubdHUm%V?5^mE`zTv^}Wt@psd5sEPgwf5luyF1o3-x6nM8PfY>_ zY*8XgZCmW{UW2up{%a@Q>#NnsZRw#$TxiQ`;WCf=Wy}4=e3k*k?Y0b+?{0+X$|dCA zK2u7u`b0giS?j~d_85^htTjgU@5Bz;8!Hr{eR8mznYc&)ONVNyP$0_t?to-^o*x-} zwPwnsL^=#u8gfjJ+ZyA2FxTQTyC(Z3K`UBfqr+kioO%9)>wSHn7q09k-a*Pk%^m|2 z3Pw3bmEW6TqZ;zPGunyUx3Q$!91VI+$mML={Tqi&G~%e*Io;O3oa_WI<-zwAJSri% zLrwsP#n0jPQ1Sb@zdf+!ENy+)=HxXAIhG=nvB7t&6nqnf_)N4rWYni~78NoXOv!X# z=|VFck~HiNzC4|HP(wX`SB)+>a)H@aX*cEN>3N?mh82DP_{SF>DHO3NCJb|Oe!f}1 z^SNT#YbqJ-$2+&CC3siNBeBJ$Em&s@{fhl=89Y=X4(XSH*+?J)u9D5FK^tc+z5X`N zCTiAZs-$+H)y&Gzq!PLDZ&<*gm833;&!}MLi*7mn;r?@_;V#N9sP?l?t={vDXl(p$ zYjRpQkNJ8|H)-di_uwp78nQS!I+NVZqqm`tG({uva!JoSy^;}UK}KJ~0ql=Nws&{o zfi_wCxL{z_i*C-qWIO{O8y+0yvue#y+ox6u)OLcwCoT#}ni~tR+yo!jH;aO_qJK`l z=v+IsQWKrA!Q@aa)vAxcMPUvMO2xd1KD@-kA-3RhS<=41zq+7-dnw9$_OyBPJ%$67UgmaFCok?b;JRRNil5FDIjEr>2e*Z=g#{}=V#1v z7JeBXI}&a~`o>+2@c!kV^^CS(F~Gnb;1!nf@J#mq+wSHl2l%2(wYpZpb<`8OS4!?V z&)Kz*46OQOi9y?Nmhd<(V?4@r1GMewG8HQC2mexdm+mrRV)W`dBoRjf&`XZoT;8Gds*O{a0~5ZLnM^t2L()45Y5??qu| zJ7K3snczIarmrA8J1To6!v}4@;FVWD)i>Wc7$6^&Q5ghysal{#d>dq6Jq~VNSe>#L*~uO(iQI8R;-G~M5oiz-RfVojkaIlk4Ls+*-{qucvQ#SXV5`h9KV8q`fenq-WI^D#CwpEtzThjYe~H zcl|E=rNf8oy)Fi&ATFrP zw6puAh=@q;6zCKe^CDD;9ckC=BX{fzs{Z7G(R2}ReF$J^aw7tXsXG#6hP~ulYpW-i zE})bz3+*tMk$DLnZBSEJSFYJ=4rW+PXh!FYKyUt!SC!F6wV0y35S2G@*5P6Q&1FZ> zfxm$58J=SXfdnx_<5pT<|MA`ebZnoU>v>o)N;@6xOPCX%k!@?qCb9o~CH%awoKlY` z;9B$R6x#p1>e7CV%0U*}XFVfM5$tSk|7VcM`ueDYk)LpVQUv>)Gt-6^`}G*n^~rS| zE!iF$?HOXxJBJ|S@vTS$SFiR_I53sL{c_Po$l?Et5RTb)Zr&=T>hobq$~RIWJ{X&P zoZ0Os3HOZh_A$impL=c6H#5C6hRFH$u!p${jtIVQ!P-|LoK}PdRfn^C^LcMA4$rBf zbwRmMg=eTehGNt9{wR zfAO{l$&jp06yOmQhpjWtCT?n5vuP(C^GPpND?~AHQJ3diG5$Jj4m^aM{ws6;N|Zgg zh|F3Oe=_YiTjw_lIbsU06@xv_ zwQE6828k0sek=7WSC|-h>$G~`@nOo`!}ilt?$oIJs23;9)*xp^sBtE~#ih;rcz4QT zF%<;|&oNR{QwN>%LX&wYFOIqgY4-+k+`cjG)cEl4`F;XX^h~C=M73hB)*Xw-yf8w$ za!728(`Zi=7gD~uFq}nHAe$?DjuhcM zkTjbtx0g^a=k}e!+?%NchW1;(#j|p<*8$}LBQN7AO{=qx_H_~Upl&=Z++V@SgQdfo zChUJ02tKm>+yeK34FOuI@I#z;>~L50v( zUta}T7NjFdU_cl=zV^`bxb6ac2Mh%C*gNi#&ES?;cOXO*f?u${ylq~}eDCm_V|}DT zAgf0-&Fa_nt>3h3+x153+U5%h!yh>I)Q9o}ji1v0cW)4**FEv3H_Ss*TE$~)B#Q&} zHwl&O$-HkHI{S)$oO;RUAAl(MTCRd|H=>a^G7qY%x^5KAe}nj#t9q+3iw47Wo`@Gr zkunWUrh?CIo-<^*aqEi{1(IF1)1SP8LA^SX$-rOi`%_Yw9 z@b$*NcX%zhb%B`=k>as{f{;!mqI{wrA*?@IueOgFbgS5zue=KHd_9_4XxA2n44H+n$Q_09 z5Ed=(?wS)jrk|6lvYC+SE92;ee@{hR{TKVGJww7`;P$+6+)_V@{_v5{dmc%NAWhGq zjhSmU77Pla3|`9fINypqtIa<04Bmnrf|D&p3a|P(LUhfViEd||)g0>ZA6&uTA|Y;# zFH88k`K>g)gY}DMS!%R;-}m~zz?7g*iNi0Ju*IE+@?}!Vu^1-plQ?XxaUqFuhnHGg zDN#l|55HE?F8_D3P~)GZJO@I6a+xsSdAyXE_uM?!Qa*}wR3;;>KIa4yQ5G(Ytpt>*poHnH4krRuRy za6!a#4Khp2P$Mv%BxQhz^Y&uiX7`LUPqGn|=bVs%eE*q$`8`VLZ9+#bNORb^zlH-~ zjh-V_#=^p4%Ob|V+w}*U>zjAK?uk+1{EU-nj^YvCBtmT9UjIYdah14QT}Ub8^MNEH z0;Z9H2^MVgaEBZbgg_-D9cL2DMZ0K2WNdT7xc8XZrOx0aSToCL-F@1jE8jFVN^If( z<+l|mcgm9E&hvP<-h_fwR?oRQt9g7ZqBI;@pq8ms&z@J4J_9UeCN-gaUj})mmipC& zZ%o!~SmwKj5&aD`eqs-J{EG~<7w!Um^3RZud3rm2Dz!rSXOJX2jFW%=$BH|x`<6kG zrj(7cv^l{iMKdamCyKuk{OG~Fk4v|l`^;z568*77ppX@5vkj5#?~2){UpwKx9MYkJ zs)!K*m?n9@6k2~?r=lOIwY~`d{~>Gg)Q-FNAp8QkK=fz1y{;=@%*dKD+F(1EIF4dW zT;v0HT=st_Yew`tMerp$EKEefq@@O2BxJK&DE(D2k5Bh-pz5azOd7jZC{*G!MxYL? zrU*o{aFIqXOq#e+1*H3^n?h@6{;1>J^_{5@#Wa;lv;xnDY1jA&T?oRTd;@l4{kxl_qbn#R_G!V4d2 z>M_tOSyw4me1+#NNe`0$W)f6kzl&JXAEVm|m?lJ_lhzpq0Lqw6TI&w&H>#NLz=tiO z1jU*Bp-y^#aiLu4E<%m}ev?1_i)<>#yu0T4s&96t5A<|r`2wUArL3>(zQx6U=ZOWm z7+i663yB$DmLg)xaX6;TK8hFO z1LAObU+%O-pVQMb2lu28AM0+S^+dexiCDE8+i$IZj4ZQdkFge?+CFg1yxhI*-zDvf zEEOB(x#ZT9YAQvai}@Pgjej24Bs8uzxt&X;1 z|FxiGi)nl&yh;9BR8&9c1dyC_h;!u-^p+EkD%&xfM>AxYr&_N}A4))k3fvDWFec6;Du?V_&LyU`e%F^wT(D=SLJ!(t!QFdwYhO9VrnB9V#zk~ z^h2uQ7bTzGPX20P;-_Cn>chZhM&I&p@?;6}t%=DN|5#%piN9{HOhPE5H5*pgSbuTP zLn2UD9BU;O)o7U+7yID`R?nI@soK+&m}+(?-;#@ly=pSdlIzK{sIuDLIx?SsPyNqe zYsPH6o58z&U07FSgdz{Bn(|y8vE)RuvrGS{v&FJ?1OgjXo{A{clZ|9{;I$2JNBCu{ z(q(3ym2C}6aKK?B5^Gt2vxm{3sG&egkAOY)$lTpC_v66_cI5H(zpx_>9_qEutM5!9 zt;!4d4MMf;mS+`bBhQuig@C(vF z{SE@QvNm zl+Z}dFhcXYIQrXrcV@lq6HjLs8Nk1rjxJ$fXjm1#HXF5!dHO#MN)W=N`DOflQZG(K zx`x$fs~>~lpeX+f`=m?1Pn|1$%=g&Kvlnnz(lso~lJz%^<+Me7<%(^Z@WTIF?;Smv zBjc4VTRX=csl`oc<9lbQ`75|@eIJ~l}4fK3~-1GjGnCamaWZN z#QDS+k2IGWLt66EiS$1)P;6VL#tv|W!dZI363G$~ihe`FyQEEZB-|sI7{#^@5fG|W z(k;1se;Xa<{t^>Ir5SZ4<#10kxIlTiM)lvg5$vPlny2|iTZAr2txrzi&=s#`|MNdR zb0woPJKM`+>R-zGa%aeuTD4#Jr&pC3xDO2ex+&862$(8KQz=YN0zX4m#WoFG9)h4M zHGabX>h$;0F-}IC3p=}YX>#@maC_zcGspBXCYzd38tbnQCH@+JU+z%+vRpi@^@;t< z|LQuqC?@RLwlJ^hGsY|YUoTG_JW&m?jQx8{VMa9xvh-L|!0l6^^45+XNuUFVSHv@7 zlsRsQ(qlho^926C-j2K6p``id^=cPWV!r>?2l)2QtRu8H_1t@hZ;K)DbrD+u_F?@9lRGHr zPP<#{X>r=SOwpvIJi%DJrM2_Y+nS{e5?zd>q8>%I_`UPOFm*fMty8XUtXg@3grdpD zn(*4NAilZMKRPdytrrh2H|tvlCUu4b@L-kCl~4QD#2a+Xf%(Gay}x`L`L>JSi}UO5nq7|2B1{m>CLA zjtbnbr?07a^t^wV2tlq*he-|(xp(&Wnbm5C+=`ZOz00qct3rOUo?L1R4p0v{xOE$( zRCx3LzQHFwK4%OVR?k|t&Lm~x>&m9y#wR#UxxmXr$++1xKPVPN?i=Z_4~kYq)OZes2S92fA4$p z2z+*r+DPWs{ob1>vPKDU*C%Kvs>0+e=?nvxbzyYo@9rum8@?~u9;@Co*zfoZlC9~T zq|jaOT5n?Jv`@d5xcv@OG!#8zA99iQP%8OobpR1%jcl3F->^<26@ON>t85;^A(pae zkk}W+UhQi=+o`J+L~=c=H@A8v)t=g_tQ%8bf3jG2XsO>mEsozHBE~6ka1j!wg@>#( z;K&{SUE>nIEV{Y1hQw89J*H*$EuAf52**Pq3t`pQ&Hcwg88V2PlIGAw)w2|eY<53s z2ny{afkOXjIk`YUQDEFwFRP5ZWhv3`f4m#cj%`4GoYI=!1!C{l1@c^R{_+45Rr-RDlK-(F> z9o&-#~n`NC44k{skE3V*&Y#$4;|U-`kemloPO$qX3&yD z(L^>-BqUXIY95B_!o@1z#`}e6JSoCxn9A+(_)QJ3JRMxGM}WUF;qpdqLxV=K!Q|N& z3Qm*H5Epexr-)n{v19FM>>}@jz*ic*OBlIF7vD@i@el<_u=n^dGJ(-s@Zu6fufn_% z55t|+scUoRe=cc`5-u($=`=D@alMbA$?;OP=}_x|d5eAB!WC3Og})2iPpvmsvIWa; zZn)xX$l>{UR*j>`#%fANMM0^*DE^*7>Mdqa7v6r>H2eb6Tq&?Gu>=JNExO^$M&%9u zPd5q3`34&2ikZ8oN67R7Uk71po5R;}lbjS3)i~tMo{9cym9^7Fn|@ zC|?vkd~S2-9sWBjuPJor9RdUu=j_h9d2Ffi(s8csr;F>v&n`z-3_pBEoGY);@y#f9 zjhr49KbZcDIl}2L9yX7M)}v)J+b{mCzT$V5mJLA`H!dvflqw3{l_A=_k0w79IU#vS z{@LzPup0CH^>s9JF|vzQbs5fyV^haMJ!8JXY+Yfevr=lj(lIeDdG1NXw=|6Hi3pkk z%Auaze^wl`SX*zJ8sADSExTge8vF(CQr&Cz3Zp;+Ae(FSJx)mAWkh-oJpVf$+3 zcnhURW3oF~<#?aI2~Nrpg7fIo8)@+z79&(zW4M^<-Q~Wlnt~y#yLI^TZ<@|H`;eCV zf+ACwL`R$W+yBwgpo9WB}!8=yf@N@S>Q&eM~JnLLUAs`He`6hqA z)O%UCCA;jNT}XkUHG9aZDx;@vvcP{GcdP*;|B90Dl~aJh`o@pyW*s%Q)X29vvJ+d3 z6{ExIlQ&-VZw}Y@-apI9Eq;_)e(%BI=dYa6Pfpd<%5ARP%Awqg=S+VKNh2^B) z$8Q^u@-SsgkgTwpjDk)&Ip9`$9&I%7wN_M{B>u?0efRRKcRQi$cqzd}4BP066)U-_ zKdDfO5f94A_EGcx1uRi{!jre0(N|kNZjZyeGKS>WzZj_86nq4QjWLI7QX|~lZ;Muk zksc({kE0199>;jK;prP0)RN8;`foYE34ZZcevnL!bt_Eu{2!Z+ldR0uQK^A@|D1f0 z0zEV`qsghg`2y2P-8%xFL(Wz-KI>;f&G_3FM)@!tA>cr-IG6gqe>IRh=PZa*3gxtH z=6-TO3;#b^IxBN}2|zlvDIJ-v?t{2$+p)uyW| z$&2a3aB}N~=F(B=jrpsDR)0*ftvE>@ceCX!`>&fHlI7;jXG*`Bd|XeUMRzY6W|0wL zG_6K-`~D2(Z2&#KyXOy?0MY_AoCn^mV{-QyCYZHk!|_Ue?=x>VNDdc1)+7!p-DtX! z)z!W8tvlHgAsg>jm%CHsZ3`}08?K<2xcGXrAqVvs{9^OsLm)u}049OW1ZRoU22xP( z1~1yP=|62wk2Rh8R-2Xa*+w_dLTQhaSYWtEN>w7}6Hj6{xdN+{kcS@P4au=nkyS%Z zP9d3Li^@MCjy~CWOz`E?n!OFjKdo<_4GUpUbScLxZRwVFUX(5Ew$(1W+2=ig8{2Pu zs?<7!uJnU2qOv|9PMjd7@`F3IAr89YGoRGhl&iD1O7EI#dhFJ&j(ib?|2i-@WTq}J zuo+4+YIgRa#25~%ga`|zY|$JTWaDAJcpjf~j!m4K#RD&9WD95&Kel`5BXqAkVxRpU z+X7qK#~$B`3ljo|LmSh#Q)zzFg1f5am*IOm0rQrSG~ZxKeh?|_K_ha#i+9?F6KT7Gv+`=InQX~m;tcQ{?kpwNWp$7A zq1zl>GMwDDMl7~N7z}DAmc>tJdIh!k+QrR{i^WWNZZ~j-QEp7wm4~V>E)5T%K)V3;QK3DvAa>I1S@4arZr4*363NDjP3BOPwlT zUnA#`Q$pBP4%QTojcIMed(^bF3M*WFMw(BCl<$>fT;2@PW53z>F*|86-C$-1uMsgZ z?+b#>+W6Wi0g49>5&tOZY9Bz|N;Ky^DkE|S_StMOy+mWOJ-rh`1_&)Pn<*8w_&Tx# zCHZ|GYm+<&xZF6N-iBeoI96{ZjDSBO(YIGrV@++;D+l9Dhqfct9_sqo^U|7X7n_9k zI`?hMtEn(TA=5^DP9?f$vtJa*b1yD01%_)ik=)I6ckBZH!4Tf&KVk295_ryN|;!|M^h z9-DdwJ?KqKjx1i)%)fZQhAmf_#YVdY2UR1cO%ZL!0hq>bN0qY1oZSLD^EkttyPYXx%pU z^$cVKu>$90fyc`MLh?8447hOeGGCo{xn+%XuL@ zOxrN!ctaO8xm0(_aKlSQ53%apr^~XL*26x}3ozt^rPr_sMgFy&OSGwjdwJKth{D@P zHaVNpH0M@r|G+l6mX4t}s9Gc|%HHb-8sI&MBFg`A{IWH0Up`)fT>atZ4OcqBkjG;x zywAy+Ip;?3uaeTAY8MT_hVaz{h`=beX-(COW}4ryCZl019-~(plL7V5k9|%{UnfM_ z-;$N?+P%8QIk?Y!g&)!N$g+3umG?m9)Ghb&LAr`)4j(Kvz;X9fla8;9ZF$)jNDf`Z znTdv_BQX_;VqHCq&&k`D!JF%Hr{wZSF`p|}#QqlSlivgOm3_ba7A7o$593;{ISsH; zL+oqE3WoytB6FDJwLZt+0D@T`@wE-~X>(osl?8|p)Bz}JcvQ5PI``Zo+Ki?N1uMij z`MW;`#)mNj5A!2Eb+(IjNsGYNBKIinZP-u&kGtDZA`=lP=U85;w{EMwG*<7QmhVNF%DcO>hbWgakH(z`G2D|_s;pLrVBE^q5< zGe3)Ry5=C5-$wBMcFH?p#aGe&Sw1)-?2K@$s-0_P-~MYno1}a)Tu-4HqIdEDC3$X$ zLO2V1eHW9CNc+N}r1LHK1eOnCw?|f|-etoqEinRl?hAu=ZjK!Bfog>3ZltHR2|$X9 zH07o!`vcq+Dm5g25Xw25xva#~;V>FGwQMyaSK}n_=>}&E;EPKI$V*_WNFECeeyX>0 zp=q{GdulbG*OUeLNjid);Hm1Y{Bm;scD*?joGn8!^U89;EV#nQbIT}+)4uzELUo)4A-3YcF7Rl%jP8a% zE^Vs1q$h*=bW_&^i;(G={6eq)={4_kq(!*lJBffepsLi7dxNjHpdngwisrx5qW?Ml z9Ey7jly^{Z9;alP#_xhI&RtIrDhhB&4Q6+162xM>dw8VL(G|NQ5QY2vFShJ=x>YSY z{k05{%HfrtGks}qAMn~yy+_HrQz3DWiZZVWj8GWAj%`EkUZK6!0U(=#3qL9LtyTeT zOKH#Xz2to`?mdX5lMTF2zBOz=8D$a5xYk5U_SRK5xpSzf9>V;ERR<%jm?hVPznnCO zeZElb{3eZkjRppc)|Vv9mZ357srgxm8bYvxiqW`tH>{i*!?-jK(AZO7;AGH zmrb$FmQHtz=c54NKtO3FQf!!}jwh89k7nCWo(y>07<_4$3n3PI$@FK7u@LRK^q3Fh z32=zuU;!mbpSR4NW_mU|J^zINm^f2ra_Z0P<}^cYS5s=I)~SBb)^3X3+-{R@y$2>5FG&GFKCbdXv~kwQAH(D88C+`k1Bc8; zviVJ$@hU1|g|alNx~+ISceCQ7d_51Od2@mK#D7BXW)Wb>St*CCA zHqzSVs!8)6M7O4G<&Sg{fcC84Iy&ok>uN9lQm#JNk=uW+C)}~jdKid47 z^mzBv+Ggz>_R>gZ0~osDmNPK=ZQadVGF>LL#chODXIURoVdD%v?7IOsD3I!_ISo_^ zXrxbY=d>E$b8fAi7P<*!Qu1*`BN!U_#whqig4RbNk1AK4JNmO9eS=%YZMjt)fC`Y= zY_6PsTZ5AGBHc~5^ok|SHQp0-ulDVl=!no(3`%$e>OW#PZR12G+GC$M8x_4(YnRIe zBJoh+dcP*#>Htu$id4>{Pdn$Y43tiVaJ&8QiLHX7b+(p$C_R9>)C_aqy~bQa7-OAF z&vN2q#F1HQ9CS9z*#;Xox{l#`IwU_1ZVRK?yZho%Y1Qb@P16$j`5>RrHxVbieYF?& z%ULZc`5VdxCma$?3~}Zso$@2&oX5;g-IH?fhIs)upOVzUTp{-MDRwF5i*--<#2s-l z0pFI72~ot=N>wtX<>pJ%?$TjYF5&0o3!m=?+9*UkN-vogBZ9Ur4|elk&7w#Z20auj zC?ilO9DNo$M%%x2s5q^2-W74`25e`HN?K`@Dl(Et{I^A=Ef=h6lvPrAAF=;kVxM0H zu5B!XX(AP!qcdJm*Kj#&)PZm76G~XHWr{I_Yn4+1JF>U-7CBT0!0zc%>#qG!b?N}&F+tmKd|&E?SCsDeQm3h zSOxG+NeTV5_Et2CuXvVCfa17}{&u4fXmJvB3nbfBgzzpAH+$AtE{Z8Z zGTmXu1w028ipGMSp#%I^GoA0Pb&Oc%(nt=HR1V~-`TR~w2DIOaC9nWuOl_HmBXh+Pwu}%K47H#AUm-qRr}5^BY@Xb zhw(b`949eIYp!g;umbGEwZ0N2>;M5@%_;_mC@)jPBfve1ZkmT7n3!Y0awi%j^&y~gm1QD<7lum>m zdJVYJ*98LhaTD)lR%PP&5_Tt$Y+CG;=PG&{z znVW4H;#v~JD;eakE^ZyA%O}tKdvIgQacv#MmdOvc4I>v{+bt$aR8H6#*M8wD>V4PC z9m|y`;8C$w68dQ^*uf%fftgl6$~t!bM{WotH`PiK-6jH}o_I=3)^o0an~0k z>KBHs){uB%&s7&|x7-0G^3;fRK1GC5B~|8SLzSdUE)DO+(@DKB!r5b$rPGl7N-kH4 zC=$~Zdyp0C&7RnhhDcL}b0`0bZ`Ru8>+1gUGfCAIjf zqR~@hu_w5Nx6OM<^QC)v#E9sMKNzi0#h2zUt?P*ti*Jqmw)tvMAzQSO0Ldu7x&MMv z&wynhXs#OyGMKU#V|4Sop;2-Rt2Eu31WRk+z2%Y7FHsI{52d_{+NE<^SQ-v*s^FV0 z7W@0xgk|SBoqz6SFi8CIGBSg!ZIDHx9u@>_4RjP{Ql*Of!!KRN_UZ=4Y8UE=Q}|T<8Kx(yD|