Skip to content

Use lifecycle node as base for controllers #244

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 12 commits into from
Mar 25, 2022
Merged

Use lifecycle node as base for controllers #244

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
2299a33
Add lifecycle nodes
Sep 21, 2021
484ad0f
Fix compile issues
Sep 21, 2021
e198942
Fix trajectory controller
Sep 21, 2021
1d7fdd6
pre-commit fixes
Sep 21, 2021
88eed0e
Add time and period to update function
bmagyar Sep 18, 2021
f3abd73
Add lifecycle nodes
Sep 21, 2021
3348ea0
Name changes
Sep 21, 2021
12d5ede
Merge branch 'master' into pr-lifecycle_nodes
destogl Mar 9, 2022
4b2eb0c
Final fixes to get things compiled.
destogl Mar 9, 2022
b6c22b1
Fixing typo in the tests.
destogl Mar 15, 2022
9af7dca
More changes to LifecycleNodes.
destogl Mar 16, 2022
7174ac6
Merge branch 'master' into pr-lifecycle_nodes
bmagyar Mar 25, 2022
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
18 changes: 9 additions & 9 deletions diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -339,11 +339,11 @@ TEST_F(TestDiffDriveController, cleanup)

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());
auto state = controller_->configure();
auto state = controller_->get_node()->configure();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
assignResourcesPosFeedback();

state = controller_->activate();
state = controller_->get_node()->activate();
ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());

waitForSetup();
Expand All @@ -358,13 +358,13 @@ TEST_F(TestDiffDriveController, cleanup)
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

state = controller_->deactivate();
state = controller_->get_node()->deactivate();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
ASSERT_EQ(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

state = controller_->cleanup();
state = controller_->get_node()->cleanup();
ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());

// should be stopped
Expand All @@ -389,14 +389,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());

auto state = controller_->configure();
auto state = controller_->get_node()->configure();
assignResourcesPosFeedback();

ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value());

state = controller_->activate();
state = controller_->get_node()->activate();
ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());

// send msg
Expand All @@ -415,7 +415,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
// deactivated
// wait so controller process the second point when deactivated
std::this_thread::sleep_for(std::chrono::milliseconds(500));
state = controller_->deactivate();
state = controller_->get_node()->deactivate();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
ASSERT_EQ(
controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
Expand All @@ -425,12 +425,12 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";

// cleanup
state = controller_->cleanup();
state = controller_->get_node()->cleanup();
ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value());
EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value());

state = controller_->configure();
state = controller_->get_node()->configure();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
executor.cancel();
}
4 changes: 2 additions & 2 deletions effort_controllers/test/test_joint_group_effort_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,10 +188,10 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);

auto node_state = controller_->configure();
auto node_state = controller_->get_node()->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

node_state = controller_->activate();
node_state = controller_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

// send a new command
Expand Down
10 changes: 5 additions & 5 deletions forward_command_controller/test/test_forward_command_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -250,10 +250,10 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);

auto node_state = controller_->configure();
auto node_state = controller_->get_node()->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

node_state = controller_->activate();
node_state = controller_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

// send a new command
Expand Down Expand Up @@ -296,7 +296,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
auto node_state = controller_->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

node_state = controller_->activate();
node_state = controller_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>();
Expand All @@ -314,7 +314,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20);
ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30);

node_state = controller_->deactivate();
node_state = controller_->get_node()->deactivate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

// command ptr should be reset (nullptr) after deactivation - same check as in `update`
Expand All @@ -338,7 +338,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT()));

// Now activate again
node_state = controller_->activate();
node_state = controller_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

// command ptr should be reset (nullptr) after activation - same check as in `update`
Expand Down
12 changes: 6 additions & 6 deletions gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,10 @@
// TODO(JafarAbdi): Remove experimental once the default standard is C++17
#include "experimental/optional"

#include "rclcpp/time.hpp"

#include "control_toolbox/pid.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

/**
* \brief Helper class to simplify integrating the GripperActionController with
Expand All @@ -46,7 +46,7 @@ class HardwareInterfaceAdapter
bool init(
std::experimental::optional<
std::reference_wrapper<hardware_interface::LoanedCommandInterface>> /* joint_handle */,
const rclcpp::Node::SharedPtr & /* node */)
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & /* node */)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could have used rclcpp_lifecycle::LifecycleNode::SharedPtr(pretty sure this is defined) as with rclcpp::Node but there is no difference.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it be better to pass a reference instead of shard_ptr here too?

{
return false;
}
Expand All @@ -73,7 +73,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
bool init(
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle,
const rclcpp::Node::SharedPtr & /* node */)
const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */)
{
joint_handle_ = joint_handle;
return true;
Expand Down Expand Up @@ -116,7 +116,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
public:
template <typename ParameterT>
auto auto_declare(
const rclcpp::Node::SharedPtr & node, const std::string & name,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const std::string & name,
const ParameterT & default_value)
{
if (!node->has_parameter(name))
Expand All @@ -132,7 +132,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
bool init(
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle,
const rclcpp::Node::SharedPtr & node)
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
{
joint_handle_ = joint_handle;
// Init PID gains from ROS parameter server
Expand Down
12 changes: 6 additions & 6 deletions joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -587,8 +587,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest)
{
SetUpStateBroadcaster();

auto node_state = state_broadcaster_->configure();
node_state = state_broadcaster_->activate();
auto node_state = state_broadcaster_->get_node()->configure();
node_state = state_broadcaster_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
ASSERT_EQ(
state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
Expand All @@ -597,10 +597,10 @@ TEST_F(JointStateBroadcasterTest, UpdateTest)

void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic)
{
auto node_state = state_broadcaster_->configure();
auto node_state = state_broadcaster_->get_node()->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

node_state = state_broadcaster_->activate();
node_state = state_broadcaster_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

rclcpp::Node test_node("test_node");
Expand Down Expand Up @@ -649,10 +649,10 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic)
void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(
const std::string & topic)
{
auto node_state = state_broadcaster_->configure();
auto node_state = state_broadcaster_->get_node()->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

node_state = state_broadcaster_->activate();
node_state = state_broadcaster_->get_node()->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

rclcpp::Node test_node("test_node");
Expand Down
2 changes: 1 addition & 1 deletion joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ struct SegmentTolerances
* \return Trajectory segment tolerances.
*/
SegmentTolerances get_segment_tolerances(
const rclcpp::Node & node, const std::vector<std::string> & joint_names)
const rclcpp_lifecycle::LifecycleNode & node, const std::vector<std::string> & joint_names)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should probably here use SharedPtr to the node and not the object itself. This is usually done in the rest of the code-base.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would disagree.

If the function itself doesn't care about the lifetime of the object, then passing by reference should be preferred. Passing a shared pointer has performance costs in incrementing/decrementing the reference count.

See https://herbsutter.com/2013/06/05/gotw-91-solution-smart-pointer-parameters for discussion on this topic.

{
const auto n_joints = joint_names.size();
SegmentTolerances tolerances;
Expand Down
2 changes: 1 addition & 1 deletion joint_trajectory_controller/test/test_trajectory_actions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest

SetUpAndActivateTrajectoryController(true, parameters);

executor_->add_node(traj_node_->get_node_base_interface());
executor_->add_node(traj_controller_->get_node()->get_node_base_interface());

SetUpActionClient();

Expand Down
32 changes: 17 additions & 15 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 @@ -70,7 +70,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure)
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
const auto future_handle_ = std::async(std::launch::async, spin, &executor);

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

// send msg
Expand Down Expand Up @@ -98,7 +98,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(traj_controller_->get_node()->get_node_base_interface());

traj_controller_->configure();
traj_controller_->get_node()->configure();
ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE);

auto cmd_interface_config = traj_controller_->command_interface_configuration();
Expand Down Expand Up @@ -245,11 +245,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
traj_controller_->wait_for_trajectory(executor);
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));

auto state = traj_controller_->deactivate();
auto state = traj_controller_->get_node()->deactivate();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));

state = traj_controller_->cleanup();
state = traj_controller_->get_node()->cleanup();
ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
// update for 0.25 seconds
const auto start_time = rclcpp::Clock().now();
Expand All @@ -270,7 +270,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
// This call is replacing the way parameters are set via launch
SetParameters();
SetPidParameters();
traj_controller_->configure();
traj_controller_->get_node()->configure();
auto state = traj_controller_->get_state();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());

Expand Down Expand Up @@ -303,7 +303,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
std::this_thread::sleep_for(FIRST_POINT_TIME);
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
// deactivated
state = traj_controller_->deactivate();
state = traj_controller_->get_node()->deactivate();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

// TODO(denis): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong?
Expand All @@ -324,7 +324,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
}

// cleanup
state = traj_controller_->cleanup();
state = traj_controller_->get_node()->cleanup();

// update loop receives a new msg and updates accordingly
traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
Expand All @@ -338,7 +338,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], allowed_delta);
EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], allowed_delta);

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

executor.cancel();
Expand Down Expand Up @@ -408,7 +408,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou
const int qos_level = 10;
int echo_received_counter = 0;
rclcpp::Subscription<JointTrajectoryControllerState>::SharedPtr subs =
traj_node_->create_subscription<JointTrajectoryControllerState>(
traj_controller_->get_node()->create_subscription<JointTrajectoryControllerState>(
controller_name_ + "/state", qos_level,
[&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; });

Expand Down Expand Up @@ -721,10 +721,11 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
// Denis: delta was 0.1 with 0.2 works for me
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2);

RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory");
RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory");
points_partial_new_velocities[0][0] =
copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]);
publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities);

// Replaced trajectory is a mix of previous and current goal
expected_desired.positions[0] = points_partial_new[0][0];
expected_desired.positions[1] = points_old[0][1];
Expand Down Expand Up @@ -758,7 +759,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
// Check that we reached end of points_old[0] trajectory
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);

RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory in the past");
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().now() - delay - std::chrono::milliseconds(100);
expected_actual.positions = {points_old[1].begin(), points_old[1].end()};
Expand All @@ -785,7 +786,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory
// Check that we reached end of points_old[0]trajectory
waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1);

RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory partially in the past");
RCLCPP_INFO(
traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past");
// New trajectory first point is in the past, second is in the future
rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100);
expected_actual.positions = {points_new[1].begin(), points_new[1].end()};
Expand All @@ -810,8 +812,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
subscribeToState();
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true);
traj_node->set_parameter(partial_joints_parameters);
traj_controller_->configure();
traj_controller_->activate();
traj_controller_->get_node()->configure();
traj_controller_->get_node()->activate();

std::vector<std::vector<double>> full_traj{{{2., 3., 4.}, {4., 6., 8.}}};
std::vector<std::vector<double>> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}};
Expand Down Expand Up @@ -1183,7 +1185,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame
auto set_parameter_and_check_result = [&]() {
EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED);
SetParameters(); // This call is replacing the way parameters are set via launch
traj_controller_->configure();
traj_controller_->get_node()->configure();
EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED);
};

Expand Down
Loading