JointGroupVelocityController: stop motion if no commands received

ros-controls · Nov 17, 2023 · 9a9bcc2 · 9a9bcc2
1 parent 67b0e72
commit 9a9bcc2
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 2 deletions.
diff --git 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<hardware_interface::VelocityJointInterface>
-        JointGroupVelocityController;
+class JointGroupVelocityController : public forward_command_controller::ForwardJointGroupCommandController<hardware_interface::VelocityJointInterface>
+{
+  using BaseClass = forward_command_controller::ForwardJointGroupCommandController<hardware_interface::VelocityJointInterface>;
+  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
@@ -45,5 +45,35 @@ void forward_command_controller::ForwardJointGroupCommandController<T>::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<std_msgs::Float64MultiArray>("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<double> 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)