Skip to content

adding checks on config and dynamic parameters for proper velocity and acceleration limits #3731

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

Merged
merged 1 commit into from
Aug 2, 2023
Merged

adding checks on config and dynamic parameters for proper velocity and acceleration limits #3731

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
31 changes: 27 additions & 4 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,11 +74,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
node->get_parameter("max_accel", max_accels_);
node->get_parameter("max_decel", max_decels_);

for (unsigned int i = 0; i != max_decels_.size(); i++) {
for (unsigned int i = 0; i != 3; i++) {
if (max_decels_[i] > 0.0) {
RCLCPP_WARN(
get_logger(),
"Positive values set of deceleration! These should be negative to slow down!");
throw std::runtime_error(
"Positive values set of deceleration! These should be negative to slow down!");
}
if (max_accels_[i] < 0.0) {
throw std::runtime_error(
"Negative values set of acceleration! These should be positive to speed up!");
}
if (min_velocities_[i] > max_velocities_[i]) {
throw std::runtime_error(
"Min velocities are higher than max velocities!");
}
}

Expand Down Expand Up @@ -358,8 +365,24 @@ VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
} else if (name == "min_velocity") {
min_velocities_ = parameter.as_double_array();
} else if (name == "max_accel") {
for (unsigned int i = 0; i != 3; i++) {
if (parameter.as_double_array()[i] < 0.0) {
RCLCPP_WARN(
get_logger(),
"Negative values set of acceleration! These should be positive to speed up!");
result.successful = false;
}
}
max_accels_ = parameter.as_double_array();
} else if (name == "max_decel") {
for (unsigned int i = 0; i != 3; i++) {
if (parameter.as_double_array()[i] > 0.0) {
RCLCPP_WARN(
get_logger(),
"Positive values set of deceleration! These should be negative to slow down!");
result.successful = false;
}
}
max_decels_ = parameter.as_double_array();
} else if (name == "deadband_velocity") {
deadband_velocities_ = parameter.as_double_array();
Expand Down
34 changes: 34 additions & 0 deletions nav2_velocity_smoother/test/test_velocity_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -593,6 +593,28 @@ TEST(VelocitySmootherTest, testInvalidParams)
EXPECT_THROW(smoother->configure(state), std::runtime_error);
}

TEST(VelocitySmootherTest, testInvalidParamsAccelDecel)
{
auto smoother =
std::make_shared<VelSmootherShim>();

std::vector<double> bad_test_accel{-10.0, -10.0, -10.0};
std::vector<double> bad_test_decel{10.0, 10.0, 10.0};
std::vector<double> bad_test_min_vel{10.0, 10.0, 10.0};
std::vector<double> bad_test_max_vel{-10.0, -10.0, -10.0};

smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel));
smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel));
rclcpp_lifecycle::State state;
EXPECT_THROW(smoother->configure(state), std::runtime_error);

smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel)));
EXPECT_THROW(smoother->configure(state), std::runtime_error);

smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel)));
EXPECT_THROW(smoother->configure(state), std::runtime_error);
}

TEST(VelocitySmootherTest, testDynamicParameter)
{
auto smoother =
Expand All @@ -613,6 +635,8 @@ TEST(VelocitySmootherTest, testDynamicParameter)
std::vector<double> min_accel{0.0, 0.0, 0.0};
std::vector<double> deadband{0.0, 0.0, 0.0};
std::vector<double> bad_test{0.0, 0.0};
std::vector<double> bad_test_accel{-10.0, -10.0, -10.0};
std::vector<double> bad_test_decel{10.0, 10.0, 10.0};

auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter("smoothing_frequency", 100.0),
Expand Down Expand Up @@ -662,6 +686,16 @@ TEST(VelocitySmootherTest, testDynamicParameter)
rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
EXPECT_FALSE(results.get().successful);

// Test invalid accel / decel
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter("max_accel", bad_test_accel)});
rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
EXPECT_FALSE(results.get().successful);
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter("max_decel", bad_test_decel)});
rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
EXPECT_FALSE(results.get().successful);

// test full state after major changes
smoother->deactivate(state);
smoother->cleanup(state);
Expand Down