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..cc332b67 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 @@ -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) */ diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp index aae399dc..f1a35df6 100644 --- a/industrial_robot_client/src/joint_trajectory_action.cpp +++ b/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