From d5cc4fd21b7a8c83f161c279b6df1a9af5788810 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 18:18:04 +0900 Subject: [PATCH 1/5] fix pitch calculation in simple planning simulator Signed-off-by: Mamoru Sobue --- simulator/simple_planning_simulator/README.md | 6 + .../media/pitch-calculation.drawio.svg | 329 ++++++++++++++++++ .../simple_planning_simulator_core.cpp | 3 +- 3 files changed, 336 insertions(+), 2 deletions(-) create mode 100644 simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d70aca794dc07..06967fe6f6c14 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 calcluation](./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..cdc5551656fd4 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,8 +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); - return ego_pitch_angle; + reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy); } void SimplePlanningSimulator::on_timer() From a30bc1095626a1a5bceaf1a7985d020735a10563 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 25 Sep 2023 23:52:33 +0900 Subject: [PATCH 2/5] add slope definition in readme Signed-off-by: Takamasa Horibe --- control/pid_longitudinal_controller/README.md | 4 + .../media/slope_definition.drawio.svg | 165 ++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 control/pid_longitudinal_controller/media/slope_definition.drawio.svg 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..b5c2032208041 --- /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 +
+
+
+ + y + + + + + + +
+
+
+ z +
+
+
+ + z + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + From 2a784ae0212e6b13fc7c586abef91dcd2569c65e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 18:34:44 +0900 Subject: [PATCH 3/5] fix Signed-off-by: Mamoru Sobue --- .../src/pid_longitudinal_controller.cpp | 4 +++- .../simple_planning_simulator_core.cpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) 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/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index cdc5551656fd4..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 @@ -314,6 +314,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const 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); + return ego_pitch_angle; } void SimplePlanningSimulator::on_timer() From cfe2cdc4ee3b42866c97a8c39220d23bef1f5b4c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 18:52:24 +0900 Subject: [PATCH 4/5] fix spell Signed-off-by: Mamoru Sobue --- simulator/simple_planning_simulator/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 06967fe6f6c14..ceecacfe8cd8c 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -93,7 +93,7 @@ In the simple_planning_simulator.launch.py, the node that outputs the `map`->`od Ego vehicle pitch angle is calculated in the following manner. -![pitch calcluation](./media/pitch-calculation.drawio.svg) +![pitch calculation](./media/pitch-calculation.drawio.svg) ## Error detection and handling From 14db08da2b22b4d6c1c96470a036a4986293f46b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 3 Oct 2023 10:25:18 +0900 Subject: [PATCH 5/5] fix figure Signed-off-by: Mamoru Sobue --- .../media/slope_definition.drawio.svg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg index b5c2032208041..7f0aa677629a3 100644 --- a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -101,7 +101,7 @@ - y + z @@ -116,7 +116,7 @@ - z + y