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):
#comment this at last
self.resources=Interaction()
#-------------
ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER, markerAction)
goal=markerGoal()
goal.order=ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result=ipClient.wait_for_result()
if ipClient.get_state()==GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
rospy.loginfo("Waiting for IP marker action server")
ipClient.wait_for_server()
goal = markerGoal()
goal.order = ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result = ipClient.wait_for_result()
if ipClient.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
def execute(self, ud):
self.resources = Interaction()
#do this job publish data to sensor controller
rospy.loginfo(
"marker detected .....setting vehicle point to marker calling %s",
header.RESET_POSE_SERVICE)
resetClient = rospy.ServiceProxy(header.RESET_POSE_SERVICE,
krakenResetPose)
msg = krakenResetPoseRequest()
resp = resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
self.ipClient.cancel_all_goals()
goal = markerGoal()
goal.order = header.ALLIGN_MARKER
self.ipClient.send_goal(goal,
done_cb=self.done_cb,
feedback_cb=self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg = krakenResetPoseRequest()
resp = resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
def execute(self, ud):
self.resources=Interaction()
#do this job publish data to sensor controller
rospy.loginfo("marker detected .....setting vehicle point to marker calling %s",header.RESET_POSE_SERVICE)
resetClient=rospy.ServiceProxy(header.RESET_POSE_SERVICE, krakenResetPose)
msg=krakenResetPoseRequest()
resp=resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,markerAction)
self.ipClient.cancel_all_goals()
goal=markerGoal()
goal.order=header.ALLIGN_MARKER
self.ipClient.send_goal(goal,done_cb= self.done_cb, feedback_cb= self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg=krakenResetPoseRequest()
resp=resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
