diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h index a0f9e166a..e8783aada 100644 --- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h +++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h @@ -103,6 +103,13 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle } } + // Check if timeout parameter has been set. Read it if set, otherwise warn about unsafe default behaviour. + if(n.hasParam("command_timeout")) + { + n.getParam("command_timeout", command_timeout_); + ROS_INFO_STREAM("Using command timeout: " << command_timeout_); + } + commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0)); sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this); @@ -120,6 +127,8 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle std::vector< std::string > joint_names_; std::vector< hardware_interface::JointHandle > joints_; realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_; + realtime_tools::RealtimeBuffer<ros::Time> last_received_command_time_buffer_; + double command_timeout_ = -1.0; unsigned int n_joints_; private: @@ -132,6 +141,9 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle return; } commands_buffer_.writeFromNonRT(msg->data); + + // Record time of last received command + last_received_command_time_buffer_.writeFromNonRT(ros::Time::now()); } }; diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index d6e64068f..2668f9a39 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -38,12 +38,33 @@ #include <velocity_controllers/joint_group_velocity_controller.h> #include <pluginlib/class_list_macros.hpp> -template <class T> -void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time) +template<> +void velocity_controllers::JointGroupVelocityController::starting(const ros::Time& /*time*/) { // Start controller with 0.0 velocities commands_buffer_.readFromRT()->assign(n_joints_, 0.0); } +template<> +void velocity_controllers::JointGroupVelocityController::update(const ros::Time& time, const ros::Duration& /*period*/) +{ + std::vector<double> & commands = *commands_buffer_.readFromRT(); + + // Check timeout + if (command_timeout_ > 0.0) + { + ros::Time& last_received_command_time = *last_received_command_time_buffer_.readFromRT(); + const double command_age = (time - last_received_command_time).toSec(); + if (std::abs(command_age) > command_timeout_) + { + ROS_WARN_STREAM_THROTTLE(10, "Commands timed out, setting to zero."); + for (std::size_t i = 0; i < commands.size(); ++i) + { commands[i] = 0; } + } + } + + for(unsigned int i=0; i<n_joints_; i++) + { joints_[i].setCommand(commands[i]); } +} PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase)