diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h index 1c296eb82..c04679e0c 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h @@ -57,7 +57,17 @@ namespace velocity_controllers * Subscribes to: * - \b command (std_msgs::Float64MultiArray) : The joint velocities to apply */ -typedef forward_command_controller::ForwardJointGroupCommandController - JointGroupVelocityController; +class JointGroupVelocityController : public forward_command_controller::ForwardJointGroupCommandController +{ + using BaseClass = forward_command_controller::ForwardJointGroupCommandController; + bool received_; + ros::WallTimer watchdog_; + ros::Subscriber sub_command_; + void commandCB(const std_msgs::Float64MultiArrayConstPtr&); + void watchDogCB(const ros::WallTimerEvent& event); + +public: + bool init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &nh) override; +}; } diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index d6e64068f..6ab323bcc 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -45,5 +45,35 @@ void forward_command_controller::ForwardJointGroupCommandController::starting commands_buffer_.readFromRT()->assign(n_joints_, 0.0); } +namespace velocity_controllers +{ + bool JointGroupVelocityController::init(hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &nh) { + if (!BaseClass::init(hw, nh)) + return false; + + double timeout; + if (!nh.param("watchdog_timeout", timeout, 1.0)) + ROS_WARN_STREAM("No watchdog_timeout parameter, defaulting to " << timeout); + watchdog_ = nh.createWallTimer(ros::WallDuration(timeout), &JointGroupVelocityController::watchDogCB, this, false, false); + sub_command_ = nh.subscribe("command", 1, &JointGroupVelocityController::commandCB, this); + return true; + } + void JointGroupVelocityController::commandCB(const std_msgs::Float64MultiArrayConstPtr&) { + received_ = true; + watchdog_.start(); // (re)start watchdog timer + } + void JointGroupVelocityController::watchDogCB(const ros::WallTimerEvent&) { + if (!isRunning()) { + watchdog_.stop(); + } + else if (!received_) { + ROS_WARN("Didn't receive new velocity commands: stopping motion"); + std::vector zeros(n_joints_, 0.0); + commands_buffer_.writeFromNonRT(zeros); + watchdog_.stop(); // wait for message before starting again + } + received_ = false; + } +} PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase)