def _wait_for_result_and_get(self, timeout=None):
execute_timeout = rospy.Duration(timeout) if timeout else rospy.Duration(10)
preempt_timeout = rospy.Duration(1)
while not self._client.wait_for_result(execute_timeout):
if not self._feedback:
# preempt action
rospy.logdebug("Canceling goal")
self._client.cancel_goal()
if self._client.wait_for_result(preempt_timeout):
rospy.loginfo("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
else:
rospy.logwarn("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
break
else:
self._feedback = False
rospy.loginfo("I received feedback, let's wait another %.2f seconds" % execute_timeout.to_sec())
state = self._client.get_state()
if state != GoalStatus.SUCCEEDED:
if state == GoalStatus.PREEMPTED:
# Timeout
_print_timeout()
raise TimeoutException("Goal did not succeed within the time limit")
else:
_print_generic_failure()
raise Exception("Goal did not succeed, it was: %s" % GoalStatus.to_string(state))
return self._client.get_result()
def action_client_generator(client, goal):
assert client.simple_state == SimpleGoalState.DONE
client.send_goal(goal)
try:
while True:
yield
status = client.get_state()
if status in (GoalStatus.ACTIVE, GoalStatus.PENDING):
continue
elif status == GoalStatus.SUCCEEDED:
break
elif status == GoalStatus.ABORTED:
raise ActionAborted(client.get_goal_status_text())
elif status == GoalStatus.PREEMPTED:
raise ActionPreempted(client.get_goal_status_text())
else:
raise ActionFailed(GoalStatus.to_string(status))
finally:
client.cancel_goal()
def _wait_for_result_and_get(self, timeout=None):
execute_timeout = rospy.Duration(
timeout) if timeout else rospy.Duration(10)
preempt_timeout = rospy.Duration(1)
while not self._client.wait_for_result(execute_timeout):
if not self._feedback:
# preempt action
rospy.logdebug("Canceling goal")
self._client.cancel_goal()
if self._client.wait_for_result(preempt_timeout):
rospy.loginfo(
"Preempt finished within specified preempt_timeout [%.2f]",
preempt_timeout.to_sec())
else:
rospy.logwarn(
"Preempt didn't finish specified preempt_timeout [%.2f]",
preempt_timeout.to_sec())
break
else:
self._feedback = False
rospy.loginfo(
"I received feedback, let's wait another %.2f seconds" %
execute_timeout.to_sec())
state = self._client.get_state()
if state != GoalStatus.SUCCEEDED:
if state == GoalStatus.PREEMPTED:
# Timeout
_print_timeout()
raise TimeoutException(
"Goal did not succeed within the time limit")
else:
_print_generic_failure()
raise Exception("Goal did not succeed, it was: %s" %
GoalStatus.to_string(state))
return self._client.get_result()
def get_status_string(status_code):
return GoalStatus.to_string(status_code)
def get_status_string(status_code):
return GoalStatus.to_string(status_code)
def trigger_action(self,component_name,action_name,blocking=True):
ah = action_handle(action_name, component_name, "", blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="action")
rospy.loginfo("<<%s>> <<%s>>", action_name, component_name)
#parameter_name = self.ns_global_prefix + "/" + component_name + "/service_ns"
#if not rospy.has_param(parameter_name):
#message = "Parameter " + parameter_name + " does not exist on ROS Parameter Server, aborting..."
#rospy.logerr(message)
#ah.set_failed(2, message)
#return ah
#service_ns_name = rospy.get_param(parameter_name)
action_full_name = "/" + component_name + "/" + action_name
action_client = actionlib.SimpleActionClient(action_full_name, TestAction)
if blocking:
# check if action is available
if not action_client.wait_for_server(rospy.Duration(1)):
message = "ActionServer %s is not running"%action_full_name
rospy.logerr(message)
ah.set_failed(4, message)
return ah
# call the action
if blocking:
rospy.loginfo("Wait for <<%s>> to <<%s>>...", component_name, action_name)
goal_state = action_client.send_goal_and_wait(TestGoal())
if not (action_client.get_state() == GoalStatus.SUCCEEDED):
message = "...state of <<" + action_name + ">> of <<" + component_name + ">> : " + GoalStatus.to_string(action_client.get_state())
rospy.logerr(message)
ah.set_failed(10, message)
return ah
else:
action_client.send_goal(TestGoal())
# full success
rospy.loginfo("...<<%s>> is <<%s>>", component_name, action_name)
ah.set_succeeded() # full success
return ah
def string_action(self,component_name, data, blocking):
action_name = "set_string"
ah = action_handle(action_name, component_name, "", blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="action")
rospy.loginfo("<<%s>> <<%s>>", action_name, component_name)
action_full_name = "/" + component_name + "/" + action_name
action_client = actionlib.SimpleActionClient(action_full_name, SetStringAction)
if blocking:
# check if action is available
if not action_client.wait_for_server(rospy.Duration(1)):
message = "ActionServer %s is not running"%action_full_name
rospy.logerr(message)
ah.set_failed(4, message)
return ah
# call the action
goal = SetStringGoal()
goal.data = data
if blocking:
rospy.loginfo("Wait for <<%s>> to <<%s>>...", component_name, action_name)
goal_state = action_client.send_goal_and_wait(goal)
if not (action_client.get_state() == GoalStatus.SUCCEEDED):
message = "...state of <<" + action_name + ">> of <<" + component_name + ">> : " + GoalStatus.to_string(action_client.get_state())
rospy.logerr(message)
ah.set_failed(10, message)
return ah
else:
action_client.send_goal(goal)
# full success
rospy.loginfo("...<<%s>> is <<%s>>", component_name, action_name)
ah.set_succeeded() # full success
return ah
try:
wait_for_clock()
client = play_motion_client(play_motion_ns)
except MoveJointException, e:
print_err(str(e))
def cancel(signum, frame):
client.cancel_goal()
rospy.signal_shutdown("goal canceled")
signal.signal(signal.SIGINT, cancel)
load_motion(motion_ns, motion_data)
goal = PlayMotionGoal(motion_name=motion_data.motion_name,
skip_planning=True)
client.send_goal(goal, None, active_cb)
client.wait_for_result()
unload_motion(motion_ns, motion_data.motion_name)
al_res = client.get_state()
pm_res = client.get_result()
if al_res != GoalStatus.SUCCEEDED or pm_res.error_code != PlayMotionResult.SUCCEEDED:
print_err("Execution failed with status {}. {}".format(
GoalStatus.to_string(al_res), pm_res.error_string))
sys.exit(1)
if __name__ == "__main__":
move_joint()
