Esempio n. 1
0
    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()
Esempio n. 2
0
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()
Esempio n. 3
0
File: client.py Progetto: ruvu/hmi
    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)
Esempio n. 6
0
	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
Esempio n. 8
0
    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()