diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index f908a777fb..304a645b97 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -124,7 +124,7 @@ DiffDriveController::init( controller_interface::return_type DiffDriveController::update() { - auto logger = lifecycle_node_->get_logger(); + auto logger = node_->get_logger(); if (lifecycle_node_->get_current_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { halt(); @@ -133,7 +133,7 @@ controller_interface::return_type DiffDriveController::update() return controller_interface::return_type::SUCCESS; } - const auto current_time = lifecycle_node_->get_clock()->now(); + const auto current_time = node_->get_clock()->now(); double & linear_command = received_velocity_msg_ptr_->twist.linear.x; double & angular_command = received_velocity_msg_ptr_->twist.angular.z; @@ -255,7 +255,7 @@ controller_interface::return_type DiffDriveController::update() CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &) { - auto logger = lifecycle_node_->get_logger(); + auto logger = node_->get_logger(); // update parameters left_wheel_names_ = lifecycle_node_->get_parameter("left_wheel_names").as_string_array(); @@ -399,7 +399,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { RCLCPP_WARN( - lifecycle_node_->get_logger(), + node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } @@ -448,7 +448,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; - previous_update_timestamp_ = lifecycle_node_->get_clock()->now(); + previous_update_timestamp_ = node_->get_clock()->now(); set_op_mode(hardware_interface::OperationMode::INACTIVE); return CallbackReturn::SUCCESS; } @@ -465,7 +465,7 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) } RCLCPP_INFO( - lifecycle_node_->get_logger(), "Lifecycle subscriber and publisher are currently active."); + node_->get_logger(), "Lifecycle subscriber and publisher are currently active."); return CallbackReturn::SUCCESS; } @@ -549,7 +549,7 @@ CallbackReturn DiffDriveController::configure_side( std::vector & registered_handles, hardware_interface::RobotHardware & robot_hardware) { - auto logger = lifecycle_node_->get_logger(); + auto logger = node_->get_logger(); if (wheel_names.empty()) { std::stringstream ss; diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 7dddf3069f..c058fb6ddf 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -38,34 +38,34 @@ CallbackReturn ForwardCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { rclcpp::Parameter joints_param, interface_param; - if (!lifecycle_node_->get_parameter("joints", joint_names_)) { - RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'joints' parameter not set"); + if (!node_->get_parameter("joints", joint_names_)) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'joints' parameter not set"); return CallbackReturn::ERROR; } if (joint_names_.empty()) { - RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'joints' parameter was empty"); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'joints' parameter was empty"); return CallbackReturn::ERROR; } - if (!lifecycle_node_->get_parameter("interface_name", interface_name_)) { - RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'interface_name' parameter not set"); + if (!node_->get_parameter("interface_name", interface_name_)) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'interface_name' parameter not set"); return CallbackReturn::ERROR; } if (interface_name_.empty()) { - RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'interface_name' parameter was empty"); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'interface_name' parameter was empty"); return CallbackReturn::ERROR; } - joints_command_subscriber_ = lifecycle_node_->create_subscription( + joints_command_subscriber_ = node_->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - RCLCPP_INFO_STREAM(get_lifecycle_node()->get_logger(), "configure successful"); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -120,7 +120,7 @@ CallbackReturn ForwardCommandController::on_activate( ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Expected %u position command interfaces, got %u", joint_names_.size(), ordered_interfaces.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -146,8 +146,8 @@ controller_interface::return_type ForwardCommandController::update() if ((*joint_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_STREAM_THROTTLE( - get_lifecycle_node()->get_logger(), - *lifecycle_node_->get_clock(), 1000, + get_node()->get_logger(), + *node_->get_clock(), 1000, "command size (" << (*joint_commands)->data.size() << ") does not match number of interfaces (" << command_interfaces_.size() << ")"); diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 5b90165fa1..434f5acdc1 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -86,7 +86,7 @@ void ForwardCommandControllerTest::SetUpController() TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) { SetUpController(); - controller_->lifecycle_node_->declare_parameter("interface_name", "dummy"); + controller_->node_->declare_parameter("interface_name", "dummy"); // configure failed, 'joints' parameter not set ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -97,21 +97,21 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet) SetUpController(); // configure failed, 'interface_name' parameter not set - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector())); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - controller_->lifecycle_node_->declare_parameter("interface_name", ""); + controller_->node_->declare_parameter("interface_name", ""); } TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector())); - controller_->lifecycle_node_->declare_parameter("interface_name", ""); + controller_->node_->declare_parameter("interface_name", ""); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -122,10 +122,10 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) SetUpController(); // configure failed, 'interface_name' paremeter not set - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2"})); - controller_->lifecycle_node_->declare_parameter("interface_name", ""); + controller_->node_->declare_parameter("interface_name", ""); // configure failed, 'interface_name' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -135,10 +135,10 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) { SetUpController(); - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -148,16 +148,16 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint4"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - auto result = controller_->lifecycle_node_->set_parameter( + auto result = controller_->node_->set_parameter( rclcpp::Parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2"}))); @@ -172,10 +172,10 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) { SetUpController(); - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "acceleration"); + controller_->node_->declare_parameter("interface_name", "acceleration"); // activate failed, 'joint4' not in interfaces ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -186,10 +186,10 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) { SetUpController(); - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -200,10 +200,10 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) SetUpController(); // configure controller - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -234,10 +234,10 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) SetUpController(); // configure controller - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong numnber of joints @@ -260,10 +260,10 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) SetUpController(); // configure controller - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -279,27 +279,27 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) { SetUpController(); - controller_->lifecycle_node_->declare_parameter( + controller_->node_->declare_parameter( "joints", rclcpp::ParameterValue(std::vector{"joint1", "joint2", "joint3"})); - controller_->lifecycle_node_->declare_parameter("interface_name", "position"); + controller_->node_->declare_parameter("interface_name", "position"); // default values ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); - auto node_state = controller_->get_lifecycle_node()->configure(); + auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_lifecycle_node()->activate(); + node_state = controller_->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( std::string( - controller_->get_lifecycle_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); std_msgs::msg::Float64MultiArray command_msg; command_msg.data = {10.0, 20.0, 30.0}; command_pub->publish(command_msg); @@ -308,7 +308,7 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); // process callbacks - rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface()); + rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful ASSERT_EQ(controller_->update(), controller_interface::return_type::SUCCESS); diff --git a/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp b/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp index 16023f0e83..82dfe76f4c 100644 --- a/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp +++ b/joint_state_controller/include/joint_state_controller/joint_state_controller.hpp @@ -67,14 +67,14 @@ class JointStateController : public controller_interface::ControllerInterface // For the JointState message, // we store the name of joints with compatible interfaces std::vector joint_names_; - std::shared_ptr> + std::shared_ptr> joint_state_publisher_; sensor_msgs::msg::JointState joint_state_msg_; // For the DynamicJointState format, we use a map to buffer values in for easier lookup // This allows to preserve whatever order or names/interfaces were initialized. std::unordered_map> name_if_value_mapping_; - std::shared_ptr> + std::shared_ptr> dynamic_joint_state_publisher_; control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; }; diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp index 91fce2619c..f5326c6316 100644 --- a/joint_state_controller/src/joint_state_controller.cpp +++ b/joint_state_controller/src/joint_state_controller.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" @@ -66,12 +67,15 @@ const rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointStateController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!node_.get()) { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } try { - joint_state_publisher_ = lifecycle_node_->create_publisher( + joint_state_publisher_ = node_->create_publisher( "joint_states", rclcpp::SystemDefaultsQoS()); dynamic_joint_state_publisher_ = - lifecycle_node_->create_publisher( + node_->create_publisher( "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); } catch (...) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -89,18 +93,12 @@ JointStateController::on_activate(const rclcpp_lifecycle::State & /*previous_sta init_joint_state_msg(); init_dynamic_joint_state_msg(); - joint_state_publisher_->on_activate(); - dynamic_joint_state_publisher_->on_activate(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointStateController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - joint_state_publisher_->on_deactivate(); - dynamic_joint_state_publisher_->on_deactivate(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -190,28 +188,24 @@ double get_value( controller_interface::return_type JointStateController::update() { + if (lifecycle_state_.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), "JointStateController is not activated"); + return controller_interface::return_type::ERROR; + } + for (const auto & state_interface : state_interfaces_) { name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] = state_interface.get_value(); RCLCPP_DEBUG( - get_lifecycle_node()->get_logger(), "%s/%s: %f\n", + get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(), state_interface.get_interface_name().c_str(), state_interface.get_value()); } - if (!joint_state_publisher_->is_activated()) { - RCUTILS_LOG_WARN_ONCE_NAMED("publisher", "joint state publisher is not activated"); - return controller_interface::return_type::ERROR; - } - - if (!dynamic_joint_state_publisher_->is_activated()) { - RCUTILS_LOG_WARN_ONCE_NAMED("publisher", "dynamic joint state publisher is not activated"); - return controller_interface::return_type::ERROR; - } - - joint_state_msg_.header.stamp = lifecycle_node_->get_clock()->now(); - dynamic_joint_state_msg_.header.stamp = lifecycle_node_->get_clock()->now(); + joint_state_msg_.header.stamp = node_->get_clock()->now(); + dynamic_joint_state_msg_.header.stamp = node_->get_clock()->now(); // update joint state message and dynamic joint state message for (auto i = 0ul; i < joint_names_.size(); ++i) { diff --git a/joint_state_controller/test/test_joint_state_controller.cpp b/joint_state_controller/test/test_joint_state_controller.cpp index 282c0a2fb7..82ffa2161c 100644 --- a/joint_state_controller/test/test_joint_state_controller.cpp +++ b/joint_state_controller/test/test_joint_state_controller.cpp @@ -191,13 +191,13 @@ TEST_F(JointStateControllerTest, UpdateTest) { SetUpStateController(); - auto node_state = state_controller_->get_lifecycle_node()->configure(); + auto node_state = state_controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); // publishers not activated yet ASSERT_EQ(state_controller_->update(), controller_interface::return_type::ERROR); - node_state = state_controller_->get_lifecycle_node()->activate(); + node_state = state_controller_->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(state_controller_->update(), controller_interface::return_type::SUCCESS); @@ -207,10 +207,10 @@ TEST_F(JointStateControllerTest, JointStatePublishTest) { SetUpStateController(); - auto node_state = state_controller_->get_lifecycle_node()->configure(); + auto node_state = state_controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = state_controller_->get_lifecycle_node()->activate(); + node_state = state_controller_->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); @@ -247,10 +247,10 @@ TEST_F(JointStateControllerTest, DynamicJointStatePublishTest) { SetUpStateController(); - auto node_state = state_controller_->get_lifecycle_node()->configure(); + auto node_state = state_controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = state_controller_->get_lifecycle_node()->activate(); + node_state = state_controller_->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/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 5a97143cd0..f28809fa7b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -140,7 +140,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; StatePublisherPtr state_publisher_; rclcpp::Duration state_publisher_period_ = rclcpp::Duration(RCUTILS_MS_TO_NS(20)); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index a01f5648df..fbe96fea51 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -96,7 +96,7 @@ struct SegmentTolerances * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( - const rclcpp_lifecycle::LifecycleNode & node, + const rclcpp::Node & node, const std::vector & joint_names) { const auto n_joints = joint_names.size(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7f95a9bf38..ddebd133b7 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -63,12 +63,12 @@ JointTrajectoryController::init(const std::string & controller_name) } // with the lifecycle node being initialized, we can declare parameters - lifecycle_node_->declare_parameter>("joints", joint_names_); - lifecycle_node_->declare_parameter("state_publish_rate", 50.0); - lifecycle_node_->declare_parameter("action_monitor_rate", 20.0); - lifecycle_node_->declare_parameter("allow_partial_joints_goal", allow_partial_joints_goal_); - lifecycle_node_->declare_parameter("constraints.stopped_velocity_tolerance", 0.01); - lifecycle_node_->declare_parameter("constraints.goal_time", 0.0); + node_->declare_parameter>("joints", joint_names_); + node_->declare_parameter("state_publish_rate", 50.0); + node_->declare_parameter("action_monitor_rate", 20.0); + node_->declare_parameter("allow_partial_joints_goal", allow_partial_joints_goal_); + node_->declare_parameter("constraints.stopped_velocity_tolerance", 0.01); + node_->declare_parameter("constraints.goal_time", 0.0); return controller_interface::return_type::SUCCESS; } @@ -101,7 +101,7 @@ state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update() { - if (lifecycle_node_->get_current_state().id() == State::PRIMARY_STATE_INACTIVE) { + if (get_current_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { halt(); is_halted = true; @@ -152,14 +152,14 @@ JointTrajectoryController::update() // if sampling the first time, set the point before you sample if (!(*traj_point_active_ptr_)->is_sampled_already()) { (*traj_point_active_ptr_)->set_point_before_trajectory_msg( - lifecycle_node_->now(), state_current); + node_->now(), state_current); } resize_joint_trajectory_point(state_error, joint_num); // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; const bool valid_point = (*traj_point_active_ptr_)->sample( - lifecycle_node_->now(), state_desired, + node_->now(), state_desired, start_segment_itr, end_segment_itr); if (valid_point) { @@ -189,7 +189,7 @@ JointTrajectoryController::update() if (rt_active_goal_) { // send feedback auto feedback = std::make_shared(); - feedback->header.stamp = lifecycle_node_->now(); + feedback->header.stamp = node_->now(); feedback->joint_names = joint_names_; feedback->actual = state_current; @@ -202,10 +202,10 @@ JointTrajectoryController::update() auto result = std::make_shared(); if (abort) { - RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); } else if (outside_goal_tolerance) { - RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to goal tolerance violation"); + RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation"); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); } rt_active_goal_->setAborted(result); @@ -220,20 +220,20 @@ JointTrajectoryController::update() rt_active_goal_->setSucceeded(res); rt_active_goal_.reset(); - RCLCPP_INFO(lifecycle_node_->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(node_->get_logger(), "Goal reached, success!"); } else if (default_tolerances_.goal_time_tolerance != 0.0) { // if we exceed goal_time_toleralance set it to aborted const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - const double difference = lifecycle_node_->now().seconds() - traj_end.seconds(); + const double difference = node_->now().seconds() - traj_end.seconds(); if (difference > default_tolerances_.goal_time_tolerance) { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); rt_active_goal_->setAborted(result); rt_active_goal_.reset(); RCLCPP_WARN( - lifecycle_node_->get_logger(), + node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", difference); } @@ -250,10 +250,10 @@ JointTrajectoryController::update() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) { - const auto logger = lifecycle_node_->get_logger(); + const auto logger = node_->get_logger(); // update parameters - joint_names_ = lifecycle_node_->get_parameter("joints").as_string_array(); + joint_names_ = node_->get_parameter("joints").as_string_array(); if (!reset()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -263,7 +263,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) RCLCPP_WARN(logger, "no joint names specified"); } - default_tolerances_ = get_segment_tolerances(*lifecycle_node_, joint_names_); + default_tolerances_ = get_segment_tolerances(*node_, joint_names_); // subscriber call back // non realtime @@ -284,7 +284,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // TODO(karsten1987): create subscriber with subscription deactivated joint_command_subscriber_ = - lifecycle_node_->create_subscription( + node_->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); // TODO(karsten1987): no lifecyle for subscriber yet @@ -292,7 +292,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // State publisher const double state_publish_rate = - lifecycle_node_->get_parameter("state_publish_rate").get_value(); + node_->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO_STREAM( logger, "Controller state will be published at " << state_publish_rate << "Hz."); @@ -303,7 +303,7 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = lifecycle_node_->create_publisher( + publisher_ = node_->create_publisher( "state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); @@ -320,16 +320,16 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) state_publisher_->msg_.error.velocities.resize(n_joints); state_publisher_->unlock(); - last_state_publish_time_ = lifecycle_node_->now(); + last_state_publish_time_ = node_->now(); // action server configuration - allow_partial_joints_goal_ = lifecycle_node_->get_parameter("allow_partial_joints_goal") + allow_partial_joints_goal_ = node_->get_parameter("allow_partial_joints_goal") .get_value(); if (allow_partial_joints_goal_) { RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } - const double action_monitor_rate = lifecycle_node_->get_parameter("action_monitor_rate") + const double action_monitor_rate = node_->get_parameter("action_monitor_rate") .get_value(); RCLCPP_INFO_STREAM( @@ -339,11 +339,11 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) using namespace std::placeholders; action_server_ = rclcpp_action::create_server( - lifecycle_node_->get_node_base_interface(), - lifecycle_node_->get_node_clock_interface(), - lifecycle_node_->get_node_logging_interface(), - lifecycle_node_->get_node_waitables_interface(), - std::string(lifecycle_node_->get_name()) + "/follow_joint_trajectory", + node_->get_node_base_interface(), + node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + std::string(node_->get_name()) + "/follow_joint_trajectory", std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), std::bind(&JointTrajectoryController::cancel_callback, this, _1), std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1) @@ -380,7 +380,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) joint_position_command_interface_)) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Expected %u position command interfaces, got %u", joint_names_.size(), joint_position_command_interface_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -390,7 +390,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) joint_position_state_interface_)) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Expected %u position state interfaces, got %u", joint_names_.size(), joint_position_state_interface_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -400,7 +400,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) joint_velocity_state_interface_)) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Expected %u velocity state interfaces, got %u", joint_names_.size(), joint_velocity_state_interface_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -425,8 +425,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) is_halted = false; subscriber_is_active_ = true; traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = lifecycle_node_->now(); - publisher_->on_activate(); + last_state_publish_time_ = node_->now(); // TODO(karsten1987): activate subscriptions of subscriber return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -441,7 +440,6 @@ JointTrajectoryController::on_deactivate(const rclcpp_lifecycle::State &) release_interfaces(); subscriber_is_active_ = false; - publisher_->on_deactivate(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -509,12 +507,12 @@ void JointTrajectoryController::publish_state( return; } - if (lifecycle_node_->now() < (last_state_publish_time_ + state_publisher_period_)) { + if (node_->now() < (last_state_publish_time_ + state_publisher_period_)) { return; } if (state_publisher_ && state_publisher_->trylock()) { - last_state_publish_time_ = lifecycle_node_->now(); + last_state_publish_time_ = node_->now(); state_publisher_->msg_.header.stamp = last_state_publish_time_; state_publisher_->msg_.desired.positions = desired_state.positions; state_publisher_->msg_.desired.velocities = desired_state.velocities; @@ -532,12 +530,12 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - RCLCPP_INFO(lifecycle_node_->get_logger(), "Received new action goal"); + RCLCPP_INFO(node_->get_logger(), "Received new action goal"); // Precondition: Running controller if (is_halted) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } @@ -546,14 +544,14 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( return rclcpp_action::GoalResponse::REJECT; } - RCLCPP_INFO(lifecycle_node_->get_logger(), "Accepted new action goal"); + RCLCPP_INFO(node_->get_logger(), "Accepted new action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( const std::shared_ptr> goal_handle) { - RCLCPP_INFO(lifecycle_node_->get_logger(), "Got request to cancel goal"); + RCLCPP_INFO(node_->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) { @@ -562,7 +560,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( set_hold_position(); RCLCPP_DEBUG( - lifecycle_node_->get_logger(), + node_->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled @@ -593,7 +591,7 @@ void JointTrajectoryController::feedback_setup_callback( } // Setup goal status checking timer - goal_handle_timer_ = lifecycle_node_->create_wall_timer( + goal_handle_timer_ = node_->create_wall_timer( action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); } @@ -651,7 +649,7 @@ void JointTrajectoryController::sort_to_local_joint_order( } if (to_remap.size() != mapping.size()) { RCLCPP_WARN( - lifecycle_node_->get_logger(), + node_->get_logger(), "Invalid input size (%d) for sorting", to_remap.size()); return to_remap; } @@ -689,7 +687,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( } if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Mismatch between joint_names (%u) and %s (%u) at point #%u.", joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; @@ -705,7 +703,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if (!allow_partial_joints_goal_) { if (trajectory.joint_names.size() != joint_names_.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); return false; } @@ -713,7 +711,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if (trajectory.joint_names.empty()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Empty joint names on incoming trajectory."); return false; } @@ -727,9 +725,9 @@ bool JointTrajectoryController::validate_trajectory_msg( for (const auto & p : trajectory.points) { trajectory_end_time += p.time_from_start; } - if (trajectory_end_time < lifecycle_node_->now()) { + if (trajectory_end_time < node_->now()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Received trajectory with non zero time start time (%f) that ends on the past (%f)", trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; @@ -742,7 +740,7 @@ bool JointTrajectoryController::validate_trajectory_msg( auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); if (it == joint_names_.end()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", incoming_joint_name.c_str()); return false; @@ -753,7 +751,7 @@ bool JointTrajectoryController::validate_trajectory_msg( for (auto i = 0ul; i < trajectory.points.size(); ++i) { if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "Time between points %u and %u is not strictly increasing, it is %f and %f respectively", i - 1, i, previous_traj_time.seconds(), rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 600d4df10e..3982547140 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 TestTrajectoryController SetUpAndActivateTrajectoryController(true, parameters); - executor_->add_node(traj_lifecycle_node_->get_node_base_interface()); + executor_->add_node(traj_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 9dd6931746..756733392b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -67,10 +67,10 @@ TEST_F(TestTrajectoryController, configuration) { SetUpTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_lifecycle_node()->get_node_base_interface()); + 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_->get_lifecycle_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -99,14 +99,14 @@ TEST_F(TestTrajectoryController, configuration) { // FAIL(); // } // -// auto traj_lifecycle_node = traj_controller->get_lifecycle_node(); +// auto traj_node = traj_controller->get_node(); // rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_lifecycle_node->get_node_base_interface()); +// executor.add_node(traj_node->get_node_base_interface()); // -// auto state = traj_lifecycle_node->configure(); +// auto state = traj_controller_->configure(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // -// state = traj_lifecycle_node->activate(); +// state = traj_node->activate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // // // wait for the subscriber and publisher to completely setup @@ -141,14 +141,14 @@ TEST_F(TestTrajectoryController, configuration) { // FAIL(); // } // -// auto traj_lifecycle_node = traj_controller->get_lifecycle_node(); +// auto traj_node = traj_controller->get_node(); // rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(traj_lifecycle_node->get_node_base_interface()); +// executor.add_node(traj_node->get_node_base_interface()); // -// auto state = traj_lifecycle_node->configure(); +// auto state = traj_controller_->configure(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // -// state = traj_lifecycle_node->activate(); +// state = traj_node->activate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // // // wait for the subscriber and publisher to completely setup @@ -176,7 +176,7 @@ TEST_F(TestTrajectoryController, configuration) { // // deactivated // // wait so controller process the second point when deactivated // std::this_thread::sleep_for(std::chrono::milliseconds(500)); -// state = traj_lifecycle_node->deactivate(); +// state = traj_controller_->deactivate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // resource_manager_->read(); // traj_controller->update(); @@ -190,7 +190,7 @@ TEST_F(TestTrajectoryController, configuration) { // // reactivated // // wait so controller process the third point when reactivated // std::this_thread::sleep_for(std::chrono::milliseconds(3000)); -// state = traj_lifecycle_node->activate(); +// state = traj_node->activate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // resource_manager_->read(); // traj_controller->update(); @@ -207,9 +207,9 @@ TEST_F(TestTrajectoryController, configuration) { TEST_F(TestTrajectoryController, cleanup) { SetUpAndActivateTrajectoryController(); - auto traj_lifecycle_node = traj_controller_->get_lifecycle_node(); + auto traj_node = traj_controller_->get_node(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_lifecycle_node->get_node_base_interface()); + executor.add_node(traj_node->get_node_base_interface()); // send msg builtin_interfaces::msg::Duration time_from_start; @@ -220,11 +220,11 @@ TEST_F(TestTrajectoryController, cleanup) { traj_controller_->wait_for_trajectory(executor); traj_controller_->update(); - auto state = traj_lifecycle_node->deactivate(); + auto state = traj_controller_->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); traj_controller_->update(); - state = traj_lifecycle_node->cleanup(); + state = traj_controller_->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // update for 0.25 seconds const auto start_time = rclcpp::Clock().now(); @@ -240,19 +240,19 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { SetUpTrajectoryController(false); // This block is replacing the way parameters are set via launch - auto traj_lifecycle_node = traj_controller_->get_lifecycle_node(); + auto traj_node = traj_controller_->get_node(); const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; const rclcpp::Parameter joint_parameters("joints", joint_names_); - traj_lifecycle_node->set_parameter(joint_parameters); - traj_lifecycle_node->configure(); - auto state = traj_lifecycle_node->get_current_state(); + traj_node->set_parameter(joint_parameters); + traj_controller_->configure(); + auto state = traj_controller_->get_current_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ActivateTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_lifecycle_node->get_node_base_interface()); + executor.add_node(traj_node->get_node_base_interface()); - state = traj_lifecycle_node->get_current_state(); + state = traj_controller_->get_current_state(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -278,7 +278,7 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { std::this_thread::sleep_for(FIRST_POINT_TIME); traj_controller_->update(); // deactivated - state = traj_lifecycle_node->deactivate(); + state = traj_controller_->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); const auto allowed_delta = 0.05; @@ -288,7 +288,7 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); // cleanup - state = traj_lifecycle_node->cleanup(); + state = traj_controller_->cleanup(); // update loop receives a new msg and updates accordingly traj_controller_->update(); @@ -302,7 +302,7 @@ TEST_F(TestTrajectoryController, correct_initialization_using_parameters) { EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], allowed_delta); EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], allowed_delta); - state = traj_lifecycle_node->configure(); + state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } @@ -355,7 +355,7 @@ void TestTrajectoryController::test_state_publish_rate_target(int target_msg_cou const int qos_level = 10; int echo_received_counter = 0; rclcpp::Subscription::SharedPtr subs = - traj_lifecycle_node_->create_subscription( + traj_node_->create_subscription( "/state", qos_level, [&](JointTrajectoryControllerState::UniquePtr) { @@ -616,7 +616,7 @@ TEST_F(TestTrajectoryController, test_trajectory_replace) { // Check that we reached end of points_old trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - RCLCPP_INFO(traj_lifecycle_node_->get_logger(), "Sending new trajectory"); + RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory"); publish(time_from_start, points_partial_new); // Replaced trajectory is a mix of previous and current goal expected_desired.positions[0] = points_partial_new[0][0]; @@ -646,7 +646,7 @@ TEST_F(TestTrajectoryController, 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_lifecycle_node_->get_logger(), "Sending new trajectory in the past"); + RCLCPP_INFO(traj_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()}; @@ -672,7 +672,7 @@ TEST_F(TestTrajectoryController, 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_lifecycle_node_->get_logger(), "Sending new trajectory partially in the past"); + RCLCPP_INFO(traj_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()}; @@ -684,20 +684,20 @@ TEST_F(TestTrajectoryController, test_ignore_partial_old_trajectory) { TEST_F(TestTrajectoryController, test_execute_partial_traj_in_future) { SetUpTrajectoryController(); - auto traj_lifecycle_node = traj_controller_->get_lifecycle_node(); + auto traj_node = traj_controller_->get_node(); RCLCPP_WARN( - traj_lifecycle_node->get_logger(), + traj_node->get_logger(), "Test disabled until current_trajectory is taken into account when adding a new trajectory."); // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 return; rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(traj_lifecycle_node->get_node_base_interface()); + executor.add_node(traj_node->get_node_base_interface()); subscribeToState(); rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - traj_lifecycle_node->set_parameter(partial_joints_parameters); - traj_lifecycle_node->configure(); - traj_lifecycle_node->activate(); + traj_node->set_parameter(partial_joints_parameters); + traj_controller_->configure(); + traj_controller_->activate(); std::vector> full_traj {{{2., 3., 4.}, {4., 6., 8.}}}; std::vector> partial_traj {{{-1., -2.}, {-2., -4, }}}; @@ -715,7 +715,7 @@ TEST_F(TestTrajectoryController, test_execute_partial_traj_in_future) { // Send partial trajectory starting after full trajecotry is complete - RCLCPP_INFO(traj_lifecycle_node->get_logger(), "Sending new trajectory in the future"); + RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); publish(points_delay, partial_traj, rclcpp::Clock().now() + delay * 2); // Wait until the end start and end of partial traj diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 264a7d0d83..ed89aa267a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -115,19 +115,19 @@ class TestTrajectoryController : public ::testing::Test { SetUpTrajectoryController(use_local_parameters); - traj_lifecycle_node_ = traj_controller_->get_lifecycle_node(); + traj_node_ = traj_controller_->get_node(); for (const auto & param : parameters) { - traj_lifecycle_node_->set_parameter(param); + traj_node_->set_parameter(param); } if (executor) { - executor->add_node(traj_lifecycle_node_->get_node_base_interface()); + executor->add_node(traj_node_->get_node_base_interface()); } // ignore velocity tolerances for this test since they arent commited in test_robot->write() rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); - traj_lifecycle_node_->set_parameter(stopped_velocity_parameters); + traj_node_->set_parameter(stopped_velocity_parameters); - traj_controller_->get_lifecycle_node()->configure(); + traj_controller_->configure(); ActivateTrajectoryController(); } @@ -165,7 +165,7 @@ class TestTrajectoryController : public ::testing::Test cmd_interfaces[2].set_value(INITIAL_POS_JOINT3); traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_lifecycle_node()->activate(); + traj_controller_->activate(); } static void TearDownTestCase() @@ -175,7 +175,7 @@ class TestTrajectoryController : public ::testing::Test void subscribeToState() { - auto traj_lifecycle_node = traj_controller_->get_lifecycle_node(); + auto traj_lifecycle_node = traj_controller_->get_node(); traj_lifecycle_node->set_parameter( rclcpp::Parameter( "state_publish_rate", @@ -300,7 +300,7 @@ class TestTrajectoryController : public ::testing::Test rclcpp::Publisher::SharedPtr trajectory_publisher_; std::shared_ptr traj_controller_; - std::shared_ptr traj_lifecycle_node_; + std::shared_ptr traj_node_; rclcpp::Subscription::SharedPtr state_subscriber_; mutable std::mutex state_mutex_;