Skip to content

Commit

Permalink
[JTC] Fix dynamic reconfigure of tolerances (backport #849)
Browse files Browse the repository at this point in the history
* Fix dynamic reconfigure of tolerances

* Fix static cast syntax
  • Loading branch information
christophfroehlich committed Nov 27, 2023
1 parent 31628e0 commit 17e44c6
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -131,11 +131,12 @@ controller_interface::return_type JointTrajectoryController::update(
if (param_listener_->is_old(params_))
{
params_ = param_listener_->get_params();
// use_closed_loop_pid_adapter_ is updated in on_configure only
default_tolerances_ = get_segment_tolerances(params_);
// update the PID gains
// variable use_closed_loop_pid_adapter_ is updated in on_configure only
if (use_closed_loop_pid_adapter_)
{
update_pids();
default_tolerances_ = get_segment_tolerances(params_);
}
}

Expand Down
53 changes: 53 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -501,6 +501,59 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
executor.cancel();
}

/**
* @brief check if dynamic tolerances are updated
*/
TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances)
{
rclcpp::executors::MultiThreadedExecutor executor;

SetUpAndActivateTrajectoryController(executor);

updateController();

// test default parameters
{
auto tols = traj_controller_->get_tolerances();
EXPECT_EQ(tols.goal_time_tolerance, 0.0);
for (size_t i = 0; i < joint_names_.size(); ++i)
{
EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0);
EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0);
EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01);
}
}

// change parameters, update and see what happens
std::vector<rclcpp::Parameter> new_tolerances{
rclcpp::Parameter("constraints.goal_time", 1.0),
rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02),
rclcpp::Parameter("constraints.joint1.trajectory", 1.0),
rclcpp::Parameter("constraints.joint2.trajectory", 2.0),
rclcpp::Parameter("constraints.joint3.trajectory", 3.0),
rclcpp::Parameter("constraints.joint1.goal", 10.0),
rclcpp::Parameter("constraints.joint2.goal", 20.0),
rclcpp::Parameter("constraints.joint3.goal", 30.0)};
for (const auto & param : new_tolerances)
{
traj_controller_->get_node()->set_parameter(param);
}
updateController();

{
auto tols = traj_controller_->get_tolerances();
EXPECT_EQ(tols.goal_time_tolerance, 1.0);
for (size_t i = 0; i < joint_names_.size(); ++i)
{
EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast<double>(i) + 1.0);
EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast<double>(i) + 1.0));
EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02);
}
}

executor.cancel();
}

/**
* @brief check if hold on startup is deactivated
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,11 @@ class TestableJointTrajectoryController

std::vector<PidPtr> get_pids() const { return pids_; }

joint_trajectory_controller::SegmentTolerances get_tolerances() const
{
return default_tolerances_;
}

bool has_active_traj() const { return has_active_trajectory(); }

bool has_trivial_traj() const
Expand Down

0 comments on commit 17e44c6

Please sign in to comment.