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"
