Exemple #1
0
	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';
Exemple #2
0
    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'
Exemple #3
0
	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'
Exemple #4
0
 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'
Exemple #5
0
    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'
Exemple #6
0
	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'