tmp modify for smoother

Signed-off-by: Takamasa Horibe <[email protected]>

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -40,6 +40,7 @@
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tier4_debug_msgs/msg/float32_stamped.hpp"         // temporary
+#include "tier4_debug_msgs/msg/bool_stamped.hpp"         // temporary
 #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"  // temporary
 #include "tier4_planning_msgs/msg/velocity_limit.hpp"       // temporary
 #include "visualization_msgs/msg/marker_array.hpp"
@@ -89,6 +90,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   rclcpp::Subscription<Trajectory>::SharedPtr sub_current_trajectory_;
   rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_;
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
+  rclcpp::Subscription<tier4_debug_msgs::msg::BoolStamped>::SharedPtr sub_tmp_;
+  bool break_path_ = false;
 
   Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
   AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_;

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -72,6 +72,10 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     "~/input/operation_mode_state", 1,
     [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; });
 
+  sub_tmp_ = create_subscription<tier4_debug_msgs::msg::BoolStamped>(
+    "/tmp/break_path", 1,
+    [this](const tier4_debug_msgs::msg::BoolStamped::ConstSharedPtr msg) { break_path_ = msg->data; });
+
   // parameter update
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MotionVelocitySmootherNode::onParameter, this, _1));
@@ -501,8 +505,16 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar
     flipVelocity(output_resampled);
   }
 
+  if (break_path_) {
+    const size_t closest = findNearestIndexFromEgo(output_resampled);
+    TrajectoryPoints clipped;
+    clipped.insert(clipped.end(), output_resampled.begin(), output_resampled.begin() + closest);
+    publishTrajectory(clipped);
+  } else {
+    publishTrajectory(output_resampled);
+  }
+
   // publish message
-  publishTrajectory(output_resampled);
 
   // publish debug message
   publishStopDistance(output);