Update end trajectory test to be timer-based

This switches how the joint trajectory action detects the end of a
trajectory. Previously, we would wait for the robot to be in-motion
then exit being in-motion. However, this caused stalling issues for
short trajectories. Now, we wait until the time expected to complete
the trajectory has passed and the robot is not in motion.

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h

@@ -116,9 +116,10 @@ class JointTrajectoryAction
   ros::Timer watchdog_timer_;
 
   /**
-   * \brief Indicates whether a motion has been started
+   * \brief The time that the active trajectory is expected to end execution.
+   * Used to determine when to begin checking for goal completion.
    */
-  bool motion_started_;
+  ros::Time expected_traj_end_;
 
   /**
    * \brief Indicates action has an active goal

motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp

@@ -89,7 +89,6 @@ JointTrajectoryAction::JointTrajectoryAction() :
                               boost::bind(&JointTrajectoryAction::watchdog, this, _1));
 
   has_active_goal_ = false;
-  motion_started_ = false;
 
   action_server_.start();
 }
@@ -102,7 +101,6 @@ void JointTrajectoryAction::robotStatusCB(
   const industrial_msgs::RobotStatusConstPtr &msg)
 {
   last_robot_status_ = msg;  // caching robot status for later use.
-  motion_started_ |= (msg->in_motion.val != industrial_msgs::TriState::FALSE);
 }
 
 void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
@@ -309,8 +307,9 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
     // Publishing the joint names for the 4 groups
     dyn_traj.joint_names = all_joint_names_;
 
-    motion_started_ = false;
     this->pub_trajectory_command_.publish(dyn_traj);
+
+    expected_traj_end_ = ros::Time::now() + gh.getGoal()->trajectory.points.back().time_from_start;
   }
   else
   {
@@ -400,9 +399,9 @@ void JointTrajectoryAction::controllerStateCB(
     return;
   }
 
-  if (!motion_started_)
+  if (ros::Time::now() < expected_traj_end_)
   {
-    ROS_DEBUG("Motion has not started, ignoring feedback");
+    ROS_DEBUG("Motion has not ended, ignoring feedback");
     return;
   }