kalan

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -161,7 +161,8 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
   const size_t src_idx, const double distance,
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
 
+TrajectoryPoint getExtendTrajectoryPoint(
+  const double extend_distance, const TrajectoryPoint & goal_point);
 }  // namespace longitudinal_utils
 }  // namespace autoware::motion::control::pid_longitudinal_controller
-
 #endif  // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -58,11 +58,23 @@ bool isValidTrajectory(const Trajectory & traj)
 double calcStopDistance(
   const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw)
 {
-  const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points);
-
-  const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
   const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     traj.points, current_pose, max_dist, max_yaw);
+  const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points);
+  const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
+  //  if (end_idx == traj.points.size() - 1 && 1 >= int(end_idx - seg_idx)) {
+  //    // Check the ego in front of the last point of trajectory or not
+  //    const auto yaw = tier4_autoware_utils::getRPY(current_pose).z;
+  //    const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw));
+  //    const Eigen::Vector2d vehicle_to_end_vec(
+  //      traj.points.back().pose.position.x - current_pose.position.x,
+  //      traj.points.back().pose.position.y - current_pose.position.y);
+  //    if (base_pose_vec.dot(vehicle_to_end_vec) < 0.0) {
+  //      return -1.0 *
+  //             tier4_autoware_utils::calcDistance3d(current_pose, traj.points.at(end_idx).pose);
+  //    }
+  //    return tier4_autoware_utils::calcDistance3d(current_pose, traj.points.at(end_idx).pose);
+  //  }
   const double signed_length_on_traj = motion_utils::calcSignedArcLength(
     traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position,
     std::min(end_idx, traj.points.size() - 2));
@@ -96,29 +108,7 @@ double getPitchByTraj(
   TrajectoryPoint point_after_distance = trajectory.points.back();
   point_after_distance.pose = pose_after_distance;
 
-  for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
-    const double dist = tier4_autoware_utils::calcDistance2d(
-      trajectory.points.at(nearest_idx), trajectory.points.at(i));
-    if (dist > wheel_base) {
-      // calculate pitch from trajectory between rear wheel (nearest) and front center (i)
-      return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i));
-    }
-  }
-
-  // close to goal
-  for (size_t i = trajectory.points.size() - 1; i > 0; --i) {
-    const double dist =
-      tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i));
-
-    if (dist > wheel_base) {
-      // calculate pitch from trajectory
-      // between wheelbase behind the end of trajectory (i) and the end of trajectory (back)
-      return calcElevationAngle(trajectory.points.at(i), trajectory.points.back());
-    }
-  }
-
-  // calculate pitch from trajectory between the beginning and end of trajectory
-  return calcElevationAngle(trajectory.points.at(0), trajectory.points.back());
+  return calcElevationAngle(trajectory.points.at(nearest_idx), point_after_distance);
 }
 
 double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to)
@@ -216,5 +206,27 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
   }
   return p;
 }
+
+TrajectoryPoint getExtendTrajectoryPoint(
+  const double extend_distance, const TrajectoryPoint & goal_point)
+{
+  tf2::Transform map2goal;
+  tf2::fromMsg(goal_point.pose, map2goal);
+  tf2::Transform local_extend_point;
+  local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0));
+  tf2::Quaternion q;
+  q.setRPY(0, 0, 0);
+  local_extend_point.setRotation(q);
+  const auto map2extend_point = map2goal * local_extend_point;
+  geometry_msgs::msg::Pose extend_pose;
+  tf2::toMsg(map2extend_point, extend_pose);
+  TrajectoryPoint extend_trajectory_point;
+  extend_trajectory_point.pose = extend_pose;
+  extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps;
+  extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps;
+  extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2;
+  return extend_trajectory_point;
+}
+
 }  // namespace longitudinal_utils
 }  // namespace autoware::motion::control::pid_longitudinal_controller

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -221,8 +221,12 @@ void PidLongitudinalController::setTrajectory(
     RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored.");
     return;
   }
-
+  // If the vehicle pass the last point of trajectory, it causes errors on control_data calculation.
+  // To handle this, we add a virtual point after the last point.
+  constexpr double virtual_point_distance = 5.0;
   m_trajectory = msg;
+  m_trajectory.points.push_back(longitudinal_utils::getExtendTrajectoryPoint(
+    virtual_point_distance, m_trajectory.points.back()));
 }
 
 rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(
@@ -870,6 +874,14 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop
     return output_motion;
   }
 
+  if (control_data.stop_dist < 0.0) {
+    const auto & p = m_stopped_state_params;
+    output_motion.vel = p.vel;
+    output_motion.acc = longitudinal_utils::applyDiffLimitFilter(
+      p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk);
+    return output_motion;
+  }
+
   double min_acc_before_stop = std::numeric_limits<double>::max();
   size_t min_acc_idx = std::numeric_limits<size_t>::max();
   for (int i = static_cast<int>(*stop_idx); i >= 0; --i) {
@@ -980,10 +992,29 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
   // deviation will be bigger.
   constexpr double ff_scale_max = 2.0;  // for safety
   constexpr double ff_scale_min = 0.5;  // for safety
-  const double ff_scale =
-    std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max);
-  const double ff_acc = target_motion.acc * ff_scale;
-
+  const double ff_scale = std::clamp(
+    1.0 + (abs(current_target_vel_abs - abs(control_data.current_motion.vel)) /
+           std::max(current_target_vel_abs, 0.1)),
+    ff_scale_min, ff_scale_max);
+
+  // Feedforward acceleration:
+  const double tmp_ff_acc =
+    control_data.target_idx == control_data.interpolated_traj.points.size() - 1 ||
+        control_data.current_motion.vel < 0.01
+      ? control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2
+      : control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 *
+          (std::max(
+             tier4_autoware_utils::calcDistance2d(
+               control_data.interpolated_traj.points.at(control_data.target_idx + 1).pose,
+               control_data.interpolated_traj.points.at(control_data.target_idx).pose),
+             std::numeric_limits<double>::min()) /
+           std::max(
+             tier4_autoware_utils::calcDistance3d(
+               control_data.interpolated_traj.points.at(control_data.target_idx + 1).pose,
+               control_data.interpolated_traj.points.at(control_data.target_idx).pose),
+             std::numeric_limits<double>::min()));
+
+  const double ff_acc = tmp_ff_acc * ff_scale;
   const double feedback_acc = ff_acc + pid_acc;
 
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc);