def execute(self, userdata):
global selfErrorPub, stateInterPub, statePub, servStartDiag, is_near_srv, initData, cartoExitMode, restartAlgo
statePub.publish(String("CartographyBuilding"))
stateInterPub.publish(Int8(5))
if restartAlgo:
print "tt" # restr ccny hector etc
servStartDiag()
rospy.loginfo("CartographieBuilding")
# make sure Cytron is good
# start algo deplacement
drivStart = rospy.ServiceProxy("car_controller/starter", Start_Control)
ast = rospy.Publisher("/start_astar", Bool)
if initData.autonomous_level:
skm = StartKillMsg()
skm.action = 1
skm.type = 1
skm.nId = 4
startKillPub.publish(skm) # start algo gestion maenouvre
skm.action = 1
skm.type = 0
skm.nId = 5
startKillPub.publish(skm) # start driving algo
skm.action = 1
skm.type = 0
skm.nId = 6
startKillPub.publish(skm) # start astar not calculing
rospy.sleep(2)
try:
drivStart(True)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def astar_arriv_cb(msg):
global posGPSBuilding,listener,i,exit,startKillPub,finishMapping
if not exit:
if i > len(posGPSBuilding):
skm = StartKillMsg()
skm.action = 0
skm.type = 0
skm.nId = 6
startKillPub.publish(skm)#stop astar path
skm.action = 0
skm.type = 0
skm.nId = 5
startKillPub.publish(skm)#stop car controller
finishMapping = True;
#launch other tf form posGPSBuilding
os.system("ruby /home/nuc1/ruby/TfFlaunchFilecreate.rb %s %f %f %f %f %f %f %s %s"%("map_gps_tf",posGPSBuilding[i][0],posGPSBuilding[i][1],0,0,0,0,"/pointtofollow","/gps_origin"))
os.system("roslaunch /home/nuc1/ruby/tf_static.launch &")
i=i+1
else:
skm = StartKillMsg()
skm.action = 0
skm.type = 0
skm.nId = 6
startKillPub.publish(skm)#stop astar path
skm.action = 0
skm.type = 0
skm.nId = 5
startKillPub.publish(skm)#stop car controller
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
statePub.publish(String("Initialisation"))
rospy.loginfo("init...")
# TODO verification branchement des sensors
# ccny hokuyo test mavros
os.system("rosrun start_node start_node_node.py &")
rospy.wait_for_service("start_node_srv")
rospy.loginfo("service starter connected")
startKillPub = rospy.Publisher("/start_kill_node", StartKillMsg, latch=True)
# 1 rosrun :
# 0 hokuyu 1 gps_follow 2 save_node 3 pwm_send
# 0 roslaunch
# 0 openni 1 rgbd 2 hector_mapping 3 mavros 4 support.launch
skm = StartKillMsg()
skm.action = 1
skm.type = 0
skm.nId = 4
startKillPub.publish(
skm
) # support.launch: gps_handler start_node diagnostic drift_detection mode stuck rc_receive state_integrateur proxy_eura_smach
rospy.wait_for_service("/IsNear")
is_near_srv = rospy.ServiceProxy("IsNear", IsNear)
rospy.sleep(3)
stateInterPub.publish(Int8(0)) # now we can publish this message not listened before
skm.action = 1
skm.type = 1
skm.nId = 3
startKillPub.publish(skm) # pwm serial send
rospy.wait_for_service("/pwm_serial_send")
skm.action = 1
skm.type = 0
skm.nId = 0
startKillPub.publish(skm) # kinect
rospy.wait_for_service("/camera_rgb_frame_tf/get_loggers") # then movie_save
skm.action = 1
skm.type = 1
skm.nId = 0
startKillPub.publish(skm) # hokuyo
rospy.wait_for_service("/hokuyo_node/self_test")
skm.action = 1
skm.type = 0
skm.nId = 3
startKillPub.publish(skm) # Mavros #Remote and/or pwm should be online ! otherwise ardu going into failsafe
rospy.sleep(8)
# wait parameter
# get Data for what to do, level of autonomous
initData = InitData() # WS.waitForInitData(2*60)
rospy.sleep(10)
if initData == "Error":
exit(0)
start = rospy.get_time()
try:
(trans1, rot1) = listener.lookupTransform("local_origin", "fcu", rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
exit(0)
Lat = 42.954306
Long = 10.599778
msgSRV = IsNearRequest()
msgSRV.type1 = 0
msgSRV.type2 = 1
msgSRV.xLont1 = trans1[0]
msgSRV.yLat1 = trans1[1]
msgSRV.xLont2 = Long
msgSRV.yLat2 = Lat
msgSRV.threshold = 500
res = is_near_srv(msgSRV).response
rospy.loginfo("Wait gps")
while rospy.get_time() - start < 1 * 60 and not res: # wait for GPS for 5 min
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
rospy.sleep(1.0 / 20.0)
if not res:
return "endInitTeleOp"
os.system(
"rosrun tf_dyn_static_broadcaster tf_dyn_static_broadcaster_node local_origin fcu local_origin start_mission &"
)
if not initData.autonomous_level:
return "endInitTeleOp"
return "endInitGPS" # ardu not an option as we can't use it properly with computer for now
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"
