diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 4b398c9c86..38f767dd3b 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -61,7 +61,8 @@ class DiffDriveController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; DIFF_DRIVE_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; DIFF_DRIVE_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 40d0421856..e816b26f11 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -132,7 +132,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons return {interface_configuration_type::INDIVIDUAL, conf_names}; } -controller_interface::return_type DiffDriveController::update() +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) @@ -145,29 +146,29 @@ controller_interface::return_type DiffDriveController::update() return controller_interface::return_type::OK; } - const auto current_time = node_->get_clock()->now(); + const auto current_time = time; - std::shared_ptr last_msg; - received_velocity_msg_ptr_.get(last_msg); + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); - if (last_msg == nullptr) + if (last_command_msg == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto dt = current_time - last_msg->header.stamp; + const auto age_of_last_command = current_time - last_command_msg->header.stamp; // Brake if cmd_vel has timeout, override the stored command - if (dt > cmd_vel_timeout_) + if (age_of_last_command > cmd_vel_timeout_) { - last_msg->twist.linear.x = 0.0; - last_msg->twist.angular.z = 0.0; + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; } // linear_command and angular_command may be limited further by SpeedLimit, // without affecting the stored twist command - double linear_command = last_msg->twist.linear.x; - double angular_command = last_msg->twist.angular.z; + double linear_command = last_command_msg->twist.linear.x; + double angular_command = last_command_msg->twist.angular.z; // Apply (possibly new) multipliers: const auto wheels = wheel_params_; @@ -247,7 +248,7 @@ controller_interface::return_type DiffDriveController::update() angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); previous_commands_.pop(); - previous_commands_.emplace(*last_msg); + previous_commands_.emplace(*last_command_msg); // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 40320248d4..ca3da08803 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -285,11 +285,15 @@ TEST_F(TestDiffDriveController, cleanup) publish(linear, angular); controller_->wait_for_twist(executor); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); state = controller_->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + 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(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -333,7 +337,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) // wait for msg is be published to the system ASSERT_TRUE(controller_->wait_for_twist(executor)); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); @@ -342,7 +348,9 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) std::this_thread::sleep_for(std::chrono::milliseconds(500)); state = controller_->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 3864aef639..157384423a 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -114,7 +114,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -127,7 +129,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); @@ -147,7 +151,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -162,7 +168,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -201,7 +209,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index f1f4d1e8bf..a1c0be6335 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -59,7 +59,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::string sensor_name_; 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 5f6fc7ec0b..5f5c880c39 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -156,11 +156,12 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update() +controller_interface::return_type ForceTorqueSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (realtime_publisher_ && realtime_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = node_->now(); + realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); realtime_publisher_->unlockAndPublish(); } 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 3d61eb5490..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 @@ -88,7 +88,9 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -220,7 +222,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) @@ -234,7 +238,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(fts_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index bbbee94740..9926a7e9b7 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -69,7 +69,8 @@ class ForwardCommandController : public controller_interface::ControllerInterfac CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::vector joint_names_; diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 7107cd03ce..49284ae5cb 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -153,7 +153,8 @@ CallbackReturn ForwardCommandController::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type ForwardCommandController::update() +controller_interface::return_type ForwardCommandController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto joint_commands = rt_command_ptr_.readFromRT(); diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index e42fe6e61a..425c08c7af 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -178,7 +178,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -191,7 +193,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); @@ -215,7 +219,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -233,7 +239,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -274,7 +282,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index a980ca5388..c041fb2de1 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -88,7 +88,8 @@ class GripperActionController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; GRIPPER_ACTION_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; GRIPPER_ACTION_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index cff250d1f6..211e3cd4d5 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -61,7 +61,8 @@ CallbackReturn GripperActionController::on_init() } template -controller_interface::return_type GripperActionController::update() +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { command_struct_rt_ = *(command_.readFromRT()); diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 50607fcab5..9a736d5cfb 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -56,7 +56,8 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; IMU_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::string sensor_name_; diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index cb5e3d6f0b..23aa33777c 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -112,11 +112,12 @@ CallbackReturn IMUSensorBroadcaster::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type IMUSensorBroadcaster::update() +controller_interface::return_type IMUSensorBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (realtime_publisher_ && realtime_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = node_->now(); + realtime_publisher_->msg_.header.stamp = time; imu_sensor_->get_values_as_message(realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 3edc555dd7..d27be76b3b 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -90,7 +90,9 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & "/test_imu_sensor_broadcaster/imu", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -171,7 +173,9 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(imu_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index ff55d0210e..175c3610f6 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -44,7 +44,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; JOINT_STATE_BROADCASTER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; JOINT_STATE_BROADCASTER_PUBLIC CallbackReturn on_init() override; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 9bc089085f..c5dbcc4c82 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -229,7 +229,8 @@ double get_value( } } -controller_interface::return_type JointStateBroadcaster::update() +controller_interface::return_type JointStateBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { for (const auto & state_interface : state_interfaces_) { @@ -240,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 = time; + dynamic_joint_state_msg_.header.stamp = time; // 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 5bf3d9dc3d..f38c108589 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -184,7 +184,9 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) auto node_state = state_broadcaster_->configure(); node_state = state_broadcaster_->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) @@ -200,7 +202,9 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st auto subscription = test_node.create_subscription(topic, 10, subs_callback); - ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -250,7 +254,9 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( auto subscription = test_node.create_subscription(topic, 10, subs_callback); - ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 9e66c35073..786ef42b0b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -81,7 +81,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::InterfaceConfiguration state_interface_configuration() const override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; JOINT_TRAJECTORY_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3e3f11f605..843ce78459 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -105,7 +105,8 @@ JointTrajectoryController::state_interface_configuration() const return conf; } -controller_interface::return_type JointTrajectoryController::update() +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) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 65e756a4d2..5bb6efaea6 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -94,7 +94,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest auto end_time = start_time + wait; while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); + traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); } }); @@ -448,7 +448,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) const double prev_pos3 = joint_pos_[2]; // run an update, it should be holding - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_EQ(prev_pos1, joint_pos_[0]); EXPECT_EQ(prev_pos2, joint_pos_[1]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d8896601c1..0a992b14f7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -79,7 +79,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure) publish(time_from_start, points); std::this_thread::sleep_for(std::chrono::milliseconds(10)); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // hw position == 0 because controller is not activated EXPECT_EQ(0.0, joint_pos_[0]); @@ -144,7 +144,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // executor.spin_once(); // -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // change in hw position @@ -192,7 +192,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // std::this_thread::sleep_for(std::chrono::milliseconds(500)); // executor.spin_once(); // -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // deactivated @@ -201,7 +201,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // state = traj_controller_->deactivate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // resource_manager_->read(); -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // no change in hw position @@ -215,7 +215,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // state = traj_node->activate(); // ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // resource_manager_->read(); -// traj_controller->update(); +// traj_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // resource_manager_->write(); // // // change in hw position to 3rd point @@ -241,11 +241,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) std::vector> points{{{3.3, 4.4, 5.5}}}; publish(time_from_start, points); traj_controller_->wait_for_trajectory(executor); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); auto state = traj_controller_->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); state = traj_controller_->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -292,11 +292,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->wait_for_trajectory(executor); // first update - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // wait so controller process the second point when deactivated std::this_thread::sleep_for(FIRST_POINT_TIME); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // deactivated state = traj_controller_->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -313,11 +313,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param state = traj_controller_->cleanup(); // update loop receives a new msg and updates accordingly - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check the traj_msg_home_ptr_ initialization code for the standard wait timing std::this_thread::sleep_for(std::chrono::milliseconds(50)); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], allowed_delta); @@ -404,7 +404,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou const auto end_time = start_time + wait; while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); } // We may miss the last message since time allowed is exactly the time needed @@ -832,15 +832,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // Expect backward commands at first EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); @@ -859,15 +859,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // Expect backward commands at first EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); @@ -916,15 +916,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // There should not be backward commands EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -943,15 +943,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // There should not be a jump toward commands EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); EXPECT_LT(joint_pos_[0], second_goal[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5224a64aeb..8ece5c393a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -303,7 +303,7 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; while (rclcpp::Clock().now() < end_time) { - traj_controller_->update(); + traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); } } diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 2c8ddecb7f..be774031f0 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -114,7 +114,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -127,7 +129,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); @@ -147,7 +151,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -162,7 +168,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); @@ -201,7 +209,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index d2c6cbc57a..b566829f9b 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -114,7 +114,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -127,7 +129,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update successful, command received - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands have been modified ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); @@ -147,7 +151,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_->rt_command_ptr_.writeFromNonRT(command_ptr); // update failed, command size does not match number of joints - ASSERT_EQ(controller_->update(), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -162,7 +168,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check joint commands are still the default ones ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); @@ -201,7 +209,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); // update successful - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check command in handle was set ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);