From cc15909f551b766dedc17256041601157b0b55e0 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Mon, 13 Jul 2015 09:18:12 -0500 Subject: [PATCH] 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/joint_trajectory_action.h | 6 ++++++ industrial_robot_client/src/joint_trajectory_action.cpp | 8 ++++++++ 2 files changed, 14 insertions(+) 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 158d5493..81114358 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 @@ -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) */ diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp index b424a27d..e60ec005 100644 --- a/industrial_robot_client/src/joint_trajectory_action.cpp +++ b/industrial_robot_client/src/joint_trajectory_action.cpp @@ -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"); @@ -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