Exemplo n.º 1
0
    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'
Exemplo n.º 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';
Exemplo n.º 3
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'
Exemplo n.º 4
0
    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'
Exemplo n.º 5
0
	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'
Exemplo n.º 6
0
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()
Exemplo n.º 7
0
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()
Exemplo n.º 8
0
	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'