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, 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 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"