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

[JTC] Sample at t + dT instead of the beginning of the control cycle #1306

Merged
merged 17 commits into from
Nov 22, 2024
+147 −73
Merged

[JTC] Sample at t + dT instead of the beginning of the control cycle #1306

Show file tree
Hide file tree
Changes from 11 commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
0dba992
Set update rate in testing controller
fmauch Aug 8, 2024
a96dd54
Tests: Call controller.configure() instead of controller.get_node().c…
fmauch Aug 28, 2024
d55fea7
Sample trajectory at point t+dT
fmauch Sep 12, 2024
9400838
Update trajectory_controller_tests
fmauch Oct 7, 2024
777e5a9
Merge branch 'master' into jtc_tdt
fmauch Oct 8, 2024
b49a0e5
Apply suggestions from code review
fmauch Oct 16, 2024
bc83608
set update_period as part of on_configure instead of on_activate
fmauch Oct 16, 2024
8c99198
Use period instead of update_period_ for the PID feedback
fmauch Oct 16, 2024
f5f33a7
Remove non-op line
fmauch Oct 16, 2024
c4022a2
Set default controller update rate parameter to 100
fmauch Oct 16, 2024
8bb8b81
Merge branch 'master' into jtc_tdt
fmauch Nov 7, 2024
6945360
Merge remote-tracking branch 'origin/master' into jtc_tdt
fmauch Nov 11, 2024
0d00727
Use current time step for checking tolerances
fmauch Nov 11, 2024
353f664
Allow not going forward when sampling a trajectory
fmauch Nov 13, 2024
b8e5de4
progress_search_start_index -> search_monotonically_increasing
bmagyar Nov 21, 2024
6c81466
Remove redundant sample call
fmauch Nov 21, 2024
43c4ac4
Merge remote-tracking branch 'origin/master' into jtc_tdt
fmauch Nov 21, 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
1 change: 1 addition & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// Parameters from ROS for joint_trajectory_controller
std::shared_ptr<ParamListener> param_listener_;
Params params_;
rclcpp::Duration update_period_{0, 0};

trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
/// Specify interpolation method. Default to splines.
Expand Down
18 changes: 15 additions & 3 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <functional>
#include <memory>

#include <stdexcept>
#include <string>
#include <vector>

Expand Down Expand Up @@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update(
if (has_active_trajectory())
{
bool first_sample = false;
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
// if sampling the first time, set the point before you sample
if (!traj_external_point_ptr_->is_sampled_already())
{
Expand All @@ -194,12 +196,15 @@ controller_interface::return_type JointTrajectoryController::update(
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
// Sample once at the beginning to establish the start time if none was given
traj_external_point_ptr_->sample(
time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
fmauch marked this conversation as resolved.
Show resolved Hide resolved
}

// find segment for current timestamp
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
// Sample setpoint for next control cycle
const bool valid_point = traj_external_point_ptr_->sample(
time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
time + update_period_, interpolation_method_, state_desired_, start_segment_itr,
end_segment_itr);
saikishor marked this conversation as resolved.
Show resolved Hide resolved

if (valid_point)
{
Expand Down Expand Up @@ -867,6 +872,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
std::string(get_node()->get_name()) + "/query_state",
std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));

if (get_update_rate() == 0)
{
throw std::runtime_error("Controller's update rate is set to 0. This should not happen!");
}
update_period_ =
rclcpp::Duration(0.0, static_cast<uint32_t>(1.0e9 / static_cast<double>(get_update_rate())));

return CallbackReturn::SUCCESS;
}

Expand Down
127 changes: 85 additions & 42 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
traj_controller_->get_node()->set_parameter(
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));

const auto state = traj_controller_->get_node()->configure();
const auto state = traj_controller_->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

// send msg
Expand Down Expand Up @@ -84,7 +84,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
rclcpp::executors::MultiThreadedExecutor executor;
SetUpTrajectoryController(executor);

const auto state = traj_controller_->get_node()->configure();
const auto state = traj_controller_->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

compare_joints(joint_names_, joint_names_);
Expand All @@ -97,7 +97,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_);
SetUpTrajectoryController(executor, {command_joint_names_param});

const auto state = traj_controller_->get_node()->configure();
const auto state = traj_controller_->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

compare_joints(joint_names_, command_joint_names_);
Expand All @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
rclcpp::executors::MultiThreadedExecutor executor;
SetUpTrajectoryController(executor);

auto state = traj_controller_->get_node()->configure();
auto state = traj_controller_->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

auto cmd_if_conf = traj_controller_->command_interface_configuration();
Expand Down Expand Up @@ -162,7 +162,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure)
SetUpTrajectoryController(executor);

// configure controller
auto state = traj_controller_->get_node()->configure();
auto state = traj_controller_->configure();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());

// cleanup controller
Expand All @@ -178,6 +178,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
SetUpTrajectoryController(executor);
traj_controller_->get_node()->set_parameter(
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10));

// This call is replacing the way parameters are set via launch
auto state = traj_controller_->configure();
Expand Down Expand Up @@ -206,8 +207,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param

// wait for reaching the first point
// controller would process the second point when deactivated below
// Since the trajectory will be sampled at time + update_period, we need to pass the time 0.15
// seconds to sample at t=0.25 sec
traj_controller_->update(
rclcpp::Time(static_cast<uint64_t>(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15));
rclcpp::Time(static_cast<uint64_t>(0.15 * 1e9)), rclcpp::Duration::from_seconds(0.15));
EXPECT_TRUE(traj_controller_->has_active_traj());
if (traj_controller_->has_position_command_interface())
{
Expand Down Expand Up @@ -644,7 +647,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
size_t n_joints = joint_names_.size();

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300);
saikishor marked this conversation as resolved.
Show resolved Hide resolved
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
Expand All @@ -653,7 +656,12 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
publish(time_from_start, points, rclcpp::Time());
traj_controller_->wait_for_trajectory(executor);

updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
const rclcpp::Duration controller_period =
rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate());

updateControllerAsync(
rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME),
controller_period);

// get states from class variables
auto state_feedback = traj_controller_->get_state_feedback();
Expand Down Expand Up @@ -745,7 +753,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
size_t n_joints = joint_names_.size();

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300);
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
Expand All @@ -756,7 +764,11 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);

updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));
const rclcpp::Duration controller_period =
rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate());
updateControllerAsync(
rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME),
controller_period);

// get states from class variables
auto state_feedback = traj_controller_->get_state_feedback();
Expand Down Expand Up @@ -1548,16 +1560,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
expected_actual.positions = {points_old[0].begin(), points_old[0].end()};
expected_desired = expected_actual;
traj_controller_->wait_for_trajectory(executor);
auto end_time = updateControllerAsync(
rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME),
rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()));

// Check that we reached end of points_old[0] trajectory
auto end_time =
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
if (traj_controller_->has_position_command_interface())
{
auto state_feedback = traj_controller_->get_state_feedback();
for (size_t i = 0; i < expected_actual.positions.size(); ++i)
{
EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01);
}
}

RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past");
// New trajectory will end before current time
rclcpp::Time new_traj_start =
rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100);
expected_actual.positions = {points_old[1].begin(), points_old[1].end()};
expected_desired = expected_actual;
expected_desired = expected_actual; // robot should be standing still by now
publish(time_from_start, points_new, new_traj_start);
waitAndCompareState(
expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time);
Expand All @@ -1577,10 +1600,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory
// send points_old and wait to reach first point
publish(time_from_start, points_old, rclcpp::Time());
expected_actual.positions = {points_old[0].begin(), points_old[0].end()};
expected_desired = expected_actual;
// Check that we reached end of points_old[0]trajectory
auto end_time =
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
traj_controller_->wait_for_trajectory(executor);
auto end_time = updateControllerAsync(
rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME),
rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()));

if (traj_controller_->has_position_command_interface())
{
// Check that we reached end of points_old[0] trajectory
auto state_feedback = traj_controller_->get_state_feedback();
for (size_t i = 0; i < expected_actual.positions.size(); ++i)
{
EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01);
}
}
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved

// send points_new before the old trajectory is finished
RCLCPP_INFO(
Expand Down Expand Up @@ -1653,6 +1686,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
// only makes sense with position command interface
return;
}
auto controller_period =
rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate());

// goal setup
std::vector<double> first_goal = {3.3, 4.4, 5.5};
Expand All @@ -1665,8 +1700,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
builtin_interfaces::msg::Duration time_from_start;
time_from_start.sec = 1;
time_from_start.nanosec = 0;
double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() /
(time_from_start.sec + time_from_start.nanosec * 1e-9);
double trajectory_frac =
controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9);
std::vector<std::vector<double>> points{{first_goal}};
publish(
time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities);
Expand All @@ -1690,7 +1725,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
// One the first update(s) there should be a "jump" in opposite direction from command
// (towards the state value)
EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD);
auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
auto end_time = updateControllerAsync(controller_period);
// Expect backward commands at first, consider advancement of the trajectory
// exact value is not directly predictable, because of the spline interpolation -> increase
// tolerance
Expand All @@ -1699,10 +1734,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
0.1);
EXPECT_GT(joint_pos_[0], joint_state_pos_[0]);
EXPECT_LT(joint_pos_[0], first_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_GT(joint_pos_[0], joint_state_pos_[0]);
EXPECT_LT(joint_pos_[0], first_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_GT(joint_pos_[0], joint_state_pos_[0]);
EXPECT_LT(joint_pos_[0], first_goal[0]);

Expand All @@ -1721,17 +1756,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
// One the first update(s) there should be a "jump" in the goal direction from command
// (towards the state value)
EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
end_time = updateControllerAsync(controller_period);
// Expect backward commands at first, consider advancement of the trajectory
// exact value is not directly predictable, because of the spline interpolation -> increase
// tolerance
EXPECT_NEAR(
joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0],
COMMON_THRESHOLD);
0.1);
EXPECT_LT(joint_pos_[0], joint_state_pos_[0]);
EXPECT_GT(joint_pos_[0], first_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_LT(joint_pos_[0], joint_state_pos_[0]);
EXPECT_GT(joint_pos_[0], first_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_LT(joint_pos_[0], joint_state_pos_[0]);
EXPECT_GT(joint_pos_[0], first_goal[0]);

Expand All @@ -1747,13 +1784,17 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
rclcpp::executors::SingleThreadedExecutor executor;
// set open loop to true, this should change behavior from above
rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
rclcpp::Parameter update_rate_param("update_rate", 100);
SetUpAndActivateTrajectoryController(
executor, {is_open_loop_parameters, update_rate_param}, true);

if (traj_controller_->has_position_command_interface() == false)
{
// only makes sense with position command interface
return;
}
auto controller_period =
rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate());

// goal setup
std::vector<double> first_goal = {3.3, 4.4, 5.5};
Expand All @@ -1764,8 +1805,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
builtin_interfaces::msg::Duration time_from_start;
time_from_start.sec = 1;
time_from_start.nanosec = 0;
double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() /
(time_from_start.sec + time_from_start.nanosec * 1e-9);
double trajectory_frac =
controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9);
std::vector<std::vector<double>> points{{first_goal}};
publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME));
traj_controller_->wait_for_trajectory(executor);
Expand All @@ -1788,17 +1829,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
// One the first update(s) there **should not** be a "jump" in opposite direction from
// command (towards the state value)
EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD);
auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01));
auto end_time = updateControllerAsync(controller_period);
// There should not be backward commands
// exact value is not directly predictable, because of the spline interpolation -> increase
// tolerance
EXPECT_NEAR(
first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0],
COMMON_THRESHOLD);
first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], 0.1);
EXPECT_GT(joint_pos_[0], first_goal[0]);
EXPECT_LT(joint_pos_[0], second_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_GT(joint_pos_[0], first_goal[0]);
EXPECT_LT(joint_pos_[0], second_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_GT(joint_pos_[0], first_goal[0]);
EXPECT_LT(joint_pos_[0], second_goal[0]);

Expand All @@ -1817,17 +1859,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
// One the first update(s) there **should not** be a "jump" in the goal direction from
// command (towards the state value)
EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
// There should not be a jump toward commands
// exact value is not directly predictable, because of the spline interpolation -> increase
// tolerance
EXPECT_NEAR(
second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0],
COMMON_THRESHOLD);
second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], 0.1);
EXPECT_LT(joint_pos_[0], second_goal[0]);
EXPECT_GT(joint_pos_[0], first_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_GT(joint_pos_[0], first_goal[0]);
EXPECT_LT(joint_pos_[0], second_goal[0]);
end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time);
end_time = updateControllerAsync(controller_period, end_time);
EXPECT_GT(joint_pos_[0], first_goal[0]);
EXPECT_LT(joint_pos_[0], second_goal[0]);

Expand Down Expand Up @@ -2098,7 +2141,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
// command interfaces: empty
command_interface_types_ = {};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
auto state = traj_controller_->get_node()->configure();
auto state = traj_controller_->configure();
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);

// command interfaces: bad_name
Expand Down Expand Up @@ -2143,7 +2186,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
command_interface_types_ = {"velocity"};
state_interface_types_ = {"position"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
state = traj_controller_->get_node()->configure();
state = traj_controller_->configure();
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
state_interface_types_ = {"velocity"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
Expand All @@ -2152,7 +2195,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
command_interface_types_ = {"effort"};
state_interface_types_ = {"position"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
state = traj_controller_->get_node()->configure();
state = traj_controller_->configure();
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
state_interface_types_ = {"velocity"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
Expand Down
Loading
Loading