Skip to content

Commit

Permalink
Add initial fix of the command subscription to the JTC
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Jul 15, 2024
1 parent 7d7914e commit 71f8066
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 36 deletions.
5 changes: 5 additions & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
const rclcpp_lifecycle::State & previous_state) override;

protected:
virtual rclcpp::CallbackGroup::SharedPtr get_callback_group()
{
return get_node()->get_node_base_interface()->get_default_callback_group();
}

// To reduce number of variables and to make the code shorter the interfaces are ordered in types
// as the following constants
const std::vector<std::string> allowed_interface_types_ = {
Expand Down
4 changes: 3 additions & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -796,10 +796,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
init_hold_position_msg();

// create subscriber and publishers
rclcpp::SubscriptionOptions options;
options.callback_group = get_callback_group();
joint_command_subscriber_ =
get_node()->create_subscription<trajectory_msgs::msg::JointTrajectory>(
"~/joint_trajectory", rclcpp::SystemDefaultsQoS(),
std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1));
std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1), options);

publisher_ = get_node()->create_publisher<ControllerStateMsg>(
"~/controller_state", rclcpp::SystemDefaultsQoS());
Expand Down
57 changes: 27 additions & 30 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

traj_controller_->update(
rclcpp::Time(static_cast<uint64_t>(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5));
Expand Down Expand Up @@ -146,7 +146,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

traj_controller_->update(
rclcpp::Time(static_cast<uint64_t>(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5));
Expand Down Expand Up @@ -204,7 +204,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

// first update
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
Expand Down Expand Up @@ -656,7 +656,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
{{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time());
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));

Expand Down Expand Up @@ -759,7 +759,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));

Expand Down Expand Up @@ -897,7 +897,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout)
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4);

Expand Down Expand Up @@ -947,7 +947,7 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
// *INDENT-ON*

publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

// update until end of trajectory -> no timeout should have occurred
updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3);
Expand Down Expand Up @@ -1019,7 +1019,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error)
{{0.1, 0.1, 0.1}}, {{0.2, 0.2, 0.2}}, {{0.3, 0.3, 0.3}}};
// *INDENT-ON*
publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));

Expand Down Expand Up @@ -1105,7 +1105,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
trajectory_publisher_->publish(traj_msg);
}

traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
updateControllerAsync(rclcpp::Duration::from_seconds(dt));

if (traj_controller_->has_position_command_interface())
Expand Down Expand Up @@ -1183,7 +1183,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list)
trajectory_publisher_->publish(traj_msg);
}

traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
updateControllerAsync(rclcpp::Duration::from_seconds(dt));

if (traj_controller_->has_position_command_interface())
Expand Down Expand Up @@ -1270,7 +1270,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe
trajectory_publisher_->publish(traj_msg);
}

traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
// update for 0.5 seconds
updateControllerAsync(rclcpp::Duration::from_seconds(0.25));

Expand Down Expand Up @@ -1515,7 +1515,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()};
// Check that we reached end of points_old trajectory
auto end_time =
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1);

RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory");
points_partial_new_velocities[0][0] =
Expand All @@ -1530,8 +1530,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
expected_desired.velocities[1] = 0.0;
expected_desired.velocities[2] = 0.0;
expected_actual = expected_desired;
waitAndCompareState(
expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1, end_time);
}

/**
Expand All @@ -1555,7 +1554,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
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);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1);

RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past");
// New trajectory will end before current time
Expand All @@ -1564,8 +1563,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
expected_actual.positions = {points_old[1].begin(), points_old[1].end()};
expected_desired = expected_actual;
publish(time_from_start, points_new, new_traj_start);
waitAndCompareState(
expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1, end_time);
}

TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory)
Expand All @@ -1585,7 +1583,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory
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);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1);

// send points_new before the old trajectory is finished
RCLCPP_INFO(
Expand All @@ -1596,8 +1594,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory
// it should not have accepted the new goal but finish the old one
expected_actual.positions = {points_old[1].begin(), points_old[1].end()};
expected_desired.positions = {points_old[1].begin(), points_old[1].end()};
waitAndCompareState(
expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1, end_time);
}

TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future)
Expand Down Expand Up @@ -1630,7 +1627,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
expected_desired = expected_actual;
// Check that we reached end of points_old[0]trajectory and are starting points_old[1]
auto end_time =
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);
waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1);

// Send partial trajectory starting after full trajecotry is complete
RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future");
Expand All @@ -1643,7 +1640,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
expected_desired = expected_actual;

waitAndCompareState(
expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time);
expected_actual, expected_desired, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time);
}

TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated)
Expand Down Expand Up @@ -1675,7 +1672,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
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);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
updateControllerAsync(rclcpp::Duration::from_seconds(1.1));

// JTC is executing trajectory in open-loop therefore:
Expand All @@ -1690,7 +1687,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
// Move joint further in the same direction as before (to the second goal)
points = {{second_goal}};
publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

// One the first update(s) there should be a "jump" in opposite direction from command
// (towards the state value)
Expand Down Expand Up @@ -1721,7 +1718,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
// Move joint back to the first goal
points = {{first_goal}};
publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME));
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

// One the first update(s) there should be a "jump" in the goal direction from command
// (towards the state value)
Expand Down Expand Up @@ -1773,7 +1770,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
(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);
traj_controller_->wait_for_trajectory();
updateControllerAsync(rclcpp::Duration::from_seconds(1.1));

// JTC is executing trajectory in open-loop therefore:
Expand All @@ -1788,7 +1785,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
// Move joint further in the same direction as before (to the second goal)
points = {{second_goal}};
publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME));
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

// One the first update(s) there **should not** be a "jump" in opposite direction from
// command (towards the state value)
Expand Down Expand Up @@ -1817,7 +1814,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
// Move joint back to the first goal
points = {{first_goal}};
publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME));
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();

// One the first update(s) there **should not** be a "jump" in the goal direction from
// command (towards the state value)
Expand Down Expand Up @@ -1988,7 +1985,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail)
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME));

// it should have aborted and be holding now
Expand Down Expand Up @@ -2020,7 +2017,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities);
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME));

// it should have aborted and be holding now
Expand Down
28 changes: 23 additions & 5 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,11 @@ class TestableJointTrajectoryController
using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController;
using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg;

rclcpp::CallbackGroup::SharedPtr get_callback_group() override
{
return get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
}

controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override
{
Expand All @@ -111,13 +116,26 @@ class TestableJointTrajectoryController
* @return true if new JointTrajectory msg was received, false if timeout
*/
bool wait_for_trajectory(
rclcpp::Executor & executor,
const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
{
bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready;
const auto wait_result = joint_cmd_sub_wait_set_.wait(timeout);
bool success = wait_result.kind() == rclcpp::WaitResultKind::Ready;
if (success)
{
executor.spin_some();
if (wait_result.kind() == rclcpp::WaitResultKind::Ready)
{
if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U])
{
trajectory_msgs::msg::JointTrajectory msg;
rclcpp::MessageInfo msg_info;
if (joint_command_subscriber_->take(msg, msg_info))
{
std::shared_ptr<void> type_erased_msg =
std::make_shared<trajectory_msgs::msg::JointTrajectory>(msg);
joint_command_subscriber_->handle_message(type_erased_msg, msg_info);
}
}
}
}
return success;
}
Expand Down Expand Up @@ -544,15 +562,15 @@ class TrajectoryControllerTest : public ::testing::Test

rclcpp::Time waitAndCompareState(
trajectory_msgs::msg::JointTrajectoryPoint expected_actual,
trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor,
trajectory_msgs::msg::JointTrajectoryPoint expected_desired,
rclcpp::Duration controller_wait_time, double allowed_delta,
rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME))
{
{
std::lock_guard<std::mutex> guard(state_mutex_);
state_msg_.reset();
}
traj_controller_->wait_for_trajectory(executor);
traj_controller_->wait_for_trajectory();
auto end_time = updateControllerAsync(controller_wait_time, start_time);

// get states from class variables
Expand Down

0 comments on commit 71f8066

Please sign in to comment.