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

Fix backward motion for graceful controller #4527

Merged
merged 2 commits into from
Jul 23, 2024
Merged

Fix backward motion for graceful controller #4527

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7362da7
Fix backward motion for graceful controller
ajtudela Jul 8, 2024
2e92612
Update smooth_control_law.cpp
ajtudela Jul 22, 2024
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
16 changes: 0 additions & 16 deletions nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@ namespace nav2_graceful_controller
struct EgocentricPolarCoordinates
{
float r; // Radial distance between the robot pose and the target pose.
// Negative value if the robot is moving backwards.
float phi; // Orientation of target with respect to the line of sight
// from the robot to the target.
float delta; // Steering angle of the robot with respect to the line of sight.
Expand Down Expand Up @@ -68,21 +67,6 @@ struct EgocentricPolarCoordinates
r = sqrt(dX * dX + dY * dY);
phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight);
delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight);
// If the robot is moving backwards, flip the sign of the radial distance
r *= backward ? -1.0 : 1.0;
}

/**
* @brief Construct a new egocentric polar coordinates for the target pose.
*
* @param target Target pose.
* @param backward If true, the robot is moving backwards. Defaults to false.
*/
explicit EgocentricPolarCoordinates(
const geometry_msgs::msg::Pose & target,
bool backward = false)
{
EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward);
}
};

Expand Down
6 changes: 2 additions & 4 deletions nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,7 @@ class SmoothControlLaw
* @param v_angular_max The maximum absolute velocity in the angular direction.
*/
void setSpeedLimit(
const double v_linear_min, const double v_linear_max,
const double v_angular_max);
const double v_linear_min, const double v_linear_max, const double v_angular_max);

/**
* @brief Compute linear and angular velocities command using the curvature.
Expand All @@ -103,8 +102,7 @@ class SmoothControlLaw
* @return Velocity command.
*/
geometry_msgs::msg::Twist calculateRegularVelocity(
const geometry_msgs::msg::Pose & target,
const bool & backward = false);
const geometry_msgs::msg::Pose & target, const bool & backward = false);

/**
* @brief Calculate the next pose of the robot by generating a velocity command using the
Expand Down
5 changes: 3 additions & 2 deletions nav2_graceful_controller/src/smooth_control_law.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,7 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius)
}

void SmoothControlLaw::setSpeedLimit(
const double v_linear_min, const double v_linear_max,
const double v_angular_max)
const double v_linear_min, const double v_linear_max, const double v_angular_max)
{
v_linear_min_ = v_linear_min;
v_linear_max_ = v_linear_max;
Expand All @@ -59,6 +58,8 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity(
auto ego_coords = EgocentricPolarCoordinates(target, current, backward);
// Calculate the curvature
double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta);
// Invert the curvature if the robot is moving backwards
curvature = backward ? -curvature : curvature;

// Adjust the linear velocity as a function of the path curvature to
// slowdown the controller as it approaches its target
Expand Down
18 changes: 15 additions & 3 deletions nav2_graceful_controller/test/test_egopolar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorWithValues) {
float phi_value = 1.2;
float delta_value = -0.5;

nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value,
delta_value);
nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value, delta_value);

EXPECT_FLOAT_EQ(r_value, coords.r);
EXPECT_FLOAT_EQ(phi_value, coords.phi);
Expand All @@ -57,6 +56,19 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) {
EXPECT_FLOAT_EQ(-1.1827937483787536, coords.delta);
}

TEST(EgocentricPolarCoordinatesTest, constructorFromPose) {
geometry_msgs::msg::Pose target;
target.position.x = 3.0;
target.position.y = 3.0;
target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI));

nav2_graceful_controller::EgocentricPolarCoordinates coords(target);

EXPECT_FLOAT_EQ(4.2426405, coords.r);
EXPECT_FLOAT_EQ(2.3561945, coords.phi);
EXPECT_FLOAT_EQ(-M_PI / 4, coords.delta);
}

TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) {
geometry_msgs::msg::Pose target;
target.position.x = -3.0;
Expand All @@ -70,7 +82,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) {

nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current, true);

EXPECT_FLOAT_EQ(-6.4031243, coords.r);
EXPECT_FLOAT_EQ(6.4031243, coords.r);
EXPECT_FLOAT_EQ(-0.096055523, coords.phi);
EXPECT_FLOAT_EQ(-1.0960555, coords.delta);
}
Expand Down
Loading