diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ab007759ef..ce5a6b09a2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -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(); @@ -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 @@ -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 @@ -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)), @@ -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(); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 157384423a..f8cd10bc4b 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -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 diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index d3dbe22c8d..e217f88a1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -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 @@ -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(); @@ -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` @@ -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` diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index cbf38e4e0a..40fde4442c 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -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 @@ -46,7 +46,7 @@ class HardwareInterfaceAdapter bool init( std::experimental::optional< std::reference_wrapper> /* joint_handle */, - const rclcpp::Node::SharedPtr & /* node */) + std::shared_ptr & /* node */) { return false; } @@ -73,7 +73,7 @@ class HardwareInterfaceAdapter bool init( std::experimental::optional> joint_handle, - const rclcpp::Node::SharedPtr & /* node */) + const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) { joint_handle_ = joint_handle; return true; @@ -116,7 +116,7 @@ class HardwareInterfaceAdapter public: template auto auto_declare( - const rclcpp::Node::SharedPtr & node, const std::string & name, + const std::shared_ptr & node, const std::string & name, const ParameterT & default_value) { if (!node->has_parameter(name)) @@ -132,7 +132,7 @@ class HardwareInterfaceAdapter bool init( std::experimental::optional> joint_handle, - const rclcpp::Node::SharedPtr & node) + const std::shared_ptr & node) { joint_handle_ = joint_handle; // Init PID gains from ROS parameter server diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 1ab4b4323d..e124708fe5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -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)), @@ -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"); @@ -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"); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b2944f7622..8f3f30d476 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -92,7 +92,7 @@ struct SegmentTolerances * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( - const rclcpp::Node & node, const std::vector & joint_names) + const rclcpp_lifecycle::LifecycleNode & node, const std::vector & joint_names) { const auto n_joints = joint_names.size(); SegmentTolerances tolerances; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 5bb6efaea6..a7926d2278 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -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(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 965e9a5919..8771fcee41 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -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 @@ -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(); @@ -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(); @@ -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()); @@ -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? @@ -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)); @@ -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(); @@ -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::SharedPtr subs = - traj_node_->create_subscription( + traj_controller_->get_node()->create_subscription( controller_name_ + "/state", qos_level, [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); @@ -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]; @@ -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()}; @@ -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()}; @@ -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> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; @@ -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); }; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 167f82aaad..94b86a6242 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -171,24 +171,24 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_node(); + for (const auto & param : parameters) { - traj_node_->set_parameter(param); + traj_controller_->get_node()->set_parameter(param); } if (executor) { - executor->add_node(traj_node_->get_node_base_interface()); + executor->add_node(traj_controller_->get_node()->get_node_base_interface()); } // ignore velocity tolerances for this test since they aren't committed in test_robot->write() rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); - traj_node_->set_parameter(stopped_velocity_parameters); + traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); // set pid parameters before configure SetPidParameters(); + traj_controller_->get_node()->configure(); - traj_controller_->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); } @@ -237,7 +237,7 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->activate(); + traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } @@ -391,7 +391,6 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Publisher::SharedPtr trajectory_publisher_; std::shared_ptr traj_controller_; - std::shared_ptr traj_node_; rclcpp::Subscription::SharedPtr state_subscriber_; mutable std::mutex state_mutex_; diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index be774031f0..fc93bb7dfa 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -188,10 +188,10 @@ TEST_F(JointGroupPositionControllerTest, 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 diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index b566829f9b..1bcbaf510e 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -188,10 +188,10 @@ TEST_F(JointGroupVelocityControllerTest, 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