def __init__(self, goal):
global positions
smach.State.__init__(self, outcomes=['reached', 'not_reached', 'failed'])
rospy.logdebug('Init state with %s'%str(goal))
if NO_MOVE_BASE:
return
##start movebasec client
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
rospy.loginfo("%s"%str(goal))
rospy.loginfo("%s"%str( positions))
pose = positions[goal]
pose['theta'] = dir_to_theta(pose['dir'])
self.goal_msg = create_goal_message(pose)
# todo check if data dictionary holds all relevant data
rospy.loginfo('Init state "%s"'%self.goal_msg)
self.counter = MAX_TRIES
def __init__(self, goal):
global positions
smach.State.__init__(self, outcomes=['reached', 'not_reached', 'failed'])
rospy.logdebug('Init state with %s'%str(goal))
if NO_MOVE_BASE:
return
##start movebasec client
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
rospy.loginfo("%s"%str(goal))
rospy.loginfo("%s"%str( positions))
pose = positions[goal]
pose['theta'] = dir_to_theta(pose['dir'])
self.goal_msg = create_goal_message(pose)
# todo check if data dictionary holds all relevant data
rospy.loginfo('Init state "%s"'%self.goal_msg)
self.counter = MAX_TRIES
def execute(self, userdata):
global positions
global cur_move_base_goal
#rospy.loginfo('Executing state "%s"'%self.label)
goal = self.goals[self.num_goal]
pose = positions[goal]
cur_move_base_goal = goal
pose['theta'] = dir_to_theta(pose['dir'])
self.goal_msg = create_goal_message(pose)
# todo check if data dictionary holds all relevant data
rospy.loginfo('Init state "%s"'%self.goal_msg)
if not NO_MOVE_BASE:
self.client.send_goal(self.goal_msg)
self.client.wait_for_result(rospy.Duration(TIME_OUT))
#rospy.logerr(self.client.get_state())
if NO_MOVE_BASE or self.client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("reached goals")
self.client.cancel_all_goals()
#rospy.sleep(self.goal['sleep'])
self.num_goal += 1
self.counter = MAX_TRIES
if self.num_goal == len(self.goals):
return 'end_reached'
else:
return 'reached'
else:
if self.counter > 0:
self.counter -= 1
self.client.cancel_all_goals()
return 'not_reached'
else:
self.num_goal += 1
self.counter = MAX_TRIES
self.client.cancel_all_goals()
if self.num_goal == len(self.goals):
return 'end_failed'
else:
return 'failed'