def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
goal = advancedControllerGoal()
goal.flag = 0
goal.x = ud.x
goal.y = ud.y
goal.depth = ud.depth
self.resources.advancedControllerClient.cancel_all_goals()
self.resources.advancedControllerClient.send_goal(goal)
rospy.loginfo("moving to next marker at (%f, %f) and to depth %f",
ud.x, ud.y, ud.depth)
result = self.resources.advancedControllerClient.wait_for_result(
rospy.Duration(20))
print result
if self.resources.advancedControllerClient.get_state(
) == GoalStatus.SUCCEEDED:
rospy.loginfo("successfully moved to destination")
return 'reached'
else:
rospy.logerr("timed out!! while moving to destination")
return 'time_out'
def execute(self, ud):
rospy.loginfo("Starting next task....")
cm_movingToPoint=smach.Concurrence(outcomes=['marker_detected','marker_undetected','time_out'],
default_outcome='marker_undetected',
outcome_map={'marker_detected':{
'MARKER_DETECT':'succeeded'
},
'time_out':{
'CONTROLLER':'succeeded'
}
})
moveGoal=advancedControllerGoal()
moveGoal.x=ud.x2
moveGoal.y=ud.y2
moveGoal.depth=ud.depth
markGoal=markerGoal()
markGoal.order=header.DETECT_MARKER
with cm_movingToPoint:
smach.Concurrence.add('CONTROLLER',
SimpleActionState("advancedController",
advancedControllerAction ,
goal=moveGoal))
smach.Concurrence.add('MARKER_DETECT',
SimpleActionState("marker_detect",
goal=markGoal))
return 'done';
def execute(self, ud):
rospy.loginfo("Starting next task....")
cm_movingToPoint = smach.Concurrence(
outcomes=['marker_detected', 'marker_undetected', 'time_out'],
default_outcome='marker_undetected',
outcome_map={
'marker_detected': {
'MARKER_DETECT': 'succeeded'
},
'time_out': {
'CONTROLLER': 'succeeded'
}
})
moveGoal = advancedControllerGoal()
moveGoal.x = ud.x2
moveGoal.y = ud.y2
moveGoal.depth = ud.depth
markGoal = markerGoal()
markGoal.order = header.DETECT_MARKER
with cm_movingToPoint:
smach.Concurrence.add(
'CONTROLLER',
SimpleActionState("advancedController",
advancedControllerAction,
goal=moveGoal))
smach.Concurrence.add(
'MARKER_DETECT',
SimpleActionState("marker_detect", goal=markGoal))
return 'done'
def execute(self, ud):
#do necessary stuff for moving in some path
client = self.resources.advancedControllerClient
goal = advancedControllerGoal()
goal.depth = self.resources.depth
goal.x = self.resources.pose[header.PoseEnum.x]
goal.y = -0.1
client.send_goal_and_wait(goal)
#do necessary stuff for moving in some path
return 'found'
def execute(self, ud): #do necessary stuff for moving in some path client=self.resources.advancedControllerClient goal= advancedControllerGoal() goal.depth=self.resources.depth goal.x=self.resources.pose[header.PoseEnum.x] goal.y=-0.1 client.send_goal_and_wait(goal) #do necessary stuff for moving in some path return 'found'
def main():
# sm=smach.StateMachine(outcomes=['failed'])
# sm.userdata.x=None;
# with sm:
# sm.add('INIT', initialState(), transitions={'initalized':'failed','failed':'fialed'});
rospy.init_node(name="interaction", anonymous=False)
cin = Interaction()
goal2 = advancedControllerGoal()
goal2.flag = 1
goal2.x = 13
goal2.y = 20
cin.advancedControllerClient.send_goal_and_wait(goal2)
rospy.spin()
def main():
# sm=smach.StateMachine(outcomes=['failed'])
# sm.userdata.x=None;
# with sm:
# sm.add('INIT', initialState(), transitions={'initalized':'failed','failed':'fialed'});
rospy.init_node(name="interaction", anonymous=False)
cin=Interaction()
goal2=advancedControllerGoal()
goal2.flag=1
goal2.x=13
goal2.y=20
cin.advancedControllerClient.send_goal_and_wait(goal2)
rospy.spin()
def execute(self, ud):
#comment this at last
self.resources=Interaction()
#-------------
goal=advancedControllerGoal()
goal.x=ud.x
goal.y=ud.y
goal.depth=ud.depth
self.resources.advancedControllerClient.cancel_all_goals();
self.resources.advancedControllerClient.send_goal(goal)
rospy.loginfo("moving to next marker at (%f, %f) and to depth %f",ud.x,ud.y,ud.depth)
result=self.resources.advancedControllerClient.wait_for_result(ud.time_out)
if self.resources.advancedControllerClient.get_state()==GoalStatus.SUCCEEDED:
rospy.loginfo("successfully moved to destination")
return 'reached'
else:
rospy.logerr("timed out!! while moving to destination")
return 'time_out'
