Forces the action to wait for half of the duration of the trajectory

before it begins to check for completition. This prevents the action
for returning immediately for trajectories that end near the start
point and are slow to start moving.

industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h

@@ -155,6 +155,12 @@ class JointTrajectoryAction
    */
   industrial_msgs::RobotStatusConstPtr last_robot_status_;
 
+  /**
+   * \brief Time at which to start checking for completion of current 
+   * goal, if one is active
+   */
+  ros::Time time_to_check_;
+
   /**
    * \brief The watchdog period (seconds)
    */

industrial_robot_client/src/joint_trajectory_action.cpp

@@ -143,6 +143,8 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
       gh.setAccepted();
       active_goal_ = gh;
       has_active_goal_ = true;
+      time_to_check_ = ros::Time::now() + 
+          ros::Duration(active_goal_.getGoal()->trajectory.points.back().time_from_start.toSec() / 2.0);
 
       ROS_INFO("Publishing trajectory");
 
@@ -225,6 +227,12 @@ void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTra
     return;
   }
 
+  if (ros::Time::now() < time_to_check_)
+  {
+    ROS_DEBUG("Waiting to check for goal completion until halfway through trajectory");
+    return;
+  }
+
   // Checking for goal constraints
   // Checks that we have ended inside the goal constraints and has motion stopped