diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..9d25322c793e0 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -75,6 +75,10 @@ For instance, if the vehicle is attempting to start with an acceleration of `1.0 A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. +Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. + +![slope_definition](./media/slope_definition.drawio.svg) + #### PID control For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg new file mode 100644 index 0000000000000..7f0aa677629a3 --- /dev/null +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Slope is defined as positive for an uphill +
+
+
+ + Slope is defined as positive for an uphi... + + + + + + + + + + + + + + + +
+
+
+ (Pitch of ego is defined as negative when facing upward) +
+
+
+ + (Pitch of ego is defined as negative when facing upwar... + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + z + + + + + + +
+
+
+ z +
+
+
+ + y + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 644effce50824..b80874a8a2e57 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -457,7 +457,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); + // NOTE: getPitchByTraj() calculates the pitch angle as defined in + // ../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); diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d70aca794dc07..ceecacfe8cd8c 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -89,6 +89,12 @@ _Note_: The steering/velocity/acceleration dynamics is modeled by a first order Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. +### (Caveat) Pitch calculation + +Ego vehicle pitch angle is calculated in the following manner. + +![pitch calculation](./media/pitch-calculation.drawio.svg) + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. diff --git a/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg new file mode 100644 index 0000000000000..d022b29d2a730 --- /dev/null +++ b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg @@ -0,0 +1,329 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + forward
$diff_xy > 0
 + +
+
+
+ + forward... + + + + + +
+
+
+ + + ego +
+ direction +
+ + +
+
+
+ + ego... + + + + + +
+
+
+ + reverse
$diff_xy < 0
 + +
+
+
+ + reverse... + + + + + +
+
+
+ + + lane +
+ direction +
+ + +
+
+
+ + lane... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $diff_z > 0
 + +
+
+
+ + $diff_z > 0 + + + + + +
+
+
+ + $diff_z < 0
 + +
+
+
+ + $diff_z < 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + + + + Text is not SVG - cannot display + + + diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 8c2688928b4c9..208fd80d57a08 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -313,7 +313,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const std::cos(ego_yaw_against_lanelet); const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0; const double ego_pitch_angle = - reverse_sign ? std::atan2(-diff_z, -diff_xy) : std::atan2(diff_z, diff_xy); + reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy); return ego_pitch_angle; }