def execute(self, userdata): global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, listener, waypointsGPS statePub.publish(String("ReturnHomeGPS")) stateInterPub.publish(Int8(10)) servStartDiag() rospy.loginfo("ReturnHomeGPS") # need restart algo ? # start algo deplacement # needed PKG mavros pwm_serial_send algo # launch GPS algo plus evitement obstacle # wait for GPS waipoint(s) skm = StartKillMsg() skm.action = 1 skm.type = 1 skm.nId = 1 startKillPub.publish(skm) # start algo deplacement rospy.sleep(2) Lat = 42.954306 Long = 10.599778 r = rospy.Publisher("start_gps_follow", Int8) result = WS.waitForGPSData(self, 2 * 60) if result == "preempted": return "preempted" elif result == "Error" and waypointGPS == 0: err = ErrorMessage() err.type1 = 3 selfErrorPub.publish(err) rospy.sleep(1) if self.preempt_requested(): rospy.loginfo("Go building GPS is being preempted") self.service_preempt() return "preempted" elif waypointGPS != 0: text for i in waypointsGPS[::-1]: text = i + "@" text = text[:-1] temp = rospy.Publisher("gps_string", String) temp.publish(String(txt)) temp.unregister() else: Lat = result[0][0] # last coord of list of waypoints Long = result[0][1] r.publish(Int8(1)) # algo follow can really start msgSRV = IsNearRequest() msgSRV.type1 = 0 msgSRV.type2 = 1 msgSRV.xLont1 = 11 msgSRV.yLat1 = 45 msgSRV.xLont2 = Long msgSRV.yLat2 = Lat msgSRV.threshold = 1 res = is_near_srv(msgSRV).response while not res: # wait for GPS for 5 min and check when we are in range if self.preempt_requested(): r.publish(Int8(0)) r.unregister() rospy.loginfo("Go building GPS is being preempted") self.service_preempt() return "preempted" try: (trans1, rot1) = listener.lookupTransform("local_origin", "fcu", rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "try" msgSRV.xLont1 = trans1[0] msgSRV.yLat1 = trans1[1] res = is_near_srv(msgSRV).response # if end algo change algo to direct one r.publish(Int8(0)) r.unregister() WS.sendCommand(1500, 1500) servStartDiag() skm.action = 0 skm.type = 1 skm.nId = 1 startKillPub.publish(skm) # kill gps_follow return "endReturnHomeGPS"
def execute(self, userdata): global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, listener, startKillPub, result, info_state_for_emer, waypointsGPS info_state_for_emer = 0 statePub.publish(String("GoBuildingGPS")) stateInterPub.publish(Int8(1)) servStartDiag() rospy.loginfo("GoBuildingGPS") # launch GPS algo plus evitement obstacle # launch algo colour movie recorder # needed PKG gps algo move color detection mavros pwm_serial_send Lat = 42.954306 Long = 10.599778 # Start gps follow car skm = StartKillMsg() skm.action = 1 skm.type = 1 skm.nId = 1 startKillPub.publish(skm) rospy.sleep(2) r = rospy.Publisher("start_gps_follow", Int8) # wait for GPS waipoint(s) result = WS.waitForGPSData(self, 2 * 60) if result == "preempted": return "preempted" elif result == "Error": err = ErrorMessage() err.type1 = 3 selfErrorPub.publish(err) rospy.sleep(1) if self.preempt_requested(): rospy.loginfo("Go building GPS is being preempted") self.service_preempt() # Kill gps follow car skm = StartKillMsg() skm.action = 0 skm.type = 1 skm.nId = 1 startKillPub.publish(skm) return "preempted" else: Lat = result[0][0] # last coord of list of waypoints Long = result[0][1] waypointsGPS = result[1] r.publish(Int8(1)) msgSRV = IsNearRequest() msgSRV.type1 = 0 msgSRV.type2 = 1 msgSRV.xLont1 = 11 msgSRV.yLat1 = 45 msgSRV.xLont2 = Long msgSRV.yLat2 = Lat msgSRV.threshold = 1 res = is_near_srv(msgSRV).response while not res: # check arrive on place if self.preempt_requested(): r.publish(Int8(0)) r.unregister() rospy.loginfo("Go building GPS is being preempted") self.service_preempt() # Kill gps follow car skm = StartKillMsg() skm.action = 0 skm.type = 1 skm.nId = 1 startKillPub.publish(skm) return "preempted" try: (trans1, rot1) = listener.lookupTransform("local_origin", "fcu", rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue msgSRV.xLont1 = trans1[0] msgSRV.yLat1 = trans1[1] res = is_near_srv(msgSRV).response # if end algo change algo to direct one r.publish(Int8(0)) r.unregister() servStartDiag() # Kill gps follow car skm = StartKillMsg() skm.action = 0 skm.type = 1 skm.nId = 1 startKillPub.publish(skm) WS.sendCommand(1500, 1500) return "endGoBuilding"