Fix rebase error

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