def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData
WS.sendCommand(1500, 1500)
servStartDiag()
statePub.publish(String("EmergencyStopEntry"))
rospy.loginfo("EmergencyStopEntry")
WS.waitForRemoteGo(60 * 60)
return "endEmergPrep" # we have to go back to make sur algos are good
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData
WS.sendCommand(1500, 1500)
servStartDiag()
rospy.loginfo("EmergencyStopExitBuilding")
msg = WS.waitForRemoteGo(60 * 60)
if msg.data:
return "endEmer_End"
else:
return "endEmer_Exit"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv
statePub.publish(String("ExitBuildingTeleOp"))
stateInterPub.publish(Int8(8))
# start astar path and driving algo too
servStartDiag()
rospy.loginfo("ExitBuildingTeleOp")
WS.waitForRemoteGo(15 * 60)
# needed PKG pwm_serial_send mavros gps ?
servStartDiag()
WS.sendCommand(1500, 1500)
return "endExitBuilding"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, cartoExitMode, firstTime
WS.sendCommand(1500, 1500)
servStartDiag()
statePub.publish(String("EmergencyStopInt"))
rospy.loginfo("EmergencyStopInt")
# check algo state
# needed PKG sauvegarde
# save = rospy.ServiceProxy("/save_shot",Save_inst_srv)
# save(Save_inst_srv(True))
result = WS.waitForRemoteGo(60 * 60)
if result.data:
return "endEmer_Carto"
return "endEmer_Exit"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData
WS.sendCommand(1500, 1500)
servStartDiag()
rospy.loginfo("EmergencyStopReturn")
msg = WS.waitForRemoteGo(60 * 60)
if msg.data:
return "endEmergEnd"
else:
if state == 0:
return "endEmergGPS"
elif state == 1:
return "endEmergGPSArdu"
else:
return "endEmergTeleOp"
return "endEmergTeleOp"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, cartoExitMode
statePub.publish(String("ProceduralStop"))
stateInterPub.publish(Int8(6))
WS.sendCommand(1500, 1500)
if not cartoExitMode or self.preempt_requested():
self.service_preempt()
return "preempted"
servStartDiag()
rospy.loginfo("ProceduralStop")
# save = rospy.ServiceProxy("/save_shot",Save_inst_srv)
# save(Save_inst_srv(True))
if self.preempt_requested():
rospy.loginfo("ProceduralStop is being preempted")
self.service_preempt()
return "preempted"
servStartDiag()
return "endProceduralStop"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, info_state_for_emer
WS.sendCommand(1500, 1500)
servStartDiag()
statePub.publish(String("EmergencyStopGo"))
rospy.loginfo("EmergencyStopGo")
msg = WS.waitForRemoteGo(60 * 60)
if msg.data:
return "endEmergPrepEntry"
else:
if state == 0:
return "endEmergGPS"
elif state == 1:
return "endEmergGPSArdu"
else:
return "endEmergTeleOp"
return "endEmergTeleOp"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, listener
statePub.publish(String("ReturnHomeArdu"))
stateInterPub.publish(Int8(11))
servStartDiag()
rospy.loginfo("ReturnHomeArdu")
# need restart algo ?
# start algo deplacement
# needed PKG mavros
# wait for GPS waipoint(s)
# tell ardu GPS waypoint
# launch algo colour movie recorder
Lat = 10, 600 # last coord
Long = 42, 4555
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
if self.preempt_requested():
rospy.loginfo("Return Home GPS Ardu 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
# stop autonomous ardu
WS.sendCommand(1500, 1500)
servStartDiag()
return "endReturnHomeArdu"
def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, listener, info_state_for_emer
info_state_for_emer = 1
rospy.loginfo("GoBuilfingGPSArdu")
statePub.publish(String("GoBuildingGPSArdu"))
stateInterPub.publish(Int8(2))
servStartDiag()
# wait for GPS waipoint(s)
# tell ardu GPS waypoint
# launch algo colour movie recorder
Long = 10, 600 # last coord
Lat = 42, 4555
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: # while we're not 1m close to gps location
if self.preempt_requested():
rospy.loginfo("Go building GPS Ardu 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):
continue
msgSRV.xLont1 = trans1[0]
msgSRV.yLat1 = trans1[1]
res = is_near_srv(msgSRV).response
# if end algo change algo to direct one
# stop autonomous ardu
WS.sendCommand(1500, 1500)
servStartDiag()
return "endGoBuilding"
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, LatEntry, LongEntry
statePub.publish(String("InitEntryBuilding"))
stateInterPub.publish(Int8(4))
servStartDiag()
WS.sendCommand(1500, 1500)
rospy.loginfo("InitEntryBuilding")
try:
(trans1, rot1) = listener.lookupTransform("local_origin", "fcu", rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "t"
# check if orentation send
lat1 = 42.954303
long1 = 10.599793
(e, xB1, yB1) = LLtoUTM.LLtoUTM(23, lat1, long1) # west north end
lat2 = 42.954285
long2 = 10.600006
(e, xB2, yB2) = LLtoUTM.LLtoUTM(23, lat2, long2) # east north end
lat3 = 42.954209 # not the end of the building
long3 = 10.599991
(e, xB3, yB3) = LLtoUTM.LLtoUTM(23, lat3, long3)
lat4 = 42.954093 # south of building not a corner
long4 = 10.599861
(e, xB4, yB4) = LLtoUTM.LLtoUTM(23, lat4, long4)
############ Setting for init Map ######################
largeur1 = m.sqrt((xB1 - xB2) ** 2 + (yB1 - yB2) ** 2)
a1 = yB2 - yB1
b1 = xB1 - xB2
c1 = -(b1 * yB1 + a1 * xB2)
longueur1 = abs(a1 * xB4 + b1 * yB4 + c1) / (m.sqrt(a1 ** 2 + b1 ** 2))
a1 = yB2 - yB1
b1 = xB1 - xB2
c1 = -(b1 * yB1 + a1 * xB2)
a2 = yB2 - yB3
b2 = xB3 - xB2
c2 = -(b2 * yB3 + a2 * xB3)
d1 = abs(a1 * trans1[0] + b1 * trans1[1] + c1) / (m.sqrt(a1 ** 2 + b1 ** 2))
d2 = abs(a2 * trans1[0] + b2 * trans1[1] + c2) / (m.sqrt(a2 ** 2 + b2 ** 2))
angle1 = atan(a1) + m.pi / 2.0 + m.pi
angle2 = angle1 + m.pi
angle3 = atan(a1)
########################
heading = [angle1, angle2, angle3]
if initData.autonomous_level:
WS.findHeading(listener, heading[initData.heading])
else: # if teleOp wait for signal from remote to indicate that we are in good position
WS.waitForRemoteGo(5 * 1)
# calc size map needed
Lat = 42.954306
Long = 10.599778 # coordonate of building
msgSRV = IsNearRequest()
msgSRV.type1 = 0
msgSRV.type2 = 1
msgSRV.xLont1 = 11
msgSRV.yLat1 = 45
msgSRV.xLont2 = Long
msgSRV.yLat2 = Lat
msgSRV.threshold = 20
res = 0
start = rospy.get_time()
while not 1 and rospy.get_time() - start < 1 * 60: # wait for GPS for 1 min
if self.preempt_requested():
rospy.loginfo("Init Entry 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 "t"
msgSRV.xLont1 = trans1[0]
msgSRV.yLat1 = trans1[1]
res = is_near_srv(msgSRV).response
LatEntry = trans1[0]
LongEntry = trans1[1]
if initData.heading == 0:
widthMap = largeur1 + 6
heigthMap = d1 + longueur1 + 1 + 2
dist = max(widthMap, heigthMap)
poseY = 1 / dist
poseX = (dist - (3 + d2)) / dist
elif initData.heading == 1:
widthMap = largeur1 + d2 + 1 + 2
heigthMap = longueur1 + 6
dist = max(widthMap, heigthMap)
poseY = (3 + d1) / dist
poseX = 1 / dist
elif initData.heading == 2:
widthMap = d2 + 1 + 2
heigthMap = longueur1 + 6
dist = max(widthMap, heigthMap)
poseY = (dist - d1 - 3) / heightMap
poseX = 1 / dist
else:
dist = largeur1 * 2.1
poseX = 0.5
poseY = 0.5
dist = largeur1 * 2.1
poseX = 0.5
poseY = 0.5
w = dist / 0.05
os.system("ruby /home/nuc1/ruby/launchFilecreate.rb %d %f %f" % (w, poseX, poseY))
skm = StartKillMsg()
skm.action = 1
skm.type = 0
skm.nId = 1
startKillPub.publish(skm) # start ccny_rgbd
rospy.sleep(5)
(r, p, yaw) = trans.euler_from_quaternion(rot1)
os.system(
"rosrun tf static_transform_publisher %f %f %f 0 0 0 /map /gps_origin 100 &"
% (-trans1[0], -trans1[1], -trans1[2])
) # start tf (local_origin fcu) as map to gps_origin
os.system("rosrun tf_dyn_static_broadcaster tf_dyn_static_broadcaster_node &") # launch ccny hector static
skm.action = 1
skm.type = 0
skm.nId = 2
startKillPub.publish(skm) # start hector Mapping
skm.action = 1
skm.type = 1
skm.nId = 2
# startKillPub.publish(skm)#start save_node
rospy.sleep(5)
servStartDiag()
return "endEntryBuilding"
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"
cartoExitMode = 3
if cartoExitMode == 1:
self.service_preempt()
try:
drivStart(False)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
ast.publish(Bool(False))
return "preempted"
elif True:
WS.waitForRemoteGo(30 * 60)
os.system("rosrun save_2d_map save_map.py &")
os.system("rosservice call /save_octomap /home/nuc1/data/octomap.ot")
os.system("rosservice call /save_pcd_map /home/nuc1/data/map3D.pcd")
return "endCartographieBuilding"
WS.sendCommand(1500, 1500)
try:
drivStart(False)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
servStartDiag()
ast.publish(Bool(False))
self.service_preempt()
return "endCartographieBuilding"
global carto_finished
carto_finished = False
def checkCartoFin(msg):