def execute(self, ud): #comment this at last #self.resources=Interaction() #------------- rospy.loginfo("waiting for buoy_detect Server") buoyClient=SimpleActionClient(header.BUOY_DETECT_ACTION_SERVER, buoyAction) buoyClient.wait_for_server() rospy.loginfo("connected to server") goal=buoyGoal() goal.order=ip_header.ALLIGN_BUOY buoyClient.send_goal(goal, feedback_cb= self.feedback_cb) result=buoyClient.wait_for_result(rospy.Duration(ud.time_out)) if not result: buoyClient.cancel_goal() if buoyClient.get_state()==GoalStatus.SUCCEEDED: rospy.loginfo("successfully aligned the vehicle with the gate ") return 'aligned' else: rospy.logerr("buoy not allogned and timed out before alliging it!!!") return 'timed_out'
def execute(self, ud): #comment this at last #self.resources=Interaction() #------------- rospy.loginfo("[TP]...Starting buoy detecting task...") """ This is a concurrence container containing the SimpleActionState for both controllerSevrer and buoyServer the outcome map specifices what to do when one of the states of cm reach a final outcome Everytime the controller goal is provided using input key of the state """ cm=smach.Concurrence(outcomes=['buoy_found','buoy_unfound','time_out'], default_outcome='buoy_unfound', outcome_map={ 'buoy_found':{'DETECT_SERVER':'succeeded'}, 'time_out':{'CONTROLLER_SERVER':'succeeded'} }, input_keys=['u_r','u_p','u_y','u_d']) cm.userdata.u_r=None cm.userdata.u_p=None cm.userdata.u_y=None cm.userdata.u_d=None with cm: goalb=buoyGoal() goalb.order=ip_header.ALLIGN_BUOY smach.Concurrence.add('DETECT_SERVER', SimpleActionState(header.BUOY_DETECT_ACTION_SERVER, buoyAction, goal)) smach.Concurrence.add('CONTROLLER_SERVER', SimpleActionState(header.CONTROLLER_SERVER, controllerAction, goal_slots=['r','p','y','d']), remapping={ 'r':'u_r','p':'u_p','y':'u_y','d':'u_d' } ) """ activating the buoy and going to a specific depth """ cm.userdata.u_r=self.resources.pose[header.PoseEnum.roll] cm.userdata.u_d=self.tDepth cm.userdata.u_p=self.resources.pose[header.PoseEnum.pitch] cm.userdata.u_y=self.resources.pose[header.PoseEnum.yaw] rospy.loginfo("moving to depth %f",self.tDepth) outcome=cm.execute() if(outcome=='time_out'): rospy.loginfo("succesfully reached depth %f ",self.tDepth) elif outcome=='buoy_found': rospy.loginfo("Found buoy while reaching depth %d !!",self.tDepth) return 'buoy_found' """ now setting yaw -45 degrees and checking for buoy within this range """ cm.userdata.u_r=self.resources.pose[header.PoseEnum.roll] cm.userdata.u_d=self.resources.pose[self.tDepth] cm.userdata.u_p=self.resources.pose[header.PoseEnum.pitch] cm.userdata.u_y=-45 rospy.loginfo("turning --45 degrees left ") outcome=cm.execute() if(outcome=='time_out'): rospy.loginfo("completed turning 90 degrees and buoy not found ") elif outcome=='buoy_found': rospy.loginfo("Found buoy while reaching -45%") return 'buoy_found' cm.userdata.u_r=self.resources.pose[header.PoseEnum.roll] cm.userdata.u_d=self.resources.pose[self.tDepth] cm.userdata.u_p=self.resources.pose[header.PoseEnum.pitch] cm.userdata.u_y=90 rospy.loginfo("turning 90 degrees right ") outcome=cm.execute() if(outcome=='time_out'): rospy.loginfo("completed turning 90 degrees and buoy not found ") return 'time_out' elif outcome=='buoy_found': rospy.loginfo("Found buoy while reaching 90 degrees") return 'buoy_found'