diff --git a/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp b/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp index db34da588..201caad3f 100644 --- a/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp +++ b/ur_controllers/include/ur_controllers/force_torque_sensor_broadcaster.hpp @@ -22,6 +22,8 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" namespace ur_controllers { @@ -67,7 +69,7 @@ class ForceTorqueStateBroadcaster : public controller_interface::ControllerInter controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update() override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index 8fae91855..38ce02fc4 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -36,6 +36,8 @@ #include "ur_dashboard_msgs/msg/safety_mode.hpp" #include "ur_msgs/srv/set_io.hpp" #include "ur_msgs/srv/set_speed_slider_fraction.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" namespace ur_controllers { @@ -74,7 +76,7 @@ class GPIOController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update() override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp index 75b2f3282..a34bda045 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -27,6 +27,8 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" namespace ur_controllers { @@ -42,7 +44,7 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; - controller_interface::return_type update() override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; protected: struct TimeData diff --git a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp index ec9320490..c7c125657 100644 --- a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp +++ b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp @@ -31,6 +31,8 @@ #include "controller_interface/controller_interface.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" #include "std_msgs/msg/float64.hpp" namespace ur_controllers @@ -46,7 +48,7 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update() override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; @@ -58,7 +60,6 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte protected: std::vector sensor_names_; - rclcpp::Time last_publish_time_; double publish_rate_; std::shared_ptr> speed_scaling_state_publisher_; diff --git a/ur_controllers/src/force_torque_sensor_broadcaster.cpp b/ur_controllers/src/force_torque_sensor_broadcaster.cpp index 7f6063e5c..557999b3c 100644 --- a/ur_controllers/src/force_torque_sensor_broadcaster.cpp +++ b/ur_controllers/src/force_torque_sensor_broadcaster.cpp @@ -57,7 +57,8 @@ controller_interface::InterfaceConfiguration ForceTorqueStateBroadcaster::state_ return config; } -controller_interface::return_type ur_controllers::ForceTorqueStateBroadcaster::update() +controller_interface::return_type +ur_controllers::ForceTorqueStateBroadcaster::update(const rclcpp::Time& time, const rclcpp::Duration& /*period*/) { geometry_msgs::msg::Vector3 f_vec; geometry_msgs::msg::Vector3 t_vec; @@ -90,7 +91,7 @@ controller_interface::return_type ur_controllers::ForceTorqueStateBroadcaster::u } } - wrench_state_msg_.header.stamp = get_node()->get_clock()->now(); + wrench_state_msg_.header.stamp = time; wrench_state_msg_.header.frame_id = fts_params_.frame_id; // update wrench state message diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index 1352ad7b4..60e7a8a91 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -111,7 +111,8 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta return config; } -controller_interface::return_type ur_controllers::GPIOController::update() +controller_interface::return_type ur_controllers::GPIOController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { publishIO(); publishToolData(); diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index ea071560c..950d52eff 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -46,7 +46,8 @@ CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecyc return JointTrajectoryController::on_activate(state); } -controller_interface::return_type ScaledJointTrajectoryController::update() +controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, + const rclcpp::Duration& /*period*/) { if (state_interfaces_.back().get_name() == "speed_scaling") { scaling_factor_ = state_interfaces_.back().get_value(); @@ -132,7 +133,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update() // Main Speed scaling difference... // Adjust time with scaling factor TimeData time_data; - time_data.time = node_->now(); + time_data.time = time; rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds(); time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period); time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; @@ -185,7 +186,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update() if (active_goal) { // send feedback auto feedback = std::make_shared(); - feedback->header.stamp = node_->now(); + feedback->header.stamp = time; feedback->joint_names = joint_names_; feedback->actual = state_current; @@ -224,7 +225,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update() // TODO(anyone): This will break in speed scaling we have to discuss how to handle the goal // time when the robot scales itself down. - const double difference = node_->now().seconds() - traj_end.seconds(); + const double difference = time.seconds() - traj_end.seconds(); if (difference > default_tolerances_.goal_time_tolerance) { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); diff --git a/ur_controllers/src/speed_scaling_state_broadcaster.cpp b/ur_controllers/src/speed_scaling_state_broadcaster.cpp index 223283eb7..773f2f9b2 100644 --- a/ur_controllers/src/speed_scaling_state_broadcaster.cpp +++ b/ur_controllers/src/speed_scaling_state_broadcaster.cpp @@ -93,7 +93,6 @@ SpeedScalingStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*prev rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SpeedScalingStateBroadcaster::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { - last_publish_time_ = node_->now(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -103,15 +102,15 @@ SpeedScalingStateBroadcaster::on_deactivate(const rclcpp_lifecycle::State& /*pre return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -controller_interface::return_type SpeedScalingStateBroadcaster::update() +controller_interface::return_type SpeedScalingStateBroadcaster::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& period) { - if (publish_rate_ > 0.0 && (node_->now() - last_publish_time_) > rclcpp::Duration(1.0 / publish_rate_, 0.0)) { + if (publish_rate_ > 0.0 && period > rclcpp::Duration(1.0 / publish_rate_, 0.0)) { // Speed scaling is the only interface of the controller speed_scaling_state_msg_.data = state_interfaces_[0].get_value() * 100.0; // publish speed_scaling_state_publisher_->publish(speed_scaling_state_msg_); - last_publish_time_ = node_->now(); } return controller_interface::return_type::OK; }