Fix rebase error

Author: KeisukeShima

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"

obstacle_stop_planner_refine/include/obstacle_stop_planner/trajectory.hpp

@@ -31,6 +31,9 @@ struct DecimateTrajectoryMap
   std::map<size_t /* decimate */, size_t /* origin */> 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);

obstacle_stop_planner_refine/package.xml

@@ -12,7 +12,6 @@
 
   <depend>rclcpp</depend>
   <depend>autoware_debug_msgs</depend>
-  <depend>autoware_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_utils</depend>

obstacle_stop_planner_refine/src/node.cpp

@@ -22,7 +22,6 @@
 #include <tuple>
 
 #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<autoware_planning_msgs::msg::Trajectory>("~/output/trajectory", 1);
-  stop_reason_diag_pub_ =
-    this->create_publisher<diagnostic_msgs::msg::DiagnosticStatus>("~/output/stop_reason", 1);
 
   // Subscribers
   obstacle_pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -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;

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(

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*/)