Skip to content

Commit

Permalink
Merge pull request ros-industrial#119 from Jmeyer1292/issue_trajector…
Browse files Browse the repository at this point in the history
…y_tolerance_checks

Further Trajectory Completion Adjustments
  • Loading branch information
shaun-edwards committed Jul 28, 2015
2 parents c18645f + cc15909 commit f1e07f1
Show file tree
Hide file tree
Showing 2 changed files with 14 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -154,6 +154,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)
*/
Expand Down
8 changes: 8 additions & 0 deletions industrial_robot_client/src/joint_trajectory_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,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");

Expand Down Expand Up @@ -222,6 +224,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

Expand Down

0 comments on commit f1e07f1

Please sign in to comment.