diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp index ed364a25..b1503a2d 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -80,6 +81,10 @@ class TwistController : public controller_interface::ControllerInterface { void configure_joint_names_(); void configure_twist_impl_(); + // some safety checks + std::atomic updates_since_last_command_ = 0; + double max_time_without_command_ = 0.2; + // joint veloctiy computation std::unique_ptr twist_impl_ptr_; lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_; diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index e01d03e7..f93f0981 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -66,6 +66,7 @@ controller_interface::CallbackReturn TwistController::on_init() { twist_subscription_ptr_ = this->get_node()->create_subscription( "command/twist", 1, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { rt_twist_ptr_.writeFromNonRT(msg); + updates_since_last_command_ = 0; }); this->get_node()->declare_parameter("robot_name", "lbr"); this->get_node()->declare_parameter("chain_root", "lbr_link_0"); @@ -85,7 +86,7 @@ controller_interface::CallbackReturn TwistController::on_init() { } controller_interface::return_type TwistController::update(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { + const rclcpp::Duration &period) { auto twist_command = rt_twist_ptr_.readFromRT(); if (!twist_command || !(*twist_command)) { return controller_interface::return_type::OK; @@ -98,6 +99,13 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { return controller_interface::return_type::OK; } + if (updates_since_last_command_ > + static_cast(max_time_without_command_ / period.seconds())) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "No twist command received within time %f. Stopping the controller.", + max_time_without_command_); + return controller_interface::return_type::ERROR; + } // pass joint positions to q_ std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { @@ -115,6 +123,8 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / ++i; }); + ++updates_since_last_command_; + return controller_interface::return_type::OK; }