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'