From bcad8cf0a1d1a70375c3beb3b0cba504fb487eaa Mon Sep 17 00:00:00 2001
From: Shaun Edwards <shaun.edwards@gmail.com>
Date: Thu, 25 Jun 2015 17:38:34 -0500
Subject: [PATCH] Revert "Revert "Merge pull request #113 from
 simonschmeisser/indigo""

This reverts commit fb28e26fdbd3c316941d4d66af2f1c9b5410cb0d.
---
 .../joint_trajectory_action.h                 | 13 +++--
 .../src/joint_trajectory_action.cpp           | 51 +++++++++----------
 2 files changed, 30 insertions(+), 34 deletions(-)

diff --git a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h
index c1718698..158d5493 100644
--- a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h
+++ b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h
@@ -103,6 +103,11 @@ class JointTrajectoryAction
    */
   ros::Timer watchdog_timer_;
 
+  /**
+    * \brief Controller was alive during the last watchdog interval
+    */
+  bool controller_alive_;
+
   /**
    * \brief Indicates action has an active goal
    */
@@ -144,12 +149,6 @@ class JointTrajectoryAction
    */
   control_msgs::FollowJointTrajectoryFeedbackConstPtr last_trajectory_state_;
 
-  /**
-   * \brief Indicates trajectory state has been received.  Used by
-   * watchdog to determine if the robot driver is responding.
-   */
-  bool trajectory_state_recvd_;
-
   /**
    * \brief Cache of the last subscribed status message
    */
@@ -158,7 +157,7 @@ class JointTrajectoryAction
   /**
    * \brief The watchdog period (seconds)
    */
-  static const double WATCHD0G_PERIOD_;// = 1.0;
+  static const double WATCHDOG_PERIOD_;// = 1.0;
 
   /**
    * \brief Watch dog callback, used to detect robot driver failures
diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp
index aebfde70..6f73a0da 100644
--- a/industrial_robot_client/src/joint_trajectory_action.cpp
+++ b/industrial_robot_client/src/joint_trajectory_action.cpp
@@ -39,12 +39,13 @@ namespace industrial_robot_client
 namespace joint_trajectory_action
 {
 
-const double JointTrajectoryAction::WATCHD0G_PERIOD_ = 1.0;
+const double JointTrajectoryAction::WATCHDOG_PERIOD_ = 1.0;
 const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01;
 
 JointTrajectoryAction::JointTrajectoryAction() :
     action_server_(node_, "joint_trajectory_action", boost::bind(&JointTrajectoryAction::goalCB, this, _1),
-                   boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false)
+                   boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false),
+                       controller_alive_(false)
 {
   ros::NodeHandle pn("~");
 
@@ -62,7 +63,7 @@ JointTrajectoryAction::JointTrajectoryAction() :
   sub_trajectory_state_ = node_.subscribe("feedback_states", 1, &JointTrajectoryAction::controllerStateCB, this);
   sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this);
 
-  watchdog_timer_ = node_.createTimer(ros::Duration(WATCHD0G_PERIOD_), &JointTrajectoryAction::watchdog, this);
+  watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), &JointTrajectoryAction::watchdog, this, true);
   action_server_.start();
 }
 
@@ -82,34 +83,27 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
   {
     ROS_DEBUG("Waiting for subscription to joint trajectory state");
   }
-  if (!trajectory_state_recvd_)
-  {
-    ROS_DEBUG("Trajectory state not received since last watchdog");
-  }
+
+  ROS_WARN("Trajectory state not received for %f seconds", WATCHDOG_PERIOD_);
+  controller_alive_ = false;
+
 
   // Aborts the active goal if the controller does not appear to be active.
   if (has_active_goal_)
   {
-    if (!trajectory_state_recvd_)
+    // last_trajectory_state_ is null if the subscriber never makes a connection
+    if (!last_trajectory_state_)
     {
-      // last_trajectory_state_ is null if the subscriber never makes a connection
-      if (!last_trajectory_state_)
-      {
-        ROS_WARN("Aborting goal because we have never heard a controller state message.");
-      }
-      else
-      {
-        ROS_WARN_STREAM(
-            "Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ << " seconds");
-      }
-
-      abortGoal();
-
+      ROS_WARN("Aborting goal because we have never heard a controller state message.");
+    }
+    else
+    {
+      ROS_WARN_STREAM(
+          "Aborting goal because we haven't heard from the controller in " << WATCHDOG_PERIOD_ << " seconds");
     }
-  }
 
-  // Reset the trajectory state received flag
-  trajectory_state_recvd_ = false;
+    abortGoal();
+  }
 }
 
 void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
@@ -117,7 +111,7 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
   ROS_INFO("Received new goal");
 
   // reject all goals as long as we haven't heard from the remote controller
-  if (!trajectory_state_recvd_)
+  if (!controller_alive_)
   {
     ROS_ERROR("Joint trajectory action rejected: waiting for (initial) feedback from controller");
     control_msgs::FollowJointTrajectoryResult rslt;
@@ -216,9 +210,12 @@ void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle & gh)
 
 void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
 {
-  //ROS_DEBUG("Checking controller state feedback");
+  ROS_DEBUG("Checking controller state feedback");
   last_trajectory_state_ = msg;
-  trajectory_state_recvd_ = true;
+  controller_alive_ = true;
+
+  watchdog_timer_.stop();
+  watchdog_timer_.start();
 
   if (!has_active_goal_)
   {