From f720dd032d03e21cbe076288d519acc846944012 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 24 Mar 2021 18:26:17 +0900 Subject: [PATCH] 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*/)