Merge branch 'main' into feat/use-resampling-function-in-motion-utils

common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp

@@ -44,6 +44,8 @@ AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent)
     connect(screen_capture_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickScreenCapture()));
     cap_layout->addWidget(screen_capture_button_ptr_);
     cap_layout->addWidget(ros_time_label_);
+    // initialize file name system clock is better for identification.
+    setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds());
     cap_layout->addWidget(new QLabel(" [.mp4] "));
   }
 
@@ -104,13 +106,6 @@ void AutowareScreenCapturePanel::onClickScreenCapture()
   return;
 }
 
-void AutowareScreenCapturePanel::convertPNGImagesToMP4()
-{
-  writer.release();
-  // remove temporary created folder
-  std::filesystem::remove_all(root_folder_);
-}
-
 void AutowareScreenCapturePanel::onClickVideoCapture()
 {
   const int clock = static_cast<int>(1e3 / capture_hz_->value());
@@ -130,9 +125,7 @@ void AutowareScreenCapturePanel::onClickVideoCapture()
     case State::WAITING_FOR_CAPTURE:
       // initialize setting
       {
-        counter_ = 0;
-        root_folder_ = ros_time_label_->text().toStdString();
-        std::filesystem::create_directory(root_folder_);
+        capture_file_name_ = ros_time_label_->text().toStdString();
       }
       capture_to_mp4_button_ptr_->setText("capturing rviz screen");
       capture_to_mp4_button_ptr_->setStyleSheet("background-color: #FF0000;");
@@ -146,17 +139,14 @@ void AutowareScreenCapturePanel::onClickVideoCapture()
                              .size();
         current_movie_size = cv::Size(qsize.width(), qsize.height());
         writer.open(
-          "capture/" + root_folder_ + ".mp4", fourcc, capture_hz_->value(), current_movie_size);
+          "capture/" + capture_file_name_ + ".mp4", fourcc, capture_hz_->value(),
+          current_movie_size);
       }
       capture_timer_->start(clock);
       state_ = State::CAPTURING;
       break;
-    case State::CAPTURING: {
+    case State::CAPTURING:
       capture_timer_->stop();
-    }
-      capture_to_mp4_button_ptr_->setText("writing to video");
-      capture_to_mp4_button_ptr_->setStyleSheet("background-color: #FFFF00;");
-      convertPNGImagesToMP4();
       capture_to_mp4_button_ptr_->setText("waiting for capture");
       capture_to_mp4_button_ptr_->setStyleSheet("background-color: #00FF00;");
       state_ = State::WAITING_FOR_CAPTURE;
@@ -181,7 +171,6 @@ void AutowareScreenCapturePanel::onTimer()
     cv::Mat new_image;
     cv::resize(image, new_image, current_movie_size);
     writer.write(new_image);
-
   } else {
     writer.write(image);
   }
@@ -190,17 +179,14 @@ void AutowareScreenCapturePanel::onTimer()
 
 void AutowareScreenCapturePanel::update()
 {
-  setFormatDate(ros_time_label_, raw_node_->get_clock()->now().seconds());
+  setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds());
 }
 
 void AutowareScreenCapturePanel::save(rviz_common::Config config) const { Panel::save(config); }
 
 void AutowareScreenCapturePanel::load(const rviz_common::Config & config) { Panel::load(config); }
 
-AutowareScreenCapturePanel::~AutowareScreenCapturePanel()
-{
-  std::filesystem::remove_all(root_folder_);
-}
+AutowareScreenCapturePanel::~AutowareScreenCapturePanel() = default;
 
 #include <pluginlib/class_list_macros.hpp>
 PLUGINLIB_EXPORT_CLASS(AutowareScreenCapturePanel, rviz_common::Panel)

common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp

@@ -58,7 +58,6 @@ class AutowareScreenCapturePanel : public rviz_common::Panel
   void onInitialize() override;
   void createWallTimer();
   void onTimer();
-  void convertPNGImagesToMP4();
   void save(rviz_common::Config config) const override;
   void load(const rviz_common::Config & config) override;
   void onCaptureTrigger(
@@ -67,7 +66,6 @@ class AutowareScreenCapturePanel : public rviz_common::Panel
 
 public Q_SLOTS:
   void onClickScreenCapture();
-  void onClickCaptureToVideo();
   void onClickVideoCapture();
   void onRateChanged();
 
@@ -80,8 +78,7 @@ public Q_SLOTS:
   QMainWindow * main_window_;
   enum class State { WAITING_FOR_CAPTURE, CAPTURING };
   State state_;
-  std::string root_folder_;
-  size_t counter_;
+  std::string capture_file_name_;
   bool is_capture_;
   cv::VideoWriter writer;
   cv::Size current_movie_size;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -202,7 +202,8 @@ class AvoidanceModule : public SceneModuleInterface
   void generateExtendedDrivableArea(ShiftedPath * shifted_path) const;
 
   // -- velocity planning --
-  void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path) const;
+  std::shared_ptr<double> ego_velocity_starting_avoidance_ptr_;
+  void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path);
 
   // clean up shifter
   void postProcess(PathShifter & shifter) const;

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1895,11 +1895,15 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
   }
 }
 
-void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & path) const
+void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & path)
 {
   const auto ego_idx = avoidance_data_.ego_closest_path_index;
   const auto N = path.shift_length.size();
 
+  if (!ego_velocity_starting_avoidance_ptr_) {
+    ego_velocity_starting_avoidance_ptr_ = std::make_shared<double>(getEgoSpeed());
+  }
+
   // find first shift-change point from ego
   constexpr auto SHIFT_DIFF_THR = 0.1;
   size_t target_idx = N;
@@ -1919,33 +1923,42 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted
     return;
   }
 
-  // calc time to the shift-change point
   constexpr auto NO_ACCEL_TIME_THR = 3.0;
-  const auto s = avoidance_data_.arclength_from_ego.at(target_idx) -
-                 avoidance_data_.arclength_from_ego.at(ego_idx);
-  const auto t = s / std::max(getEgoSpeed(), 1.0);
-  if (t > NO_ACCEL_TIME_THR) {
-    DEBUG_PRINT(
-      "shift point is far (s: %f, t: %f, ego_i: %lu, target_i: %lu). No velocity limit is applied.",
-      s, t, ego_idx, target_idx);
-    return;
-  }
 
-  // calc max velocity with given acceleration
-  const auto v0 = getEgoSpeed();
-  const auto vmax = std::max(
-    parameters_.min_avoidance_speed_for_acc_prevention,
-    std::sqrt(v0 * v0 + 2.0 * s * parameters_.max_avoidance_acceleration));
+  // update ego velocity if the shift point is far
+  const auto s_from_ego = avoidance_data_.arclength_from_ego.at(target_idx) -
+                          avoidance_data_.arclength_from_ego.at(ego_idx);
+  const auto t_from_ego = s_from_ego / std::max(getEgoSpeed(), 1.0);
+  if (t_from_ego > NO_ACCEL_TIME_THR) {
+    *ego_velocity_starting_avoidance_ptr_ = getEgoSpeed();
+  }
+
+  // calc index and velocity to NO_ACCEL_TIME_THR
+  const auto v0 = *ego_velocity_starting_avoidance_ptr_;
+  auto vmax = 0.0;
+  size_t insert_idx = ego_idx;
+  for (size_t i = ego_idx; i <= target_idx; ++i) {
+    const auto s =
+      avoidance_data_.arclength_from_ego.at(target_idx) - avoidance_data_.arclength_from_ego.at(i);
+    const auto t = s / std::max(v0, 1.0);
+    if (t < NO_ACCEL_TIME_THR) {
+      insert_idx = i;
+      vmax = std::max(
+        parameters_.min_avoidance_speed_for_acc_prevention,
+        std::sqrt(v0 * v0 + 2.0 * s * parameters_.max_avoidance_acceleration));
+      break;
+    }
+  }
 
   // apply velocity limit
   constexpr size_t V_LIM_APPLY_IDX_MARGIN = 0;
-  for (size_t i = ego_idx + V_LIM_APPLY_IDX_MARGIN; i < N; ++i) {
+  for (size_t i = insert_idx + V_LIM_APPLY_IDX_MARGIN; i < N; ++i) {
     path.path.points.at(i).point.longitudinal_velocity_mps =
       std::min(path.path.points.at(i).point.longitudinal_velocity_mps, static_cast<float>(vmax));
   }
 
   DEBUG_PRINT(
-    "s: %f, t: %f, v0: %f, a: %f, vmax: %f, ego_i: %lu, target_i: %lu", s, t, v0,
+    "s: %f, t: %f, v0: %f, a: %f, vmax: %f, ego_i: %lu, target_i: %lu", s_from_ego, t_from_ego, v0,
     parameters_.max_avoidance_acceleration, vmax, ego_idx, target_idx);
 }
 

planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp

@@ -113,7 +113,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
 
   // function
   geometry_msgs::msg::PoseStamped getCurrentPose();
-  bool isDataReady(const PlannerData planner_data) const;
+  bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const;
   autoware_auto_planning_msgs::msg::Path generatePath(
     const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg,
     const PlannerData & planner_data);

planning/behavior_velocity_planner/src/node.cpp

@@ -203,35 +203,46 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
 }
 
 // NOTE: argument planner_data must not be referenced for multithreading
-bool BehaviorVelocityPlannerNode::isDataReady(const PlannerData planner_data) const
+bool BehaviorVelocityPlannerNode::isDataReady(
+  const PlannerData planner_data, rclcpp::Clock clock) const
 {
   const auto & d = planner_data;
 
   // from tf
   if (d.current_pose.header.frame_id == "") {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Frame id of current pose is missing");
     return false;
   }
 
   // from callbacks
   if (!d.current_velocity) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity");
     return false;
   }
   if (!d.current_accel) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration");
     return false;
   }
   if (!d.predicted_objects) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for predicted_objects");
     return false;
   }
   if (!d.no_ground_pointcloud) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud");
     return false;
   }
   if (!d.route_handler_) {
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), clock, 3000, "Waiting for the initialization of route_handler");
     return false;
   }
   if (!d.route_handler_->isMapMsgReady()) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map");
     return false;
   }
   if (!d.velocity_smoother_) {
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), clock, 3000, "Waiting for the initialization of velocity smoother");
     return false;
   }
   return true;
@@ -381,6 +392,7 @@ void BehaviorVelocityPlannerNode::onTrigger(
   const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg)
 {
   mutex_.lock();  // for planner_data_
+
   // Check ready
   try {
     planner_data_.current_pose =
@@ -391,7 +403,7 @@ void BehaviorVelocityPlannerNode::onTrigger(
     return;
   }
 
-  if (!isDataReady(planner_data_)) {
+  if (!isDataReady(planner_data_, *get_clock())) {
     mutex_.unlock();
     return;
   }

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

@@ -12,7 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <interpolation/linear_interpolation.hpp>
 #include <interpolation/spline_interpolation.hpp>
+#include <interpolation/zero_order_hold.hpp>
+#include <motion_utils/resample/resample.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <utilization/path_utilization.hpp>
 
@@ -84,8 +87,9 @@ bool splineInterpolate(
   // do spline for xy
   const std::vector<double> resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s);
   const std::vector<double> resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s);
-  const std::vector<double> resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s);
-  const std::vector<double> resampled_v = ::interpolation::slerp(base_s, base_v, resampled_s);
+  const std::vector<double> resampled_z = ::interpolation::lerp(base_s, base_z, resampled_s);
+  const std::vector<double> resampled_v =
+    ::interpolation::zero_order_hold(base_s, base_v, resampled_s);
 
   // set xy
   output->points.clear();
@@ -133,7 +137,6 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
   const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval)
 {
   const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")};
-  autoware_auto_planning_msgs::msg::Path interpolated_path;
 
   const double epsilon = 0.01;
   std::vector<double> x;
@@ -151,7 +154,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
     return path;
   }
 
-  double path_len = length;
+  double path_len = std::min(length, motion_utils::calcArcLength(path.points));
   {
     double s = 0.0;
     for (size_t idx = 0; idx < path.points.size(); ++idx) {
@@ -162,7 +165,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
       v.push_back(path_point.longitudinal_velocity_mps);
       if (idx != 0) {
         const auto path_point_prev = path.points.at(idx - 1);
-        s += tier4_autoware_utils::calcDistance3d(path_point_prev.pose, path_point.pose);
+        s += tier4_autoware_utils::calcDistance2d(path_point_prev.pose, path_point.pose);
       }
       if (s > path_len) {
         break;
@@ -205,44 +208,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
     return path;
   }
 
-  // Interpolate
-  const auto x_interp = interpolation::slerp(s_in, x, s_out);
-  const auto y_interp = interpolation::slerp(s_in, y, s_out);
-  const auto z_interp = interpolation::slerp(s_in, z, s_out);
-
-  // Find a nearest segment for each point in s_out and use the velocity of the segment's beginning
-  // point. Note that if s_out is almost the same value as s_in within DOUBLE_EPSILON range, the
-  // velocity of s_out should be same as the one of s_in.
-  std::vector<double> v_interp;
-  size_t closest_segment_idx = 0;
-  for (size_t i = 0; i < s_out.size() - 1; ++i) {
-    for (size_t j = closest_segment_idx; j < s_in.size() - 1; ++j) {
-      if (s_in.at(j) - DOUBLE_EPSILON < s_out.at(i) && s_out.at(i) < s_in.at(j + 1)) {
-        // find closest segment in s_in
-        closest_segment_idx = j;
-      }
-    }
-    v_interp.push_back(v.at(closest_segment_idx));
-  }
-  v_interp.push_back(v.back());
-
-  // Insert path point to interpolated_path
-  for (size_t idx = 0; idx < v_interp.size() - 1; ++idx) {
-    autoware_auto_planning_msgs::msg::PathPoint path_point;
-    path_point.pose.position.x = x_interp.at(idx);
-    path_point.pose.position.y = y_interp.at(idx);
-    path_point.pose.position.z = z_interp.at(idx);
-    path_point.longitudinal_velocity_mps = v_interp.at(idx);
-    const double yaw =
-      std::atan2(y_interp.at(idx + 1) - y_interp.at(idx), x_interp.at(idx + 1) - x_interp.at(idx));
-    tf2::Quaternion quat;
-    quat.setRPY(0, 0, yaw);
-    path_point.pose.orientation = tf2::toMsg(quat);
-    interpolated_path.points.push_back(path_point);
-  }
-  interpolated_path.points.push_back(path.points.back());
-
-  return interpolated_path;
+  return motion_utils::resamplePath(path, s_out);
 }
 
 autoware_auto_planning_msgs::msg::Path filterLitterPathPoint(