update pitch option

Signed-off-by: Berkay Karaman <[email protected]>
brkay54 · Nov 10, 2023 · dcf159d · dcf159d
1 parent 1e03581
commit dcf159d
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 24 deletions.
diff --git a/...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -191,6 +191,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   double m_min_jerk;
 
   // slope compensation
+  bool m_enable_adaptive_trajectory;
+  double m_adaptive_trajectory_velocity_th;
   std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
   double m_max_pitch_rad;
   double m_min_pitch_rad;

diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
@@ -183,19 +183,13 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
         return trajectory.points.at(i).pose;
       }
       double ratio = remain_dist / dist;
+      const auto p0 = trajectory.points.at(i).pose;
+      const auto p1 = trajectory.points.at(i + 1).pose;
       p = trajectory.points.at(i).pose;
-      p.position.x = interpolation::lerp(
-        trajectory.points.at(i).pose.position.x, trajectory.points.at(i + 1).pose.position.x,
-        ratio);
-      p.position.y = interpolation::lerp(
-        trajectory.points.at(i).pose.position.y, trajectory.points.at(i + 1).pose.position.y,
-        ratio);
-      p.position.z = interpolation::lerp(
-        trajectory.points.at(i).pose.position.z, trajectory.points.at(i + 1).pose.position.z,
-        ratio);
-      p.orientation = lerpOrientation(
-        trajectory.points.at(i).pose.orientation, trajectory.points.at(i + 1).pose.orientation,
-        ratio);
+      p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio);
+      p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio);
+      p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio);
+      p.orientation = lerpOrientation(p0.orientation, p1.orientation, ratio);
       break;
     }
     remain_dist -= dist;

diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -163,6 +163,9 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   m_min_jerk = node.declare_parameter<double>("min_jerk");  // [m/s^3]
 
   // parameters for slope compensation
+  m_enable_adaptive_trajectory = node.declare_parameter<bool>("enable_adaptive_trajectory");
+  m_adaptive_trajectory_velocity_th =
+    node.declare_parameter<double>("adaptive_trajectory_velocity_th");  // [m/s^2]
   const double lpf_pitch_gain{node.declare_parameter<double>("lpf_pitch_gain")};
   m_lpf_pitch = std::make_shared<LowpassFilter1d>(0.0, lpf_pitch_gain);
   m_max_pitch_rad = node.declare_parameter<double>("max_pitch_rad");  // [rad]
@@ -497,20 +500,20 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed
   const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation);
   const double traj_pitch =
-    longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base);
-  control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch);
-  const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation);
-  const double traj_pitch = longitudinal_utils::getPitchByTraj(
-    control_data.interpolated_traj, control_data.target_idx, m_wheel_base);
+    longitudinal_utils::getPitchByTraj(m_trajectory, control_data.target_idx, m_wheel_base);
 
-  // if vehicle is not in driving state, send the raw slope angle
-  if (m_control_state == ControlState::DRIVE) {
-    control_data.slope_angle = traj_pitch;
-    m_lpf_pitch->filter(control_data.slope_angle);
+  if (m_enable_adaptive_trajectory) {
+    // if velocity is high, use target idx for slope, otherwise, use raw_pitch
+    if (control_data.current_motion.vel > m_adaptive_trajectory_velocity_th) {
+      control_data.slope_angle = traj_pitch;
+      m_lpf_pitch->filter(control_data.slope_angle);
+    } else {
+      control_data.slope_angle = raw_pitch;
+      // send filtered slope angle to smooth transition between raw and traj pitch
+      control_data.slope_angle = m_lpf_pitch->filter(control_data.slope_angle);
+    }
   } else {
-    control_data.slope_angle = raw_pitch;
-    // send filtered slope angle to smooth transition between raw and traj pitch
-    control_data.slope_angle = m_lpf_pitch->filter(control_data.slope_angle);
+    control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
   }
 
   updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch);