Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(pid_longitudinal_controller): take into account the current acceleration to calculate delayed pose #4616

Merged
merged 4 commits into from
Aug 17, 2023
Merged

feat(pid_longitudinal_controller): take into account the current acceleration to calculate delayed pose #4616

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
548912b
feat(pid_longitudinal_controller): take into account the current acce…
brkay54 Aug 13, 2023
7999d9e
Update for reverse driving
brkay54 Aug 15, 2023
1a44f70
update for wrong calculation
brkay54 Aug 16, 2023
9805531
update
brkay54 Aug 17, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions ...itudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,11 +72,12 @@ double getPitchByTraj(
double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);

/**
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for
* delayed time
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
* acceleration for delayed time
*/
Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const double delay_time, const double current_vel);
const Pose & current_pose, const double delay_time, const double current_vel,
const double current_acc);

/**
* @brief apply linear interpolation to orientation
Expand Down
15 changes: 13 additions & 2 deletions control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2018-2021 Tier IV, Inc. All rights reserved.

Check notice on line 1 in control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession 

The ratio of primitive types in function arguments increases from 62.07% to 63.33%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -129,11 +129,22 @@
}

Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const double delay_time, const double current_vel)
const Pose & current_pose, const double delay_time, const double current_vel,
const double current_acc)
{
if (delay_time <= 0.0) {
return current_pose;
}

// check time to stop
const double time_to_stop = -current_vel / current_acc;

const double delay_time_calculation =
time_to_stop > 0.0 && time_to_stop < delay_time ? time_to_stop : delay_time;
// simple linear prediction
const double yaw = tf2::getYaw(current_pose.orientation);
const double running_distance = delay_time * current_vel;
const double running_distance =
delay_time_calculation * current_vel + 0.5 * current_acc * delay_time * delay_time_calculation;
brkay54 marked this conversation as resolved.
Show resolved Hide resolved
const double dx = running_distance * std::cos(yaw);
const double dy = running_distance * std::sin(yaw);

Expand Down
2 changes: 1 addition & 1 deletion control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -626,7 +626,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
Motion target_motion{};
if (m_control_state == ControlState::DRIVE) {
const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay(
current_pose, m_delay_compensation_time, current_vel);
current_pose, m_delay_compensation_time, current_vel, current_acc);
const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose);
target_motion = Motion{
target_interpolated_point.longitudinal_velocity_mps,
Expand Down
78 changes: 63 additions & 15 deletions control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -197,72 +197,120 @@
quaternion_tf.setRPY(0.0, 0.0, 0.0);
current_pose.orientation = tf2::toMsg(quaternion_tf);

// With a delay and/or a velocity of 0.0 there is no change of position
// With a delay acceleration and/or a velocity of 0.0 there is no change of position
double delay_time = 0.0;
double current_vel = 0.0;
double current_acc = 0.0;
Pose delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

delay_time = 1.0;
current_vel = 0.0;
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
current_acc = 0.0;
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

delay_time = 0.0;
current_vel = 1.0;
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
current_acc = 0.0;
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

// With both delay and velocity: change of position
delay_time = 1.0;
current_vel = 1.0;
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
current_acc = 0.0;

delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

// With all, acceleration, delay and velocity: change of position
delay_time = 1.0;
current_vel = 1.0;
current_acc = 1.0;

delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(
delayed_pose.position.x,
current_pose.position.x + current_vel * delay_time +
0.5 * current_acc * delay_time * delay_time,
abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

// Vary the yaw
quaternion_tf.setRPY(0.0, 0.0, M_PI);
current_pose.orientation = tf2::toMsg(quaternion_tf);
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x - current_vel * delay_time, abs_err);
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(
delayed_pose.position.x,
current_pose.position.x - current_vel * delay_time -
0.5 * current_acc * delay_time * delay_time,
abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

quaternion_tf.setRPY(0.0, 0.0, M_PI_2);
current_pose.orientation = tf2::toMsg(quaternion_tf);
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y + current_vel * delay_time, abs_err);
EXPECT_NEAR(
delayed_pose.position.y,
current_pose.position.y + current_vel * delay_time +
0.5 * current_acc * delay_time * delay_time,
abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

quaternion_tf.setRPY(0.0, 0.0, -M_PI_2);
current_pose.orientation = tf2::toMsg(quaternion_tf);
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y - current_vel * delay_time, abs_err);
EXPECT_NEAR(
delayed_pose.position.y,
current_pose.position.y - current_vel * delay_time -
0.5 * current_acc * delay_time * delay_time,
abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

// Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI
quaternion_tf.setRPY(0.0, M_PI_4, 0.0);
current_pose.orientation = tf2::toMsg(quaternion_tf);
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err);
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(
delayed_pose.position.x,
current_pose.position.x + current_vel * delay_time +
0.5 * current_acc * delay_time * delay_time,
abs_err);
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);

// Vary the roll : no effect
quaternion_tf.setRPY(M_PI_2, 0.0, 0.0);
current_pose.orientation = tf2::toMsg(quaternion_tf);
delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err);
delayed_pose =
longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
EXPECT_NEAR(
delayed_pose.position.x,
current_pose.position.x + current_vel * delay_time +
0.5 * current_acc * delay_time * delay_time,
abs_err);

Check warning on line 313 in control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method 

TEST:TestLongitudinalControllerUtils:calcPoseAfterTimeDelay has 111 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
}
Expand Down
Loading