From 2299a33e96515bb2f5d426ce53a8e3e23805a3dc Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:03:32 +0300 Subject: [PATCH 01/10] Add lifecycle nodes --- .../src/diff_drive_controller.cpp | 110 +++++++++--------- .../test/test_diff_drive_controller.cpp | 62 +++++----- .../src/joint_group_effort_controller.cpp | 2 +- .../test_joint_group_effort_controller.cpp | 22 ++-- .../test_force_torque_sensor_broadcaster.cpp | 66 +++++------ .../src/forward_command_controller.cpp | 10 +- .../test/test_forward_command_controller.cpp | 52 ++++----- .../test/test_imu_sensor_broadcaster.cpp | 22 ++-- .../src/joint_state_broadcaster.cpp | 16 +-- .../test/test_joint_state_broadcaster.cpp | 6 +- .../src/joint_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller.cpp | 48 ++++---- .../test/test_trajectory_controller_utils.hpp | 10 +- .../src/joint_group_position_controller.cpp | 2 +- .../test_joint_group_position_controller.cpp | 20 ++-- .../src/joint_group_velocity_controller.cpp | 2 +- .../test_joint_group_velocity_controller.cpp | 22 ++-- 17 files changed, 238 insertions(+), 238 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e816b26f11..290968daae 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -135,8 +135,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons controller_interface::return_type DiffDriveController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto logger = node_->get_logger(); - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + auto logger = lifecycle_node_->get_logger(); + if (get_lifecycle_node()->get_current_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { @@ -278,11 +278,11 @@ controller_interface::return_type DiffDriveController::update( CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &) { - auto logger = node_->get_logger(); + auto logger = lifecycle_node_->get_logger(); // update parameters - left_wheel_names_ = node_->get_parameter("left_wheel_names").as_string_array(); - right_wheel_names_ = node_->get_parameter("right_wheel_names").as_string_array(); + left_wheel_names_ = lifecycle_node_->get_parameter("left_wheel_names").as_string_array(); + right_wheel_names_ = lifecycle_node_->get_parameter("right_wheel_names").as_string_array(); if (left_wheel_names_.size() != right_wheel_names_.size()) { @@ -298,16 +298,16 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - wheel_params_.separation = node_->get_parameter("wheel_separation").as_double(); + wheel_params_.separation = lifecycle_node_->get_parameter("wheel_separation").as_double(); wheel_params_.wheels_per_side = - static_cast(node_->get_parameter("wheels_per_side").as_int()); - wheel_params_.radius = node_->get_parameter("wheel_radius").as_double(); + static_cast(lifecycle_node_->get_parameter("wheels_per_side").as_int()); + wheel_params_.radius = lifecycle_node_->get_parameter("wheel_radius").as_double(); wheel_params_.separation_multiplier = - node_->get_parameter("wheel_separation_multiplier").as_double(); + lifecycle_node_->get_parameter("wheel_separation_multiplier").as_double(); wheel_params_.left_radius_multiplier = - node_->get_parameter("left_wheel_radius_multiplier").as_double(); + lifecycle_node_->get_parameter("left_wheel_radius_multiplier").as_double(); wheel_params_.right_radius_multiplier = - node_->get_parameter("right_wheel_radius_multiplier").as_double(); + lifecycle_node_->get_parameter("right_wheel_radius_multiplier").as_double(); const auto wheels = wheel_params_; @@ -317,61 +317,61 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize( - node_->get_parameter("velocity_rolling_window_size").as_int()); + lifecycle_node_->get_parameter("velocity_rolling_window_size").as_int()); - odom_params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = node_->get_parameter("base_frame_id").as_string(); + odom_params_.odom_frame_id = lifecycle_node_->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = lifecycle_node_->get_parameter("base_frame_id").as_string(); - auto pose_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array(); + auto pose_diagonal = lifecycle_node_->get_parameter("pose_covariance_diagonal").as_double_array(); std::copy( pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - auto twist_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array(); + auto twist_diagonal = lifecycle_node_->get_parameter("twist_covariance_diagonal").as_double_array(); std::copy( twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); - odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); + odom_params_.open_loop = lifecycle_node_->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = lifecycle_node_->get_parameter("enable_odom_tf").as_bool(); cmd_vel_timeout_ = std::chrono::milliseconds{ - static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = node_->get_parameter("publish_limited_velocity").as_bool(); - use_stamped_vel_ = node_->get_parameter("use_stamped_vel").as_bool(); + static_cast(lifecycle_node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; + publish_limited_velocity_ = lifecycle_node_->get_parameter("publish_limited_velocity").as_bool(); + use_stamped_vel_ = lifecycle_node_->get_parameter("use_stamped_vel").as_bool(); try { limiter_linear_ = SpeedLimiter( - node_->get_parameter("linear.x.has_velocity_limits").as_bool(), - node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), - node_->get_parameter("linear.x.has_jerk_limits").as_bool(), - node_->get_parameter("linear.x.min_velocity").as_double(), - node_->get_parameter("linear.x.max_velocity").as_double(), - node_->get_parameter("linear.x.min_acceleration").as_double(), - node_->get_parameter("linear.x.max_acceleration").as_double(), - node_->get_parameter("linear.x.min_jerk").as_double(), - node_->get_parameter("linear.x.max_jerk").as_double()); + lifecycle_node_->get_parameter("linear.x.has_velocity_limits").as_bool(), + lifecycle_node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), + lifecycle_node_->get_parameter("linear.x.has_jerk_limits").as_bool(), + lifecycle_node_->get_parameter("linear.x.min_velocity").as_double(), + lifecycle_node_->get_parameter("linear.x.max_velocity").as_double(), + lifecycle_node_->get_parameter("linear.x.min_acceleration").as_double(), + lifecycle_node_->get_parameter("linear.x.max_acceleration").as_double(), + lifecycle_node_->get_parameter("linear.x.min_jerk").as_double(), + lifecycle_node_->get_parameter("linear.x.max_jerk").as_double()); } catch (const std::runtime_error & e) { - RCLCPP_ERROR(node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); } try { limiter_angular_ = SpeedLimiter( - node_->get_parameter("angular.z.has_velocity_limits").as_bool(), - node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), - node_->get_parameter("angular.z.has_jerk_limits").as_bool(), - node_->get_parameter("angular.z.min_velocity").as_double(), - node_->get_parameter("angular.z.max_velocity").as_double(), - node_->get_parameter("angular.z.min_acceleration").as_double(), - node_->get_parameter("angular.z.max_acceleration").as_double(), - node_->get_parameter("angular.z.min_jerk").as_double(), - node_->get_parameter("angular.z.max_jerk").as_double()); + lifecycle_node_->get_parameter("angular.z.has_velocity_limits").as_bool(), + lifecycle_node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), + lifecycle_node_->get_parameter("angular.z.has_jerk_limits").as_bool(), + lifecycle_node_->get_parameter("angular.z.min_velocity").as_double(), + lifecycle_node_->get_parameter("angular.z.max_velocity").as_double(), + lifecycle_node_->get_parameter("angular.z.min_acceleration").as_double(), + lifecycle_node_->get_parameter("angular.z.max_acceleration").as_double(), + lifecycle_node_->get_parameter("angular.z.min_jerk").as_double(), + lifecycle_node_->get_parameter("angular.z.max_jerk").as_double()); } catch (const std::runtime_error & e) { - RCLCPP_ERROR(node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } if (!reset()) @@ -385,7 +385,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & if (publish_limited_velocity_) { limited_velocity_publisher_ = - node_->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + lifecycle_node_->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = std::make_shared>(limited_velocity_publisher_); } @@ -400,33 +400,33 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & // initialize command subscriber if (use_stamped_vel_) { - velocity_command_subscriber_ = node_->create_subscription( + velocity_command_subscriber_ = lifecycle_node_->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { - RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + RCLCPP_WARN(lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { RCLCPP_WARN_ONCE( - node_->get_logger(), + lifecycle_node_->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " "time, this message will only be shown once"); - msg->header.stamp = node_->get_clock()->now(); + msg->header.stamp = lifecycle_node_->get_clock()->now(); } received_velocity_msg_ptr_.set(std::move(msg)); }); } else { - velocity_command_unstamped_subscriber_ = node_->create_subscription( + velocity_command_unstamped_subscriber_ = lifecycle_node_->create_subscription( DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { - RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + RCLCPP_WARN(lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } @@ -434,12 +434,12 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & std::shared_ptr twist_stamped; received_velocity_msg_ptr_.get(twist_stamped); twist_stamped->twist = *msg; - twist_stamped->header.stamp = node_->get_clock()->now(); + twist_stamped->header.stamp = lifecycle_node_->get_clock()->now(); }); } // initialize odometry publisher and messasge - odometry_publisher_ = node_->create_publisher( + odometry_publisher_ = lifecycle_node_->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = std::make_shared>( @@ -464,7 +464,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } // initialize transform publisher and message - odometry_transform_publisher_ = node_->create_publisher( + odometry_transform_publisher_ = lifecycle_node_->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_transform_publisher_ = std::make_shared>( @@ -476,7 +476,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_ = node_->get_clock()->now(); + previous_update_timestamp_ = lifecycle_node_->get_clock()->now(); return CallbackReturn::SUCCESS; } @@ -495,14 +495,14 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { RCLCPP_ERROR( - node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); + lifecycle_node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); return CallbackReturn::ERROR; } is_halted = false; subscriber_is_active_ = true; - RCLCPP_DEBUG(node_->get_logger(), "Subscriber and publisher are now active."); + RCLCPP_DEBUG(lifecycle_node_->get_logger(), "Subscriber and publisher are now active."); return CallbackReturn::SUCCESS; } @@ -574,7 +574,7 @@ CallbackReturn DiffDriveController::configure_side( const std::string & side, const std::vector & wheel_names, std::vector & registered_handles) { - auto logger = node_->get_logger(); + auto logger = lifecycle_node_->get_logger(); if (wheel_names.empty()) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ca3da08803..1d3c47b213 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -176,16 +176,16 @@ TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_sid const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -196,12 +196,12 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); auto extended_right_wheel_names = right_wheel_names; extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -212,9 +212,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -232,9 +232,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -246,9 +246,9 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_resources_assigned) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -261,20 +261,20 @@ TEST_F(TestDiffDriveController, cleanup) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); + controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); + executor.add_node(controller_->get_lifecycle_node()->get_node_base_interface()); + auto state = controller_->get_lifecycle_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); assignResources(); - state = controller_->activate(); + state = controller_->get_lifecycle_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); waitForSetup(); @@ -289,13 +289,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_lifecycle_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_lifecycle_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped @@ -310,24 +310,24 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( + controller_->get_lifecycle_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); + controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(controller_->get_lifecycle_node()->get_node_base_interface()); - auto state = controller_->configure(); + auto state = controller_->get_lifecycle_node()->configure(); assignResources(); 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_lifecycle_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); // send msg @@ -346,7 +346,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_lifecycle_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)), @@ -356,12 +356,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_lifecycle_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_lifecycle_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index e607e3c27f..7c49a017db 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -41,7 +41,7 @@ CallbackReturn JointGroupEffortController::on_init() { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupEffortController constructor. - get_node()->set_parameter( + get_lifecycle_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT)); } catch (const std::exception & e) diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 157384423a..d0a2dc0b29 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -75,7 +75,7 @@ TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector()}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -84,7 +84,7 @@ TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty) TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -94,13 +94,13 @@ TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -110,7 +110,7 @@ TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails) TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -142,7 +142,7 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints @@ -164,7 +164,7 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -181,7 +181,7 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // default values ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -197,7 +197,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_lifecycle_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); @@ -206,7 +206,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface()); // update successful ASSERT_EQ( @@ -222,7 +222,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index db8f6e7022..4049ad2338 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -113,8 +113,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -125,7 +125,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -136,11 +136,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -151,7 +151,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe SetUpFTSBroadcaster(); // set the 'sensor_name' empty - fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", ""}); // configure failed, 'sensor_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -162,8 +162,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe SetUpFTSBroadcaster(); // set the 'interface_names' empty - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", ""}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", ""}); // configure failed, 'interface_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -174,10 +174,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -188,11 +188,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // set the 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -203,8 +203,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -216,8 +216,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -232,9 +232,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -248,8 +248,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -271,9 +271,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -295,13 +295,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 49284ae5cb..afab03c252 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -59,7 +59,7 @@ CallbackReturn ForwardCommandController::on_configure( if (joint_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + RCLCPP_ERROR(get_lifecycle_node()->get_logger(), "'joints' parameter was empty"); return CallbackReturn::ERROR; } @@ -71,15 +71,15 @@ CallbackReturn ForwardCommandController::on_configure( if (interface_name_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + RCLCPP_ERROR(get_lifecycle_node()->get_logger(), "'interface_name' parameter was empty"); return CallbackReturn::ERROR; } - joints_command_subscriber_ = get_node()->create_subscription( + joints_command_subscriber_ = get_lifecycle_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + RCLCPP_INFO(get_lifecycle_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -167,7 +167,7 @@ controller_interface::return_type ForwardCommandController::update( if ((*joint_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *node_->get_clock(), 1000, + get_lifecycle_node()->get_logger(), *node_->get_clock(), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 425c08c7af..6ab7836ac6 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -75,7 +75,7 @@ void ForwardCommandControllerTest::SetUpController() TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) { SetUpController(); - controller_->get_node()->set_parameter({"interface_name", "dummy"}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "dummy"}); // configure failed, 'joints' parameter not set ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -86,17 +86,17 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet) SetUpController(); // configure failed, 'interface_name' parameter not set - controller_->get_node()->set_parameter({"joints", std::vector()}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - controller_->get_node()->set_parameter({"interface_name", ""}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); } TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector()}); - controller_->get_node()->set_parameter({"interface_name", ""}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -107,8 +107,8 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) SetUpController(); // configure failed, 'interface_name' parameter not set - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); - controller_->get_node()->set_parameter({"interface_name", ""}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); // configure failed, 'interface_name' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -118,8 +118,8 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -129,14 +129,14 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_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_->get_node()->set_parameter( + auto result = controller_->get_lifecycle_node()->set_parameter( {"joints", std::vector{"joint1", "joint2"}}); ASSERT_TRUE(result.successful); @@ -149,8 +149,8 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"interface_name", "acceleration"}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "acceleration"}); // activate failed, 'joint4' not in interfaces ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -161,8 +161,8 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -173,8 +173,8 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) SetUpController(); // configure controller - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -208,8 +208,8 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) SetUpController(); // configure controller - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -234,8 +234,8 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) SetUpController(); // configure controller - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -253,8 +253,8 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"interface_name", "position"}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); // default values ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -270,7 +270,7 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_lifecycle_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); @@ -279,7 +279,7 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface()); // update successful ASSERT_EQ( diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index d27be76b3b..3eff71b990 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -115,9 +115,9 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) SetUpIMUBroadcaster(); // set the 'interface_names' - imu_broadcaster_->get_node()->set_parameter( + imu_broadcaster_->get_lifecycle_node()->set_parameter( {"interface_names.angular_velocity.x", "imu_sensor/angular_velocity.x"}); - imu_broadcaster_->get_node()->set_parameter( + imu_broadcaster_->get_lifecycle_node()->set_parameter( {"interface_names.linear_acceleration.z", "imu_sensor/linear_acceleration.z"}); // configure failed, 'frame_id' parameter not set @@ -129,7 +129,7 @@ TEST_F(IMUSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) SetUpIMUBroadcaster(); // set the 'sensor_name' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -140,10 +140,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) SetUpIMUBroadcaster(); // set the 'sensor_name' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -154,8 +154,8 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) SetUpIMUBroadcaster(); // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -167,8 +167,8 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) SetUpIMUBroadcaster(); // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -183,8 +183,8 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) SetUpIMUBroadcaster(); // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c5dbcc4c82..3fa121533c 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -78,22 +78,22 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); + use_local_topics_ = get_lifecycle_node()->get_parameter("use_local_topics").as_bool(); try { const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; - joint_state_publisher_ = get_node()->create_publisher( + joint_state_publisher_ = get_lifecycle_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); dynamic_joint_state_publisher_ = - get_node()->create_publisher( + get_lifecycle_node()->create_publisher( topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); } catch (const std::exception & e) { - // get_node() may throw, logging raw here + // get_lifecycle_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -165,7 +165,7 @@ bool JointStateBroadcaster::init_joint_data() // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there rclcpp::Parameter extra_joints; - if (get_node()->get_parameter("extra_joints", extra_joints)) + if (get_lifecycle_node()->get_parameter("extra_joints", extra_joints)) { const std::vector & extra_joints_names = extra_joints.as_string_array(); for (const auto & extra_joint_name : extra_joints_names) @@ -237,12 +237,12 @@ controller_interface::return_type JointStateBroadcaster::update( name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] = state_interface.get_value(); RCLCPP_DEBUG( - get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(), + get_lifecycle_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(), state_interface.get_interface_name().c_str(), state_interface.get_value()); } - joint_state_msg_.header.stamp = time; - dynamic_joint_state_msg_.header.stamp = time; + joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); + dynamic_joint_state_msg_.header.stamp = get_lifecycle_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_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index f38c108589..8531935a50 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -235,7 +235,7 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest) TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic) { SetUpStateBroadcaster(); - state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); + state_broadcaster_->get_lifecycle_node()->set_parameter({"use_local_topics", true}); test_published_joint_state_message("joint_state_broadcaster/joint_states"); } @@ -298,7 +298,7 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest) TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic) { SetUpStateBroadcaster(); - state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); + state_broadcaster_->get_lifecycle_node()->set_parameter({"use_local_topics", true}); test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states"); } @@ -323,7 +323,7 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) // Add extra joints as parameters const std::vector extra_joint_names = {"extra1", "extra2", "extra3"}; - state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names}); + state_broadcaster_->get_lifecycle_node()->set_parameter({"extra_joints", extra_joint_names}); // configure ok ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 843ce78459..54cade9bd5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -108,7 +108,7 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } @@ -895,7 +895,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( RCLCPP_INFO(node_->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0a992b14f7..02b5f8e23f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -65,10 +65,10 @@ TEST_P(TrajectoryControllerTestParameterized, configure) SetUpTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + executor.add_node(traj_controller_->get_lifecycle_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_lifecycle_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -94,10 +94,10 @@ TEST_P(TrajectoryControllerTestParameterized, activate) SetUpTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + executor.add_node(traj_controller_->get_lifecycle_node()->get_node_base_interface()); - traj_controller_->configure(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + traj_controller_->get_lifecycle_node()->configure(); + ASSERT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -121,7 +121,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // FAIL(); // } // -// auto traj_node = traj_controller->get_node(); +// auto traj_node = traj_controller->get_lifecycle_node(); // rclcpp::executors::MultiThreadedExecutor executor; // executor.add_node(traj_node->get_node_base_interface()); // @@ -163,7 +163,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // FAIL(); // } // -// auto traj_node = traj_controller->get_node(); +// auto traj_node = traj_controller->get_lifecycle_node(); // rclcpp::executors::MultiThreadedExecutor executor; // executor.add_node(traj_node->get_node_base_interface()); // @@ -230,7 +230,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) { SetUpAndActivateTrajectoryController(); - auto traj_node = traj_controller_->get_node(); + auto traj_node = traj_controller_->get_lifecycle_node(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(traj_node->get_node_base_interface()); @@ -243,11 +243,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_lifecycle_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_lifecycle_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // update for 0.25 seconds const auto start_time = rclcpp::Clock().now(); @@ -267,15 +267,15 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); - traj_controller_->configure(); - auto state = traj_controller_->get_state(); + traj_controller_->get_lifecycle_node()->configure(); + auto state = traj_controller_->get_lifecycle_node()->get_current_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ActivateTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + executor.add_node(traj_controller_->get_lifecycle_node()->get_node_base_interface()); - state = traj_controller_->get_state(); + state = traj_controller_->get_lifecycle_node()->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]); @@ -298,7 +298,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_lifecycle_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? @@ -310,7 +310,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); // cleanup - state = traj_controller_->cleanup(); + state = traj_controller_->get_lifecycle_node()->cleanup(); // update loop receives a new msg and updates accordingly traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -324,7 +324,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_lifecycle_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); @@ -747,7 +747,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) { SetUpTrajectoryController(); - auto traj_node = traj_controller_->get_node(); + auto traj_node = traj_controller_->get_lifecycle_node(); RCLCPP_WARN( traj_node->get_logger(), "Test disabled until current_trajectory is taken into account when adding a new trajectory."); @@ -760,8 +760,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_lifecycle_node()->configure(); + traj_controller_->get_lifecycle_node()->activate(); std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; std::vector> partial_traj{ @@ -1107,10 +1107,10 @@ INSTANTIATE_TEST_CASE_P( TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { - EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); SetParameters(); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); - EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + traj_controller_->get_lifecycle_node()->configure(); + EXPECT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); }; SetUpTrajectoryController(false); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8ece5c393a..83087308b1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -140,7 +140,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetParameters() { - auto node = traj_controller_->get_node(); + auto node = traj_controller_->get_lifecycle_node(); const rclcpp::Parameter joint_names_param("joints", joint_names_); const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_); const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); @@ -153,7 +153,7 @@ class TrajectoryControllerTest : public ::testing::Test { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_node(); + traj_node_ = traj_controller_->get_lifecycle_node(); for (const auto & param : parameters) { traj_node_->set_parameter(param); @@ -167,7 +167,7 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); traj_node_->set_parameter(stopped_velocity_parameters); - traj_controller_->configure(); + traj_controller_->get_lifecycle_node()->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); } @@ -216,14 +216,14 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->activate(); + traj_controller_->get_lifecycle_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } void subscribeToState() { - auto traj_lifecycle_node = traj_controller_->get_node(); + auto traj_lifecycle_node = traj_controller_->get_lifecycle_node(); traj_lifecycle_node->set_parameter( rclcpp::Parameter("state_publish_rate", static_cast(100))); diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index e52f9425fd..acdf35b333 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -41,7 +41,7 @@ CallbackReturn JointGroupPositionController::on_init() { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupPositionController constructor. - get_node()->set_parameter( + get_lifecycle_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_POSITION)); } catch (const std::exception & e) diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index be774031f0..d635b1e906 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -75,7 +75,7 @@ TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) TEST_F(JointGroupPositionControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector()}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -84,7 +84,7 @@ TEST_F(JointGroupPositionControllerTest, JointsParameterIsEmpty) TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -94,13 +94,13 @@ TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -110,7 +110,7 @@ TEST_F(JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails) TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -142,7 +142,7 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints @@ -164,7 +164,7 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -181,7 +181,7 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // default values ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -197,7 +197,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_lifecycle_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); @@ -206,7 +206,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface()); // update successful ASSERT_EQ( diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index c2c7d762bd..238119f994 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -41,7 +41,7 @@ CallbackReturn JointGroupVelocityController::on_init() { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupVelocityController constructor. - get_node()->set_parameter( + get_lifecycle_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_VELOCITY)); } catch (const std::exception & e) diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index b566829f9b..0686119e4a 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -75,7 +75,7 @@ TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) TEST_F(JointGroupVelocityControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector()}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -84,7 +84,7 @@ TEST_F(JointGroupVelocityControllerTest, JointsParameterIsEmpty) TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -94,13 +94,13 @@ TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -110,7 +110,7 @@ TEST_F(JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails) TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -142,7 +142,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints @@ -164,7 +164,7 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -181,7 +181,7 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // default values ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -197,7 +197,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_lifecycle_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); @@ -206,7 +206,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface()); // update successful ASSERT_EQ( @@ -222,7 +222,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) { SetUpController(); - controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); From 484ad0fe89e77781d784a4bdfcdfdfda11a51edd Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:18:49 +0300 Subject: [PATCH 02/10] Fix compile issues --- .../test_joint_group_effort_controller.cpp | 4 +- .../src/force_torque_sensor_broadcaster.cpp | 26 +++--- .../src/forward_command_controller.cpp | 8 +- .../test/test_forward_command_controller.cpp | 4 +- .../src/imu_sensor_broadcaster.cpp | 14 +-- .../test/test_joint_state_broadcaster.cpp | 12 +-- .../tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 86 +++++++++---------- .../test_joint_group_position_controller.cpp | 4 +- .../test_joint_group_velocity_controller.cpp | 4 +- 10 files changed, 82 insertions(+), 82 deletions(-) diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index d0a2dc0b29..bb34e4e131 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_lifecycle_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_lifecycle_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 5f5c880c39..eaec018769 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -53,13 +53,13 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init() CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); - interface_names_[0] = node_->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] = node_->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] = node_->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] = node_->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] = node_->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] = node_->get_parameter("interface_names.torque.z").as_string(); + sensor_name_ = lifecycle_node_->get_parameter("sensor_name").as_string(); + interface_names_[0] = lifecycle_node_->get_parameter("interface_names.force.x").as_string(); + interface_names_[1] = lifecycle_node_->get_parameter("interface_names.force.y").as_string(); + interface_names_[2] = lifecycle_node_->get_parameter("interface_names.force.z").as_string(); + interface_names_[3] = lifecycle_node_->get_parameter("interface_names.torque.x").as_string(); + interface_names_[4] = lifecycle_node_->get_parameter("interface_names.torque.y").as_string(); + interface_names_[5] = lifecycle_node_->get_parameter("interface_names.torque.z").as_string(); const bool no_interface_names_defined = std::count(interface_names_.begin(), interface_names_.end(), "") == 6; @@ -67,7 +67,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (sensor_name_.empty() && no_interface_names_defined) { RCLCPP_ERROR( - node_->get_logger(), + lifecycle_node_->get_logger(), "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be specified."); return CallbackReturn::ERROR; @@ -76,16 +76,16 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (!sensor_name_.empty() && !no_interface_names_defined) { RCLCPP_ERROR( - node_->get_logger(), + lifecycle_node_->get_logger(), "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together."); return CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = lifecycle_node_->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } @@ -105,7 +105,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( try { // register ft sensor data publisher - sensor_state_publisher_ = node_->create_publisher( + sensor_state_publisher_ = lifecycle_node_->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } @@ -121,7 +121,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(lifecycle_node_->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index afab03c252..eed17c91d6 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -55,7 +55,7 @@ CallbackReturn ForwardCommandController::on_init() CallbackReturn ForwardCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - joint_names_ = node_->get_parameter("joints").as_string_array(); + joint_names_ = lifecycle_node_->get_parameter("joints").as_string_array(); if (joint_names_.empty()) { @@ -66,7 +66,7 @@ CallbackReturn ForwardCommandController::on_configure( // Specialized, child controllers set interfaces before calling configure function. if (interface_name_.empty()) { - interface_name_ = node_->get_parameter("interface_name").as_string(); + interface_name_ = lifecycle_node_->get_parameter("interface_name").as_string(); } if (interface_name_.empty()) @@ -139,7 +139,7 @@ CallbackReturn ForwardCommandController::on_activate( command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), + lifecycle_node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), ordered_interfaces.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -167,7 +167,7 @@ controller_interface::return_type ForwardCommandController::update( if ((*joint_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( - get_lifecycle_node()->get_logger(), *node_->get_clock(), 1000, + get_lifecycle_node()->get_logger(), *lifecycle_node_->get_clock(), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 6ab7836ac6..d0e01c186d 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -261,10 +261,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_lifecycle_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_lifecycle_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 23aa33777c..12f832c582 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -33,7 +33,7 @@ CallbackReturn IMUSensorBroadcaster::on_init() catch (const std::exception & e) { RCLCPP_ERROR( - node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + lifecycle_node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -43,17 +43,17 @@ CallbackReturn IMUSensorBroadcaster::on_init() CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); + sensor_name_ = lifecycle_node_->get_parameter("sensor_name").as_string(); if (sensor_name_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'sensor_name' parameter has to be specified."); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = lifecycle_node_->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } @@ -63,7 +63,7 @@ CallbackReturn IMUSensorBroadcaster::on_configure( { // register ft sensor data publisher sensor_state_publisher_ = - node_->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); + lifecycle_node_->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } catch (const std::exception & e) @@ -78,7 +78,7 @@ CallbackReturn IMUSensorBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(lifecycle_node_->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 8531935a50..97fda27fdd 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -181,8 +181,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) { SetUpStateBroadcaster(); - auto node_state = state_broadcaster_->configure(); - node_state = state_broadcaster_->activate(); + auto node_state = state_broadcaster_->get_lifecycle_node()->configure(); + node_state = state_broadcaster_->get_lifecycle_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)), @@ -191,10 +191,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_lifecycle_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = state_broadcaster_->activate(); + node_state = state_broadcaster_->get_lifecycle_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); @@ -243,10 +243,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_lifecycle_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = state_broadcaster_->activate(); + node_state = state_broadcaster_->get_lifecycle_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 2d247eeb8b..73e9b11667 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/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 54cade9bd5..92e8f5a3b2 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -167,11 +167,11 @@ controller_interface::return_type JointTrajectoryController::update( if (open_loop_control_) { (*traj_point_active_ptr_) - ->set_point_before_trajectory_msg(node_->now(), last_commanded_state_); + ->set_point_before_trajectory_msg(lifecycle_node_->now(), last_commanded_state_); } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(node_->now(), state_current); + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(lifecycle_node_->now(), state_current); } } resize_joint_trajectory_point(state_error, joint_num); @@ -181,7 +181,7 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(anyone): this is kind-of open-loop concept? I am right? const bool valid_point = (*traj_point_active_ptr_) - ->sample(node_->now(), state_desired, start_segment_itr, end_segment_itr); + ->sample(lifecycle_node_->now(), state_desired, start_segment_itr, end_segment_itr); if (valid_point) { @@ -237,7 +237,7 @@ controller_interface::return_type JointTrajectoryController::update( { // send feedback auto feedback = std::make_shared(); - feedback->header.stamp = node_->now(); + feedback->header.stamp = lifecycle_node_->now(); feedback->joint_names = joint_names_; feedback->actual = state_current; @@ -252,12 +252,12 @@ controller_interface::return_type JointTrajectoryController::update( if (abort) { - RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(lifecycle_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(node_->get_logger(), "Aborted due to goal tolerance violation"); + RCLCPP_WARN(lifecycle_node_->get_logger(), "Aborted due to goal tolerance violation"); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); } active_goal->setAborted(result); @@ -278,7 +278,7 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_INFO(node_->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(lifecycle_node_->get_logger(), "Goal reached, success!"); } else if (default_tolerances_.goal_time_tolerance != 0.0) { @@ -286,7 +286,7 @@ controller_interface::return_type JointTrajectoryController::update( 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 = node_->now().seconds() - traj_end.seconds(); + const double difference = lifecycle_node_->now().seconds() - traj_end.seconds(); if (difference > default_tolerances_.goal_time_tolerance) { auto result = std::make_shared(); @@ -296,7 +296,7 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_WARN( - node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + lifecycle_node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", difference); } } @@ -416,10 +416,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) { - const auto logger = node_->get_logger(); + const auto logger = lifecycle_node_->get_logger(); // update parameters - joint_names_ = node_->get_parameter("joints").as_string_array(); + joint_names_ = lifecycle_node_->get_parameter("joints").as_string_array(); if (!reset()) { @@ -434,7 +434,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { - command_interface_types_ = node_->get_parameter("command_interfaces").as_string_array(); + command_interface_types_ = lifecycle_node_->get_parameter("command_interfaces").as_string_array(); } if (command_interface_types_.empty()) @@ -528,7 +528,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // Read always state interfaces from the parameter because they can be used // independently from the controller's type. // Specialized, child controllers should set its default value. - state_interface_types_ = node_->get_parameter("state_interfaces").as_string_array(); + state_interface_types_ = lifecycle_node_->get_parameter("state_interfaces").as_string_array(); if (state_interface_types_.empty()) { @@ -605,10 +605,10 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*node_, joint_names_); + default_tolerances_ = get_segment_tolerances(*lifecycle_node_, joint_names_); // Read parameters customizing controller for special cases - open_loop_control_ = node_->get_parameter("open_loop_control").get_value(); + open_loop_control_ = lifecycle_node_->get_parameter("open_loop_control").get_value(); // subscriber callback // non realtime @@ -628,14 +628,14 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S }; // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = node_->create_subscription( + joint_command_subscriber_ = lifecycle_node_->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); // TODO(karsten1987): no lifecycle for subscriber yet // joint_command_subscriber_->on_activate(); // State publisher - const double state_publish_rate = node_->get_parameter("state_publish_rate").get_value(); + const double state_publish_rate = lifecycle_node_->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); if (state_publish_rate > 0.0) { @@ -646,7 +646,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = lifecycle_node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); const auto n_joints = joint_names_.size(); @@ -670,26 +670,26 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } state_publisher_->unlock(); - last_state_publish_time_ = node_->now(); + last_state_publish_time_ = lifecycle_node_->now(); // action server configuration - allow_partial_joints_goal_ = node_->get_parameter("allow_partial_joints_goal").get_value(); + allow_partial_joints_goal_ = lifecycle_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 = - node_->get_parameter("action_monitor_rate").get_value(); + lifecycle_node_->get_parameter("action_monitor_rate").get_value(); RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); using namespace std::placeholders; action_server_ = rclcpp_action::create_server( - 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", + 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", std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), std::bind(&JointTrajectoryController::cancel_callback, this, _1), std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); @@ -731,7 +731,7 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), + lifecycle_node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } @@ -745,7 +745,7 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), + lifecycle_node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } @@ -772,7 +772,7 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St subscriber_is_active_ = true; traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = node_->now(); + last_state_publish_time_ = lifecycle_node_->now(); // Initialize current state storage if hardware state has tracking offset resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); @@ -860,14 +860,14 @@ void JointTrajectoryController::publish_state( return; } - if (node_->now() < (last_state_publish_time_ + state_publisher_period_)) + if (lifecycle_node_->now() < (last_state_publish_time_ + state_publisher_period_)) { return; } if (state_publisher_ && state_publisher_->trylock()) { - last_state_publish_time_ = node_->now(); + last_state_publish_time_ = lifecycle_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; @@ -892,12 +892,12 @@ void JointTrajectoryController::publish_state( rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - RCLCPP_INFO(node_->get_logger(), "Received new action goal"); + RCLCPP_INFO(lifecycle_node_->get_logger(), "Received new action goal"); // Precondition: Running controller if (get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_ERROR(node_->get_logger(), "Can't accept new action goals. Controller is not running."); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } @@ -906,14 +906,14 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( return rclcpp_action::GoalResponse::REJECT; } - RCLCPP_INFO(node_->get_logger(), "Accepted new action goal"); + RCLCPP_INFO(lifecycle_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(node_->get_logger(), "Got request to cancel goal"); + RCLCPP_INFO(lifecycle_node_->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) const auto active_goal = *rt_active_goal_.readFromNonRT(); @@ -924,7 +924,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( set_hold_position(); RCLCPP_DEBUG( - node_->get_logger(), "Canceling active action goal because cancel callback received."); + lifecycle_node_->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -953,7 +953,7 @@ void JointTrajectoryController::feedback_setup_callback( rt_active_goal_.writeFromNonRT(rt_goal); // Setup goal status checking timer - goal_handle_timer_ = node_->create_wall_timer( + goal_handle_timer_ = lifecycle_node_->create_wall_timer( action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } @@ -1018,7 +1018,7 @@ void JointTrajectoryController::sort_to_local_joint_order( } if (to_remap.size() != mapping.size()) { - RCLCPP_WARN(node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + RCLCPP_WARN(lifecycle_node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } std::vector output; @@ -1058,7 +1058,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - node_->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + lifecycle_node_->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; } @@ -1074,14 +1074,14 @@ bool JointTrajectoryController::validate_trajectory_msg( if (trajectory.joint_names.size() != joint_names_.size()) { RCLCPP_ERROR( - node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); + lifecycle_node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); return false; } } if (trajectory.joint_names.empty()) { - RCLCPP_ERROR(node_->get_logger(), "Empty joint names on incoming trajectory."); + RCLCPP_ERROR(lifecycle_node_->get_logger(), "Empty joint names on incoming trajectory."); return false; } @@ -1096,10 +1096,10 @@ bool JointTrajectoryController::validate_trajectory_msg( { trajectory_end_time += p.time_from_start; } - if (trajectory_end_time < node_->now()) + if (trajectory_end_time < lifecycle_node_->now()) { RCLCPP_ERROR( - node_->get_logger(), + lifecycle_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; @@ -1114,7 +1114,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if (it == joint_names_.end()) { RCLCPP_ERROR( - node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", + lifecycle_node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", incoming_joint_name.c_str()); return false; } @@ -1126,7 +1126,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( - node_->get_logger(), + lifecycle_node_->get_logger(), "Time between points %zu and %zu 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/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index d635b1e906..14c830b3da 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_lifecycle_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_lifecycle_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 0686119e4a..b8b4360e84 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_lifecycle_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_lifecycle_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command From e1989420e266b49fd865f6de198c769e4bc856cf Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:31:18 +0300 Subject: [PATCH 03/10] Fix trajectory controller --- .../test/test_trajectory_actions.cpp | 2 +- .../test/test_trajectory_controller.cpp | 8 ++++---- .../test/test_trajectory_controller_utils.hpp | 8 +++----- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 5bb6efaea6..996b5842b0 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_lifecycle_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 02b5f8e23f..f04deb54a2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -394,7 +394,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_lifecycle_node()->create_subscription( controller_name_ + "/state", qos_level, [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); @@ -676,7 +676,7 @@ 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_lifecycle_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]; @@ -708,7 +708,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_lifecycle_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()}; @@ -735,7 +735,7 @@ 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_lifecycle_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()}; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 83087308b1..b008463657 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -153,19 +153,18 @@ class TrajectoryControllerTest : public ::testing::Test { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_lifecycle_node(); for (const auto & param : parameters) { - traj_node_->set_parameter(param); + traj_controller_->get_lifecycle_node()->set_parameter(param); } if (executor) { - executor->add_node(traj_node_->get_node_base_interface()); + executor->add_node(traj_controller_->get_lifecycle_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_lifecycle_node()->set_parameter(stopped_velocity_parameters); traj_controller_->get_lifecycle_node()->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); @@ -354,7 +353,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_; From 1d7fdd618a2ed141707ea1c27490168871b2bb9c Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:40:53 +0300 Subject: [PATCH 04/10] pre-commit fixes --- .../src/diff_drive_controller.cpp | 51 ++++++++++------- .../test_joint_group_effort_controller.cpp | 9 ++- .../test_force_torque_sensor_broadcaster.cpp | 48 ++++++++++------ .../src/forward_command_controller.cpp | 4 +- .../test/test_forward_command_controller.cpp | 12 ++-- .../src/imu_sensor_broadcaster.cpp | 7 ++- .../src/joint_trajectory_controller.cpp | 57 ++++++++++++------- .../test/test_trajectory_controller.cpp | 22 +++++-- .../test_joint_group_position_controller.cpp | 9 ++- .../test_joint_group_velocity_controller.cpp | 9 ++- 10 files changed, 145 insertions(+), 83 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 290968daae..0eef4de9e6 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -326,7 +326,8 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & std::copy( pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - auto twist_diagonal = lifecycle_node_->get_parameter("twist_covariance_diagonal").as_double_array(); + auto twist_diagonal = + lifecycle_node_->get_parameter("twist_covariance_diagonal").as_double_array(); std::copy( twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); @@ -353,7 +354,8 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } catch (const std::runtime_error & e) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); + RCLCPP_ERROR( + lifecycle_node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); } try @@ -371,7 +373,8 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } catch (const std::runtime_error & e) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); + RCLCPP_ERROR( + lifecycle_node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } if (!reset()) @@ -384,8 +387,8 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & if (publish_limited_velocity_) { - limited_velocity_publisher_ = - lifecycle_node_->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + limited_velocity_publisher_ = lifecycle_node_->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = std::make_shared>(limited_velocity_publisher_); } @@ -405,7 +408,8 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { - RCLCPP_WARN(lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + RCLCPP_WARN( + lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) @@ -421,21 +425,23 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } else { - velocity_command_unstamped_subscriber_ = lifecycle_node_->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) - { - RCLCPP_WARN(lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = lifecycle_node_->get_clock()->now(); - }); + velocity_command_unstamped_subscriber_ = + lifecycle_node_->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = lifecycle_node_->get_clock()->now(); + }); } // initialize odometry publisher and messasge @@ -495,7 +501,8 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); + lifecycle_node_->get_logger(), + "Either left wheel interfaces, right wheel interfaces are non existent"); return CallbackReturn::ERROR; } diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index bb34e4e131..b6230a46fe 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -94,13 +94,15 @@ TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -197,7 +199,8 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) // 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()); + std::string(controller_->get_lifecycle_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); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 4049ad2338..70135b1aeb 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -113,8 +113,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -139,8 +141,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'interface_names' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -188,8 +192,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.z", "fts_sensor/torque.z"}); // set the 'frame_id' fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); @@ -232,8 +238,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.z", "fts_sensor/torque.z"}); fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -271,8 +279,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.z", "fts_sensor/torque.z"}); fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -295,12 +305,18 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.y", "fts_sensor/force.y"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.force.z", "fts_sensor/force.z"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.x", "fts_sensor/torque.x"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.y", "fts_sensor/torque.y"}); + fts_broadcaster_->get_lifecycle_node()->set_parameter( + {"interface_names.torque.z", "fts_sensor/torque.z"}); fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index eed17c91d6..2bfee7c684 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -139,8 +139,8 @@ CallbackReturn ForwardCommandController::on_activate( command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), - ordered_interfaces.size()); + lifecycle_node_->get_logger(), "Expected %zu position command interfaces, got %zu", + joint_names_.size(), ordered_interfaces.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index d0e01c186d..76040df75e 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -107,7 +107,8 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) SetUpController(); // configure failed, 'interface_name' parameter not set - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint2"}}); controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); // configure failed, 'interface_name' is empty @@ -118,7 +119,8 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint2"}}); controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); // configure successful @@ -129,7 +131,8 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint4"}}); controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); // activate failed, 'joint4' is not a valid joint name for the hardware @@ -270,7 +273,8 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) // 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()); + std::string(controller_->get_lifecycle_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); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 12f832c582..dc16285ae0 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -33,7 +33,8 @@ CallbackReturn IMUSensorBroadcaster::on_init() catch (const std::exception & e) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + lifecycle_node_->get_logger(), "Exception thrown during init stage with message: %s \n", + e.what()); return CallbackReturn::ERROR; } @@ -62,8 +63,8 @@ CallbackReturn IMUSensorBroadcaster::on_configure( try { // register ft sensor data publisher - sensor_state_publisher_ = - lifecycle_node_->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); + sensor_state_publisher_ = lifecycle_node_->create_publisher( + "~/imu", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } catch (const std::exception & e) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 92e8f5a3b2..62ecc313c6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -108,7 +108,9 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if ( + get_lifecycle_node()->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } @@ -171,7 +173,8 @@ controller_interface::return_type JointTrajectoryController::update( } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(lifecycle_node_->now(), state_current); + (*traj_point_active_ptr_) + ->set_point_before_trajectory_msg(lifecycle_node_->now(), state_current); } } resize_joint_trajectory_point(state_error, joint_num); @@ -296,8 +299,8 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_WARN( - lifecycle_node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - difference); + lifecycle_node_->get_logger(), + "Aborted due goal_time_tolerance exceeding by %f seconds", difference); } } } @@ -434,7 +437,8 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { - command_interface_types_ = lifecycle_node_->get_parameter("command_interfaces").as_string_array(); + command_interface_types_ = + lifecycle_node_->get_parameter("command_interfaces").as_string_array(); } if (command_interface_types_.empty()) @@ -628,14 +632,16 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S }; // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = lifecycle_node_->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); + joint_command_subscriber_ = + lifecycle_node_->create_subscription( + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); // TODO(karsten1987): no lifecycle for subscriber yet // joint_command_subscriber_->on_activate(); // State publisher - const double state_publish_rate = lifecycle_node_->get_parameter("state_publish_rate").get_value(); + const double state_publish_rate = + lifecycle_node_->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); if (state_publish_rate > 0.0) { @@ -646,7 +652,8 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = lifecycle_node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = + lifecycle_node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); const auto n_joints = joint_names_.size(); @@ -673,7 +680,8 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S last_state_publish_time_ = lifecycle_node_->now(); // action server configuration - allow_partial_joints_goal_ = lifecycle_node_->get_parameter("allow_partial_joints_goal").get_value(); + allow_partial_joints_goal_ = + lifecycle_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"); @@ -731,8 +739,8 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_command_interface_[index].size()); + lifecycle_node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", + joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -745,8 +753,8 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), - interface.c_str(), joint_state_interface_[index].size()); + lifecycle_node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", + joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -895,9 +903,12 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( RCLCPP_INFO(lifecycle_node_->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if ( + get_lifecycle_node()->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "Can't accept new action goals. Controller is not running."); + RCLCPP_ERROR( + lifecycle_node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } @@ -924,7 +935,8 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( set_hold_position(); RCLCPP_DEBUG( - lifecycle_node_->get_logger(), "Canceling active action goal because cancel callback received."); + lifecycle_node_->get_logger(), + "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -1018,7 +1030,8 @@ void JointTrajectoryController::sort_to_local_joint_order( } if (to_remap.size() != mapping.size()) { - RCLCPP_WARN(lifecycle_node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + RCLCPP_WARN( + lifecycle_node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } std::vector output; @@ -1058,8 +1071,9 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + lifecycle_node_->get_logger(), + "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", joint_names_size, + string_for_vector_field.c_str(), vector_field.size(), i); return false; } return true; @@ -1074,7 +1088,8 @@ bool JointTrajectoryController::validate_trajectory_msg( if (trajectory.joint_names.size() != joint_names_.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); + lifecycle_node_->get_logger(), + "Joints on incoming trajectory don't match the controller joints."); return false; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f04deb54a2..5d4a7ed58d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -97,7 +97,9 @@ TEST_P(TrajectoryControllerTestParameterized, activate) executor.add_node(traj_controller_->get_lifecycle_node()->get_node_base_interface()); traj_controller_->get_lifecycle_node()->configure(); - ASSERT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + traj_controller_->get_lifecycle_node()->get_current_state().id(), + State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -108,7 +110,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -708,7 +711,8 @@ 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_controller_->get_lifecycle_node()->get_logger(), "Sending new trajectory in the past"); + RCLCPP_INFO( + traj_controller_->get_lifecycle_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()}; @@ -735,7 +739,9 @@ 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_controller_->get_lifecycle_node()->get_logger(), "Sending new trajectory partially in the past"); + RCLCPP_INFO( + traj_controller_->get_lifecycle_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()}; @@ -1107,10 +1113,14 @@ INSTANTIATE_TEST_CASE_P( TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { - EXPECT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + traj_controller_->get_lifecycle_node()->get_current_state().id(), + State::PRIMARY_STATE_UNCONFIGURED); SetParameters(); // This call is replacing the way parameters are set via launch traj_controller_->get_lifecycle_node()->configure(); - EXPECT_EQ(traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + traj_controller_->get_lifecycle_node()->get_current_state().id(), + State::PRIMARY_STATE_UNCONFIGURED); }; SetUpTrajectoryController(false); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 14c830b3da..402273571b 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -94,13 +94,15 @@ TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -197,7 +199,8 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) // 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()); + std::string(controller_->get_lifecycle_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); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index b8b4360e84..0caf948939 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -94,13 +94,15 @@ TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_lifecycle_node()->set_parameter( + {"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -197,7 +199,8 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) // 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()); + std::string(controller_->get_lifecycle_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); From 88eed0e047b84a431cbd6bdc569b6942a91b19f7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 18 Sep 2021 11:42:57 +0100 Subject: [PATCH 05/10] Add time and period to update function --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 3fa121533c..49a681c1ef 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -241,8 +241,8 @@ controller_interface::return_type JointStateBroadcaster::update( state_interface.get_interface_name().c_str(), state_interface.get_value()); } - joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); - dynamic_joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); + joint_state_msg_.header.stamp = get_node()->get_clock()->now(); + dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now(); // update joint state message and dynamic joint state message for (auto i = 0ul; i < joint_names_.size(); ++i) From f3abd73be77c4585e22ebe9aa218d6de3d03348b Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 03:03:32 +0300 Subject: [PATCH 06/10] Add lifecycle nodes --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 49a681c1ef..3fa121533c 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -241,8 +241,8 @@ controller_interface::return_type JointStateBroadcaster::update( state_interface.get_interface_name().c_str(), state_interface.get_value()); } - joint_state_msg_.header.stamp = get_node()->get_clock()->now(); - dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now(); + joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); + dynamic_joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); // update joint state message and dynamic joint state message for (auto i = 0ul; i < joint_names_.size(); ++i) From 3348ea04dd249d8643359acec332c7d39a7c5882 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 21 Sep 2021 16:35:01 +0300 Subject: [PATCH 07/10] Name changes --- .../src/diff_drive_controller.cpp | 143 +++++++++--------- .../test/test_diff_drive_controller.cpp | 62 ++++---- .../src/joint_group_effort_controller.cpp | 2 +- .../test_joint_group_effort_controller.cpp | 29 ++-- .../src/force_torque_sensor_broadcaster.cpp | 26 ++-- .../test_force_torque_sensor_broadcaster.cpp | 82 ++++------ .../src/forward_command_controller.cpp | 18 +-- .../test/test_forward_command_controller.cpp | 60 ++++---- .../src/imu_sensor_broadcaster.cpp | 17 +-- .../test/test_imu_sensor_broadcaster.cpp | 22 +-- .../src/joint_state_broadcaster.cpp | 16 +- .../test/test_joint_state_broadcaster.cpp | 18 +-- .../src/joint_trajectory_controller.cpp | 115 ++++++-------- .../test/test_trajectory_actions.cpp | 2 +- .../test/test_trajectory_controller.cpp | 65 ++++---- .../test/test_trajectory_controller_utils.hpp | 14 +- .../src/joint_group_position_controller.cpp | 2 +- .../test_joint_group_position_controller.cpp | 27 ++-- .../src/joint_group_velocity_controller.cpp | 2 +- .../test_joint_group_velocity_controller.cpp | 29 ++-- 20 files changed, 345 insertions(+), 406 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 0eef4de9e6..e816b26f11 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -135,8 +135,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons controller_interface::return_type DiffDriveController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto logger = lifecycle_node_->get_logger(); - if (get_lifecycle_node()->get_current_state().id() == State::PRIMARY_STATE_INACTIVE) + auto logger = node_->get_logger(); + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { @@ -278,11 +278,11 @@ 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(); - right_wheel_names_ = lifecycle_node_->get_parameter("right_wheel_names").as_string_array(); + left_wheel_names_ = node_->get_parameter("left_wheel_names").as_string_array(); + right_wheel_names_ = node_->get_parameter("right_wheel_names").as_string_array(); if (left_wheel_names_.size() != right_wheel_names_.size()) { @@ -298,16 +298,16 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - wheel_params_.separation = lifecycle_node_->get_parameter("wheel_separation").as_double(); + wheel_params_.separation = node_->get_parameter("wheel_separation").as_double(); wheel_params_.wheels_per_side = - static_cast(lifecycle_node_->get_parameter("wheels_per_side").as_int()); - wheel_params_.radius = lifecycle_node_->get_parameter("wheel_radius").as_double(); + static_cast(node_->get_parameter("wheels_per_side").as_int()); + wheel_params_.radius = node_->get_parameter("wheel_radius").as_double(); wheel_params_.separation_multiplier = - lifecycle_node_->get_parameter("wheel_separation_multiplier").as_double(); + node_->get_parameter("wheel_separation_multiplier").as_double(); wheel_params_.left_radius_multiplier = - lifecycle_node_->get_parameter("left_wheel_radius_multiplier").as_double(); + node_->get_parameter("left_wheel_radius_multiplier").as_double(); wheel_params_.right_radius_multiplier = - lifecycle_node_->get_parameter("right_wheel_radius_multiplier").as_double(); + node_->get_parameter("right_wheel_radius_multiplier").as_double(); const auto wheels = wheel_params_; @@ -317,64 +317,61 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize( - lifecycle_node_->get_parameter("velocity_rolling_window_size").as_int()); + node_->get_parameter("velocity_rolling_window_size").as_int()); - odom_params_.odom_frame_id = lifecycle_node_->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = lifecycle_node_->get_parameter("base_frame_id").as_string(); + odom_params_.odom_frame_id = node_->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = node_->get_parameter("base_frame_id").as_string(); - auto pose_diagonal = lifecycle_node_->get_parameter("pose_covariance_diagonal").as_double_array(); + auto pose_diagonal = node_->get_parameter("pose_covariance_diagonal").as_double_array(); std::copy( pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - auto twist_diagonal = - lifecycle_node_->get_parameter("twist_covariance_diagonal").as_double_array(); + auto twist_diagonal = node_->get_parameter("twist_covariance_diagonal").as_double_array(); std::copy( twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - odom_params_.open_loop = lifecycle_node_->get_parameter("open_loop").as_bool(); - odom_params_.enable_odom_tf = lifecycle_node_->get_parameter("enable_odom_tf").as_bool(); + odom_params_.open_loop = node_->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool(); cmd_vel_timeout_ = std::chrono::milliseconds{ - static_cast(lifecycle_node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = lifecycle_node_->get_parameter("publish_limited_velocity").as_bool(); - use_stamped_vel_ = lifecycle_node_->get_parameter("use_stamped_vel").as_bool(); + static_cast(node_->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; + publish_limited_velocity_ = node_->get_parameter("publish_limited_velocity").as_bool(); + use_stamped_vel_ = node_->get_parameter("use_stamped_vel").as_bool(); try { limiter_linear_ = SpeedLimiter( - lifecycle_node_->get_parameter("linear.x.has_velocity_limits").as_bool(), - lifecycle_node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), - lifecycle_node_->get_parameter("linear.x.has_jerk_limits").as_bool(), - lifecycle_node_->get_parameter("linear.x.min_velocity").as_double(), - lifecycle_node_->get_parameter("linear.x.max_velocity").as_double(), - lifecycle_node_->get_parameter("linear.x.min_acceleration").as_double(), - lifecycle_node_->get_parameter("linear.x.max_acceleration").as_double(), - lifecycle_node_->get_parameter("linear.x.min_jerk").as_double(), - lifecycle_node_->get_parameter("linear.x.max_jerk").as_double()); + node_->get_parameter("linear.x.has_velocity_limits").as_bool(), + node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), + node_->get_parameter("linear.x.has_jerk_limits").as_bool(), + node_->get_parameter("linear.x.min_velocity").as_double(), + node_->get_parameter("linear.x.max_velocity").as_double(), + node_->get_parameter("linear.x.min_acceleration").as_double(), + node_->get_parameter("linear.x.max_acceleration").as_double(), + node_->get_parameter("linear.x.min_jerk").as_double(), + node_->get_parameter("linear.x.max_jerk").as_double()); } catch (const std::runtime_error & e) { - RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "Error configuring linear speed limiter: %s", e.what()); } try { limiter_angular_ = SpeedLimiter( - lifecycle_node_->get_parameter("angular.z.has_velocity_limits").as_bool(), - lifecycle_node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), - lifecycle_node_->get_parameter("angular.z.has_jerk_limits").as_bool(), - lifecycle_node_->get_parameter("angular.z.min_velocity").as_double(), - lifecycle_node_->get_parameter("angular.z.max_velocity").as_double(), - lifecycle_node_->get_parameter("angular.z.min_acceleration").as_double(), - lifecycle_node_->get_parameter("angular.z.max_acceleration").as_double(), - lifecycle_node_->get_parameter("angular.z.min_jerk").as_double(), - lifecycle_node_->get_parameter("angular.z.max_jerk").as_double()); + node_->get_parameter("angular.z.has_velocity_limits").as_bool(), + node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), + node_->get_parameter("angular.z.has_jerk_limits").as_bool(), + node_->get_parameter("angular.z.min_velocity").as_double(), + node_->get_parameter("angular.z.max_velocity").as_double(), + node_->get_parameter("angular.z.min_acceleration").as_double(), + node_->get_parameter("angular.z.max_acceleration").as_double(), + node_->get_parameter("angular.z.min_jerk").as_double(), + node_->get_parameter("angular.z.max_jerk").as_double()); } catch (const std::runtime_error & e) { - RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "Error configuring angular speed limiter: %s", e.what()); } if (!reset()) @@ -387,8 +384,8 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & if (publish_limited_velocity_) { - limited_velocity_publisher_ = lifecycle_node_->create_publisher( - DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + limited_velocity_publisher_ = + node_->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = std::make_shared>(limited_velocity_publisher_); } @@ -403,49 +400,46 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & // initialize command subscriber if (use_stamped_vel_) { - velocity_command_subscriber_ = lifecycle_node_->create_subscription( + velocity_command_subscriber_ = node_->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { - RCLCPP_WARN( - lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { RCLCPP_WARN_ONCE( - lifecycle_node_->get_logger(), + node_->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " "time, this message will only be shown once"); - msg->header.stamp = lifecycle_node_->get_clock()->now(); + msg->header.stamp = node_->get_clock()->now(); } received_velocity_msg_ptr_.set(std::move(msg)); }); } else { - velocity_command_unstamped_subscriber_ = - lifecycle_node_->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - lifecycle_node_->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = lifecycle_node_->get_clock()->now(); - }); + velocity_command_unstamped_subscriber_ = node_->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) + { + RCLCPP_WARN(node_->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = node_->get_clock()->now(); + }); } // initialize odometry publisher and messasge - odometry_publisher_ = lifecycle_node_->create_publisher( + odometry_publisher_ = node_->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = std::make_shared>( @@ -470,7 +464,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & } // initialize transform publisher and message - odometry_transform_publisher_ = lifecycle_node_->create_publisher( + odometry_transform_publisher_ = node_->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_transform_publisher_ = std::make_shared>( @@ -482,7 +476,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(); return CallbackReturn::SUCCESS; } @@ -501,15 +495,14 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), - "Either left wheel interfaces, right wheel interfaces are non existent"); + node_->get_logger(), "Either left wheel interfaces, right wheel interfaces are non existent"); return CallbackReturn::ERROR; } is_halted = false; subscriber_is_active_ = true; - RCLCPP_DEBUG(lifecycle_node_->get_logger(), "Subscriber and publisher are now active."); + RCLCPP_DEBUG(node_->get_logger(), "Subscriber and publisher are now active."); return CallbackReturn::SUCCESS; } @@ -581,7 +574,7 @@ CallbackReturn DiffDriveController::configure_side( const std::string & side, const std::vector & wheel_names, std::vector & registered_handles) { - auto logger = lifecycle_node_->get_logger(); + auto logger = node_->get_logger(); if (wheel_names.empty()) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 1d3c47b213..12ba84fd4c 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -176,16 +176,16 @@ TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_sid const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -196,12 +196,12 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); auto extended_right_wheel_names = right_wheel_names; extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -212,9 +212,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -232,9 +232,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -246,9 +246,9 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_resources_assigned) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -261,20 +261,20 @@ TEST_F(TestDiffDriveController, cleanup) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_lifecycle_node()->get_node_base_interface()); - auto state = controller_->get_lifecycle_node()->configure(); + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); assignResources(); - state = controller_->get_lifecycle_node()->activate(); + state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); waitForSetup(); @@ -289,13 +289,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_->get_lifecycle_node()->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_->get_lifecycle_node()->cleanup(); + state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped @@ -310,24 +310,24 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_lifecycle_node()->set_parameter( + controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_lifecycle_node()->get_node_base_interface()); + executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->get_lifecycle_node()->configure(); + auto state = controller_->get_node()->configure(); assignResources(); 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_->get_lifecycle_node()->activate(); + state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); // send msg @@ -346,7 +346,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_->get_lifecycle_node()->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)), @@ -356,12 +356,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_->get_lifecycle_node()->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_->get_lifecycle_node()->configure(); + state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index 7c49a017db..e607e3c27f 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -41,7 +41,7 @@ CallbackReturn JointGroupEffortController::on_init() { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupEffortController constructor. - get_lifecycle_node()->set_parameter( + get_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_EFFORT)); } catch (const std::exception & e) diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index b6230a46fe..f8cd10bc4b 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -75,7 +75,7 @@ TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); + controller_->get_node()->set_parameter({"joints", std::vector()}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -84,7 +84,7 @@ TEST_F(JointGroupEffortControllerTest, JointsParameterIsEmpty) TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -94,15 +94,13 @@ TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint4"}}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint2"}}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -112,7 +110,7 @@ TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails) TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -144,7 +142,7 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints @@ -166,7 +164,7 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -183,24 +181,23 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // default values ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); - auto node_state = controller_->get_lifecycle_node()->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_lifecycle_node()->activate(); + node_state = controller_->get_node()->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()); + std::string(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); @@ -209,7 +206,7 @@ TEST_F(JointGroupEffortControllerTest, 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( @@ -225,7 +222,7 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index eaec018769..5f5c880c39 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -53,13 +53,13 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init() CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = lifecycle_node_->get_parameter("sensor_name").as_string(); - interface_names_[0] = lifecycle_node_->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] = lifecycle_node_->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] = lifecycle_node_->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] = lifecycle_node_->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] = lifecycle_node_->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] = lifecycle_node_->get_parameter("interface_names.torque.z").as_string(); + sensor_name_ = node_->get_parameter("sensor_name").as_string(); + interface_names_[0] = node_->get_parameter("interface_names.force.x").as_string(); + interface_names_[1] = node_->get_parameter("interface_names.force.y").as_string(); + interface_names_[2] = node_->get_parameter("interface_names.force.z").as_string(); + interface_names_[3] = node_->get_parameter("interface_names.torque.x").as_string(); + interface_names_[4] = node_->get_parameter("interface_names.torque.y").as_string(); + interface_names_[5] = node_->get_parameter("interface_names.torque.z").as_string(); const bool no_interface_names_defined = std::count(interface_names_.begin(), interface_names_.end(), "") == 6; @@ -67,7 +67,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (sensor_name_.empty() && no_interface_names_defined) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be specified."); return CallbackReturn::ERROR; @@ -76,16 +76,16 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (!sensor_name_.empty() && !no_interface_names_defined) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), + node_->get_logger(), "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together."); return CallbackReturn::ERROR; } - frame_id_ = lifecycle_node_->get_parameter("frame_id").as_string(); + frame_id_ = node_->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } @@ -105,7 +105,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( try { // register ft sensor data publisher - sensor_state_publisher_ = lifecycle_node_->create_publisher( + sensor_state_publisher_ = node_->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } @@ -121,7 +121,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(lifecycle_node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(node_->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 70135b1aeb..db8f6e7022 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -113,10 +113,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -127,7 +125,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -138,13 +136,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'interface_names' - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -155,7 +151,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe SetUpFTSBroadcaster(); // set the 'sensor_name' empty - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", ""}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); // configure failed, 'sensor_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -166,8 +162,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe SetUpFTSBroadcaster(); // set the 'interface_names' empty - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.force.x", ""}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"interface_names.torque.z", ""}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""}); // configure failed, 'interface_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -178,10 +174,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -192,13 +188,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // set the 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -209,8 +203,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -222,8 +216,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -238,11 +232,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -256,8 +248,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -279,11 +271,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -305,19 +295,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.y", "fts_sensor/force.y"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.force.z", "fts_sensor/force.z"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.x", "fts_sensor/torque.x"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.y", "fts_sensor/torque.y"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter( - {"interface_names.torque.z", "fts_sensor/torque.z"}); - fts_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 2bfee7c684..49284ae5cb 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -55,31 +55,31 @@ CallbackReturn ForwardCommandController::on_init() CallbackReturn ForwardCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - joint_names_ = lifecycle_node_->get_parameter("joints").as_string_array(); + joint_names_ = node_->get_parameter("joints").as_string_array(); if (joint_names_.empty()) { - RCLCPP_ERROR(get_lifecycle_node()->get_logger(), "'joints' parameter was empty"); + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); return CallbackReturn::ERROR; } // Specialized, child controllers set interfaces before calling configure function. if (interface_name_.empty()) { - interface_name_ = lifecycle_node_->get_parameter("interface_name").as_string(); + interface_name_ = node_->get_parameter("interface_name").as_string(); } if (interface_name_.empty()) { - RCLCPP_ERROR(get_lifecycle_node()->get_logger(), "'interface_name' parameter was empty"); + RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); return CallbackReturn::ERROR; } - joints_command_subscriber_ = get_lifecycle_node()->create_subscription( + joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - RCLCPP_INFO(get_lifecycle_node()->get_logger(), "configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -139,8 +139,8 @@ CallbackReturn ForwardCommandController::on_activate( command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Expected %zu position command interfaces, got %zu", - joint_names_.size(), ordered_interfaces.size()); + node_->get_logger(), "Expected %zu position command interfaces, got %zu", joint_names_.size(), + ordered_interfaces.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -167,7 +167,7 @@ controller_interface::return_type ForwardCommandController::update( if ((*joint_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( - get_lifecycle_node()->get_logger(), *lifecycle_node_->get_clock(), 1000, + get_node()->get_logger(), *node_->get_clock(), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 76040df75e..c95be0eb2c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -75,7 +75,7 @@ void ForwardCommandControllerTest::SetUpController() TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "dummy"}); + controller_->get_node()->set_parameter({"interface_name", "dummy"}); // configure failed, 'joints' parameter not set ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -86,17 +86,17 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet) SetUpController(); // configure failed, 'interface_name' parameter not set - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); + controller_->get_node()->set_parameter({"joints", std::vector()}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); + controller_->get_node()->set_parameter({"interface_name", ""}); } TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); + controller_->get_node()->set_parameter({"joints", std::vector()}); + controller_->get_node()->set_parameter({"interface_name", ""}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -107,9 +107,8 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty) SetUpController(); // configure failed, 'interface_name' parameter not set - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint2"}}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", ""}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_node()->set_parameter({"interface_name", ""}); // configure failed, 'interface_name' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -119,9 +118,8 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint2"}}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); + controller_->get_node()->set_parameter({"interface_name", "position"}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -131,15 +129,14 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint4"}}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); + controller_->get_node()->set_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_->get_lifecycle_node()->set_parameter( + auto result = controller_->get_node()->set_parameter( {"joints", std::vector{"joint1", "joint2"}}); ASSERT_TRUE(result.successful); @@ -152,8 +149,8 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "acceleration"}); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "acceleration"}); // activate failed, 'joint4' not in interfaces ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -164,8 +161,8 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -176,8 +173,8 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) SetUpController(); // configure controller - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -211,8 +208,8 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) SetUpController(); // configure controller - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -237,8 +234,8 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) SetUpController(); // configure controller - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "position"}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -256,25 +253,24 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); - controller_->get_lifecycle_node()->set_parameter({"interface_name", "position"}); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_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_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_lifecycle_node()->activate(); + node_state = controller_->get_node()->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()); + std::string(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); @@ -283,7 +279,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( diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index dc16285ae0..23aa33777c 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -33,8 +33,7 @@ CallbackReturn IMUSensorBroadcaster::on_init() catch (const std::exception & e) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Exception thrown during init stage with message: %s \n", - e.what()); + node_->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -44,17 +43,17 @@ CallbackReturn IMUSensorBroadcaster::on_init() CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = lifecycle_node_->get_parameter("sensor_name").as_string(); + sensor_name_ = node_->get_parameter("sensor_name").as_string(); if (sensor_name_.empty()) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "'sensor_name' parameter has to be specified."); + RCLCPP_ERROR(node_->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } - frame_id_ = lifecycle_node_->get_parameter("frame_id").as_string(); + frame_id_ = node_->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } @@ -63,8 +62,8 @@ CallbackReturn IMUSensorBroadcaster::on_configure( try { // register ft sensor data publisher - sensor_state_publisher_ = lifecycle_node_->create_publisher( - "~/imu", rclcpp::SystemDefaultsQoS()); + sensor_state_publisher_ = + node_->create_publisher("~/imu", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); } catch (const std::exception & e) @@ -79,7 +78,7 @@ CallbackReturn IMUSensorBroadcaster::on_configure( realtime_publisher_->msg_.header.frame_id = frame_id_; realtime_publisher_->unlock(); - RCLCPP_DEBUG(lifecycle_node_->get_logger(), "configure successful"); + RCLCPP_DEBUG(node_->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 3eff71b990..d27be76b3b 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -115,9 +115,9 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) SetUpIMUBroadcaster(); // set the 'interface_names' - imu_broadcaster_->get_lifecycle_node()->set_parameter( + imu_broadcaster_->get_node()->set_parameter( {"interface_names.angular_velocity.x", "imu_sensor/angular_velocity.x"}); - imu_broadcaster_->get_lifecycle_node()->set_parameter( + imu_broadcaster_->get_node()->set_parameter( {"interface_names.linear_acceleration.z", "imu_sensor/linear_acceleration.z"}); // configure failed, 'frame_id' parameter not set @@ -129,7 +129,7 @@ TEST_F(IMUSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) SetUpIMUBroadcaster(); // set the 'sensor_name' - imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // configure failed, 'frame_id' parameter not set ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -140,10 +140,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) SetUpIMUBroadcaster(); // set the 'sensor_name' - imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -154,8 +154,8 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) SetUpIMUBroadcaster(); // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); - imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -167,8 +167,8 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) SetUpIMUBroadcaster(); // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); - imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -183,8 +183,8 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) SetUpIMUBroadcaster(); // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_lifecycle_node()->set_parameter({"sensor_name", sensor_name_}); - imu_broadcaster_->get_lifecycle_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 3fa121533c..fc924ba24d 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -78,22 +78,22 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - use_local_topics_ = get_lifecycle_node()->get_parameter("use_local_topics").as_bool(); + use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); try { const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; - joint_state_publisher_ = get_lifecycle_node()->create_publisher( + joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); dynamic_joint_state_publisher_ = - get_lifecycle_node()->create_publisher( + get_node()->create_publisher( topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); } catch (const std::exception & e) { - // get_lifecycle_node() may throw, logging raw here + // get_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -165,7 +165,7 @@ bool JointStateBroadcaster::init_joint_data() // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there rclcpp::Parameter extra_joints; - if (get_lifecycle_node()->get_parameter("extra_joints", extra_joints)) + if (get_node()->get_parameter("extra_joints", extra_joints)) { const std::vector & extra_joints_names = extra_joints.as_string_array(); for (const auto & extra_joint_name : extra_joints_names) @@ -237,12 +237,12 @@ controller_interface::return_type JointStateBroadcaster::update( 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", state_interface.get_name().c_str(), + 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()); } - joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); - dynamic_joint_state_msg_.header.stamp = get_lifecycle_node()->get_clock()->now(); + joint_state_msg_.header.stamp = get_node()->get_clock()->now(); + dynamic_joint_state_msg_.header.stamp = get_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_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 97fda27fdd..efe51c3487 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -181,8 +181,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) { SetUpStateBroadcaster(); - auto node_state = state_broadcaster_->get_lifecycle_node()->configure(); - node_state = state_broadcaster_->get_lifecycle_node()->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)), @@ -191,10 +191,10 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) { - auto node_state = state_broadcaster_->get_lifecycle_node()->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_->get_lifecycle_node()->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"); @@ -235,7 +235,7 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest) TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic) { SetUpStateBroadcaster(); - state_broadcaster_->get_lifecycle_node()->set_parameter({"use_local_topics", true}); + state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); test_published_joint_state_message("joint_state_broadcaster/joint_states"); } @@ -243,10 +243,10 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic) void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( const std::string & topic) { - auto node_state = state_broadcaster_->get_lifecycle_node()->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_->get_lifecycle_node()->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"); @@ -298,7 +298,7 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest) TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic) { SetUpStateBroadcaster(); - state_broadcaster_->get_lifecycle_node()->set_parameter({"use_local_topics", true}); + state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states"); } @@ -323,7 +323,7 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) // Add extra joints as parameters const std::vector extra_joint_names = {"extra1", "extra2", "extra3"}; - state_broadcaster_->get_lifecycle_node()->set_parameter({"extra_joints", extra_joint_names}); + state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names}); // configure ok ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 62ecc313c6..843ce78459 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -108,9 +108,7 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if ( - get_lifecycle_node()->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } @@ -169,12 +167,11 @@ controller_interface::return_type JointTrajectoryController::update( if (open_loop_control_) { (*traj_point_active_ptr_) - ->set_point_before_trajectory_msg(lifecycle_node_->now(), last_commanded_state_); + ->set_point_before_trajectory_msg(node_->now(), last_commanded_state_); } else { - (*traj_point_active_ptr_) - ->set_point_before_trajectory_msg(lifecycle_node_->now(), state_current); + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(node_->now(), state_current); } } resize_joint_trajectory_point(state_error, joint_num); @@ -184,7 +181,7 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(anyone): this is kind-of open-loop concept? I am right? const bool valid_point = (*traj_point_active_ptr_) - ->sample(lifecycle_node_->now(), state_desired, start_segment_itr, end_segment_itr); + ->sample(node_->now(), state_desired, start_segment_itr, end_segment_itr); if (valid_point) { @@ -240,7 +237,7 @@ controller_interface::return_type JointTrajectoryController::update( { // 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; @@ -255,12 +252,12 @@ controller_interface::return_type JointTrajectoryController::update( 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); } active_goal->setAborted(result); @@ -281,7 +278,7 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - 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) { @@ -289,7 +286,7 @@ controller_interface::return_type JointTrajectoryController::update( 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(); @@ -299,8 +296,8 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_WARN( - lifecycle_node_->get_logger(), - "Aborted due goal_time_tolerance exceeding by %f seconds", difference); + node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + difference); } } } @@ -419,10 +416,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto 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()) { @@ -437,8 +434,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { - command_interface_types_ = - lifecycle_node_->get_parameter("command_interfaces").as_string_array(); + command_interface_types_ = node_->get_parameter("command_interfaces").as_string_array(); } if (command_interface_types_.empty()) @@ -532,7 +528,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // Read always state interfaces from the parameter because they can be used // independently from the controller's type. // Specialized, child controllers should set its default value. - state_interface_types_ = lifecycle_node_->get_parameter("state_interfaces").as_string_array(); + state_interface_types_ = node_->get_parameter("state_interfaces").as_string_array(); if (state_interface_types_.empty()) { @@ -609,10 +605,10 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*lifecycle_node_, joint_names_); + default_tolerances_ = get_segment_tolerances(*node_, joint_names_); // Read parameters customizing controller for special cases - open_loop_control_ = lifecycle_node_->get_parameter("open_loop_control").get_value(); + open_loop_control_ = node_->get_parameter("open_loop_control").get_value(); // subscriber callback // non realtime @@ -632,16 +628,14 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S }; // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = - lifecycle_node_->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); + joint_command_subscriber_ = node_->create_subscription( + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); // TODO(karsten1987): no lifecycle for subscriber yet // joint_command_subscriber_->on_activate(); // State publisher - const double state_publish_rate = - lifecycle_node_->get_parameter("state_publish_rate").get_value(); + const double state_publish_rate = node_->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate); if (state_publish_rate > 0.0) { @@ -652,8 +646,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); } - publisher_ = - lifecycle_node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + publisher_ = node_->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); const auto n_joints = joint_names_.size(); @@ -677,27 +670,26 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } 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").get_value(); + 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").get_value(); + node_->get_parameter("action_monitor_rate").get_value(); RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); 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)); @@ -739,8 +731,8 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(), + interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -753,8 +745,8 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); + node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), + interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -780,7 +772,7 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St subscriber_is_active_ = true; traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = lifecycle_node_->now(); + last_state_publish_time_ = node_->now(); // Initialize current state storage if hardware state has tracking offset resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); @@ -868,14 +860,14 @@ 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; @@ -900,15 +892,12 @@ void JointTrajectoryController::publish_state( 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 ( - get_lifecycle_node()->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Can't accept new action goals. Controller is not running."); + RCLCPP_ERROR(node_->get_logger(), "Can't accept new action goals. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } @@ -917,14 +906,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) const auto active_goal = *rt_active_goal_.readFromNonRT(); @@ -935,8 +924,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( set_hold_position(); RCLCPP_DEBUG( - lifecycle_node_->get_logger(), - "Canceling active action goal because cancel callback received."); + node_->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled auto action_res = std::make_shared(); @@ -965,7 +953,7 @@ void JointTrajectoryController::feedback_setup_callback( rt_active_goal_.writeFromNonRT(rt_goal); // 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_goal)); } @@ -1030,8 +1018,7 @@ void JointTrajectoryController::sort_to_local_joint_order( } if (to_remap.size() != mapping.size()) { - RCLCPP_WARN( - lifecycle_node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + RCLCPP_WARN(node_->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } std::vector output; @@ -1071,9 +1058,8 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), - "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", joint_names_size, - string_for_vector_field.c_str(), vector_field.size(), i); + node_->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; } return true; @@ -1088,15 +1074,14 @@ bool JointTrajectoryController::validate_trajectory_msg( if (trajectory.joint_names.size() != joint_names_.size()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), - "Joints on incoming trajectory don't match the controller joints."); + node_->get_logger(), "Joints on incoming trajectory don't match the controller joints."); return false; } } if (trajectory.joint_names.empty()) { - RCLCPP_ERROR(lifecycle_node_->get_logger(), "Empty joint names on incoming trajectory."); + RCLCPP_ERROR(node_->get_logger(), "Empty joint names on incoming trajectory."); return false; } @@ -1111,10 +1096,10 @@ bool JointTrajectoryController::validate_trajectory_msg( { 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; @@ -1129,7 +1114,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if (it == joint_names_.end()) { RCLCPP_ERROR( - lifecycle_node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", + node_->get_logger(), "Incoming joint %s doesn't match the controller's joints.", incoming_joint_name.c_str()); return false; } @@ -1141,7 +1126,7 @@ bool JointTrajectoryController::validate_trajectory_msg( 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 %zu and %zu 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 996b5842b0..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_controller_->get_lifecycle_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 5d4a7ed58d..fa56879614 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -65,10 +65,10 @@ TEST_P(TrajectoryControllerTestParameterized, configure) 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_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -94,12 +94,10 @@ TEST_P(TrajectoryControllerTestParameterized, activate) 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()); - traj_controller_->get_lifecycle_node()->configure(); - ASSERT_EQ( - traj_controller_->get_lifecycle_node()->get_current_state().id(), - State::PRIMARY_STATE_INACTIVE); + 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(); ASSERT_EQ( @@ -110,8 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); ActivateTrajectoryController(); - ASSERT_EQ( - traj_controller_->get_lifecycle_node()->get_current_state().id(), State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -124,7 +121,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // FAIL(); // } // -// auto traj_node = traj_controller->get_lifecycle_node(); +// auto traj_node = traj_controller->get_node(); // rclcpp::executors::MultiThreadedExecutor executor; // executor.add_node(traj_node->get_node_base_interface()); // @@ -166,7 +163,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // FAIL(); // } // -// auto traj_node = traj_controller->get_lifecycle_node(); +// auto traj_node = traj_controller->get_node(); // rclcpp::executors::MultiThreadedExecutor executor; // executor.add_node(traj_node->get_node_base_interface()); // @@ -233,7 +230,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) { SetUpAndActivateTrajectoryController(); - auto traj_node = traj_controller_->get_lifecycle_node(); + auto traj_node = traj_controller_->get_node(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(traj_node->get_node_base_interface()); @@ -246,11 +243,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_->get_lifecycle_node()->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_->get_lifecycle_node()->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,15 +267,15 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); - traj_controller_->get_lifecycle_node()->configure(); - auto state = traj_controller_->get_lifecycle_node()->get_current_state(); + traj_controller_->get_node()->configure(); + auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ActivateTrajectoryController(); 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()); - state = traj_controller_->get_lifecycle_node()->get_current_state(); + state = traj_controller_->get_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]); @@ -301,7 +298,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_->get_lifecycle_node()->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? @@ -313,7 +310,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); // cleanup - state = traj_controller_->get_lifecycle_node()->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)); @@ -327,7 +324,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_->get_lifecycle_node()->configure(); + state = traj_controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); @@ -397,7 +394,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_controller_->get_lifecycle_node()->create_subscription( + traj_controller_->get_node()->create_subscription( controller_name_ + "/state", qos_level, [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); @@ -679,7 +676,7 @@ 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_controller_->get_lifecycle_node()->get_logger(), "Sending new trajectory"); + RCLCPP_INFO(traj_controller_->get_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]; @@ -711,8 +708,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_controller_->get_lifecycle_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()}; @@ -740,8 +736,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO( - traj_controller_->get_lifecycle_node()->get_logger(), - "Sending new trajectory partially in the past"); + 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()}; @@ -753,7 +748,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) { SetUpTrajectoryController(); - auto traj_node = traj_controller_->get_lifecycle_node(); + auto traj_node = traj_controller_->get_node(); RCLCPP_WARN( traj_node->get_logger(), "Test disabled until current_trajectory is taken into account when adding a new trajectory."); @@ -766,8 +761,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_->get_lifecycle_node()->configure(); - traj_controller_->get_lifecycle_node()->activate(); + traj_controller_->get_node()->configure(); + traj_controller_->get_node()->activate(); std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; std::vector> partial_traj{ @@ -1113,14 +1108,10 @@ INSTANTIATE_TEST_CASE_P( TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { - EXPECT_EQ( - traj_controller_->get_lifecycle_node()->get_current_state().id(), - State::PRIMARY_STATE_UNCONFIGURED); + 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_->get_lifecycle_node()->configure(); - EXPECT_EQ( - traj_controller_->get_lifecycle_node()->get_current_state().id(), - State::PRIMARY_STATE_UNCONFIGURED); + traj_controller_->get_node()->configure(); + EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); }; SetUpTrajectoryController(false); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b008463657..27bb6d9548 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -140,7 +140,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetParameters() { - auto node = traj_controller_->get_lifecycle_node(); + auto node = traj_controller_->get_node(); const rclcpp::Parameter joint_names_param("joints", joint_names_); const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_); const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); @@ -155,18 +155,18 @@ class TrajectoryControllerTest : public ::testing::Test for (const auto & param : parameters) { - traj_controller_->get_lifecycle_node()->set_parameter(param); + traj_controller_->get_node()->set_parameter(param); } if (executor) { - executor->add_node(traj_controller_->get_lifecycle_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_controller_->get_lifecycle_node()->set_parameter(stopped_velocity_parameters); + traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); - traj_controller_->get_lifecycle_node()->configure(); + traj_controller_->get_node()->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); } @@ -215,14 +215,14 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_lifecycle_node()->activate(); + traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } 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", static_cast(100))); diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index acdf35b333..e52f9425fd 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -41,7 +41,7 @@ CallbackReturn JointGroupPositionController::on_init() { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupPositionController constructor. - get_lifecycle_node()->set_parameter( + get_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_POSITION)); } catch (const std::exception & e) diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 402273571b..fc93bb7dfa 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -75,7 +75,7 @@ TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) TEST_F(JointGroupPositionControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); + controller_->get_node()->set_parameter({"joints", std::vector()}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -84,7 +84,7 @@ TEST_F(JointGroupPositionControllerTest, JointsParameterIsEmpty) TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -94,15 +94,13 @@ TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint4"}}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint2"}}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -112,7 +110,7 @@ TEST_F(JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails) TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -144,7 +142,7 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints @@ -166,7 +164,7 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -183,24 +181,23 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // 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_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_lifecycle_node()->activate(); + node_state = controller_->get_node()->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()); + std::string(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); @@ -209,7 +206,7 @@ TEST_F(JointGroupPositionControllerTest, 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( diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 238119f994..c2c7d762bd 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -41,7 +41,7 @@ CallbackReturn JointGroupVelocityController::on_init() { // Explicitly set the interface parameter declared by the forward_command_controller // to match the value set in the JointGroupVelocityController constructor. - get_lifecycle_node()->set_parameter( + get_node()->set_parameter( rclcpp::Parameter("interface_name", hardware_interface::HW_IF_VELOCITY)); } catch (const std::exception & e) diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 0caf948939..1bcbaf510e 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -75,7 +75,7 @@ TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) TEST_F(JointGroupVelocityControllerTest, JointsParameterIsEmpty) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", std::vector()}); + controller_->get_node()->set_parameter({"joints", std::vector()}); // configure failed, 'joints' is empty ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -84,7 +84,7 @@ TEST_F(JointGroupVelocityControllerTest, JointsParameterIsEmpty) TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -94,15 +94,13 @@ TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) TEST_F(JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint4"}}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint4"}}); // 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); - controller_->get_lifecycle_node()->set_parameter( - {"joints", std::vector{"joint1", "joint2"}}); + controller_->get_node()->set_parameter({"joints", std::vector{"joint1", "joint2"}}); // activate failed, 'acceleration' is not a registered interface for `joint1` ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -112,7 +110,7 @@ TEST_F(JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails) TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet @@ -144,7 +142,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints @@ -166,7 +164,7 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet @@ -183,24 +181,23 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // default values ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); - auto node_state = controller_->get_lifecycle_node()->configure(); + auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_lifecycle_node()->activate(); + node_state = controller_->get_node()->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()); + std::string(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); @@ -209,7 +206,7 @@ TEST_F(JointGroupVelocityControllerTest, 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( @@ -225,7 +222,7 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) { SetUpController(); - controller_->get_lifecycle_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"joints", joint_names_}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); From 4b2eb0c8c62a5ac2f1b927a80cf7f0d5cd22190e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 9 Mar 2022 16:02:52 +0100 Subject: [PATCH 08/10] Final fixes to get things compiled. --- .../test/test_forward_command_controller.cpp | 6 +++--- .../test/test_trajectory_controller.cpp | 2 +- .../test/test_trajectory_controller_utils.hpp | 3 +-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index edd11799b0..e217f88a1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -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/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 280263eafd..8771fcee41 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -721,7 +721,7 @@ 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); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 89d378c28c..94b86a6242 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -172,7 +172,6 @@ class TrajectoryControllerTest : public ::testing::Test { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_node(); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); @@ -189,7 +188,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before configure SetPidParameters(); traj_controller_->get_node()->configure(); - + ActivateTrajectoryController(separate_cmd_and_state_values); } From b6c22b1651365afd16760b7e70b89e48c3566420 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 15 Mar 2022 09:09:57 +0100 Subject: [PATCH 09/10] Fixing typo in the tests. --- diff_drive_controller/test/test_diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 3966d83312..ce5a6b09a2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -390,7 +390,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) executor.add_node(controller_->get_node()->get_node_base_interface()); auto state = controller_->get_node()->configure(); - assignResources(); + assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); From 9af7dcaf8abdfd2c23210bd3bb284de3c60cc978 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 16 Mar 2022 13:38:07 +0100 Subject: [PATCH 10/10] More changes to LifecycleNodes. --- .../hardware_interface_adapter.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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