def callback( packet ) : if rospy.is_shutdown() : sys.exit() message = GPSFix() message.header.stamp = rospy.Time.now() message.header.frame_id = 'base_link' message.latitude = packet[ 'latitude' ] * 180 / math.pi message.longitude = packet[ 'longitude' ] * 180 / math.pi message.altitude = packet[ 'height' ] message.time = packet[ 'time' ] message.pdop = packet[ 'PDOP' ] message.hdop = packet[ 'HDOP' ] message.vdop = packet[ 'VDOP' ] message.tdop = packet[ 'TDOP' ] message.position_covariance = [ packet[ 'sigma-E' ], packet[ 'cov-EN' ] , 0, packet[ 'cov-EN' ], packet[ 'sigma-N' ], 0, 0, 0, packet[ 'sigma-Up'] ] if packet[ 'cov-EN' ] != 0 : message.position_covariance_type = 3 elif packet[ 'sigma-E' ] != 0 : message.position_covariance_type = 2 else : message.position_covariance_type = 0 if debug : pp.pprint( packet ) pp.pprint( message ) message.status.status = (0 if (message.position_covariance_type > 0) else -1) message.status.position_source = 1 message.status.header = message.header publisherGPSFix.publish( message ) if message.status >= 0 : publisherNavSatFix.publish( NavSatFix( message.header , NavSatStatus( message.status.status, 3 ), message.latitude, message.longitude, message.altitude, message.position_covariance, message.position_covariance_type ) )
def navsatfix_to_gpsfix(navsat_msg): # Convert sensor_msgs/NavSatFix messages to gps_common/GPSFix messages gpsfix_msg = GPSFix() gpsfix_msg.header = navsat_msg.header gpsfix_msg.status.status = navsat_msg.status.status gpsfix_msg.status.motion_source = GPSStatus.SOURCE_NONE gpsfix_msg.status.orientation_source = GPSStatus.SOURCE_NONE gpsfix_msg.status.position_source = GPSStatus.SOURCE_NONE if ((navsat_msg.status.service & NavSatStatus.SERVICE_GPS) or (navsat_msg.status.service & NavSatStatus.SERVICE_GLONASS) or (navsat_msg.status.service & NavSatStatus.SERVICE_GALILEO)): gpsfix_msg.status.motion_source = \ gpsfix_msg.status.motion_source | GPSStatus.SOURCE_GPS gpsfix_msg.status.orientation_source = \ gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_GPS gpsfix_msg.status.position_source = \ gpsfix_msg.status.position_source | GPSStatus.SOURCE_GPS if navsat_msg.status.service & NavSatStatus.SERVICE_COMPASS: gpsfix_msg.status.orientation_source = \ gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_MAGNETIC gpsfix_msg.latitude = navsat_msg.latitude gpsfix_msg.longitude = navsat_msg.longitude gpsfix_msg.altitude = navsat_msg.altitude gpsfix_msg.position_covariance = navsat_msg.position_covariance gpsfix_msg.position_covariance_type = navsat_msg.position_covariance_type return gpsfix_msg
def publishGps(pub_gps, frame): message = GPSFix() #header now = rospy.get_rostime() message.header.stamp.secs = now.secs message.header.stamp.nsecs = now.nsecs message.header.frame_id = '/filter_send_gps' #status message.status.header.seq = message.header.seq message.status.header.stamp.secs = now.secs message.status.header.stamp.nsecs = now.nsecs message.status.header.frame_id = frame message.status.satellites_used = nbSat message.status.status = status #autres message.latitude = latitude message.longitude = longitude message.altitude = altitude message.track = route message.speed = speed message.time = time message.hdop = hdop pub_gps.publish(message)
def testAutoOriginFromGPSFix(self): rospy.init_node('test_initialize_origin') gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2) origin_sub = self.subscribeToOrigin() gps_msg = GPSFix() gps_msg.latitude = swri['latitude'] gps_msg.longitude = swri['longitude'] gps_msg.altitude = swri['altitude'] gps_msg.track = swri['heading'] r = rospy.Rate(10.0) while not rospy.is_shutdown(): gps_pub.publish(gps_msg) r.sleep()
def testAutoOriginFromGPSFix(self): rospy.init_node('test_auto_origin_from_gps_fix') gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2, latch=True) origin_sub = self.subscribeToOrigin() self.test_stamp = True gps_msg = GPSFix() gps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.latitude = swri['latitude'] gps_msg.longitude = swri['longitude'] gps_msg.altitude = swri['altitude'] gps_msg.track = swri['heading'] gps_msg.header.stamp = msg_stamp gps_pub.publish(gps_msg) rospy.spin() self.assertTrue(self.got_origin)
def testAutoOriginFromGPSFix(self): rospy.init_node('test_initialize_origin') gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2) origin_sub = self.subscribeToOrigin() self.test_stamp = True gps_msg = GPSFix() gps_msg.latitude = swri['latitude'] gps_msg.longitude = swri['longitude'] gps_msg.altitude = swri['altitude'] gps_msg.track = swri['heading'] gps_msg.header.stamp = msg_stamp r = rospy.Rate(10.0) while not rospy.is_shutdown(): gps_pub.publish(gps_msg) r.sleep()
def testAutoOriginFromGPSFix(self): rospy.init_node('test_auto_origin_from_gps_fix') gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2) origin_sub = self.subscribeToOrigin() self.test_stamp = True gps_msg = GPSFix() gps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.latitude = swri['latitude'] gps_msg.longitude = swri['longitude'] gps_msg.altitude = swri['altitude'] gps_msg.track = swri['heading'] gps_msg.header.stamp = msg_stamp r = rospy.Rate(100.0) while not rospy.is_shutdown(): gps_pub.publish(gps_msg) r.sleep()
def parse_origin(local_xy_origin): global _gps_fix local_xy_origins = rospy.get_param('~local_xy_origins', []) for origin in local_xy_origins: if origin["name"] == local_xy_origin: _gps_fix = GPSFix() _gps_fix.header.frame_id = _local_xy_frame _gps_fix.status.header.frame_id = local_xy_origin _gps_fix.latitude = origin["latitude"] _gps_fix.longitude = origin["longitude"] _gps_fix.altitude = origin["altitude"] _gps_fix.track = 90 _origin_pub.publish(_gps_fix)
def parse_origin(local_xy_origin): global _gps_fix local_xy_origins = rospy.get_param('~local_xy_origins', []) for origin in local_xy_origins: if origin["name"] == local_xy_origin: _gps_fix = GPSFix() _gps_fix.header.frame_id = _local_xy_frame _gps_fix.status.header.frame_id = local_xy_origin _gps_fix.latitude = origin["latitude"] _gps_fix.longitude = origin["longitude"] _gps_fix.altitude = origin["altitude"] _origin_pub.publish(_gps_fix) return
def SimGPS(): global pose #data to be published gpsData = GPSFix() pub = rospy.Publisher('gpssim', GPSFix, queue_size=10) rospy.init_node('SimGPS', anonymous=True) rospy.Subscriber('base_pose_ground_truth', Odometry, pose_callback) #load initial point (lat = init_point[0][0], lon = init_point[0][1]) init_point_file = GPSListOfPoints() init_point_file.loadListFromFile(filename) #load initial point to an object GPSPoint() origin = GPSPoint(init_point_file[0]) r = rospy.Rate(1) #1 Hz (rate of /gpssim) while not rospy.is_shutdown(): #Transform pose to XY point pose_point = XYPoint(pose.pose.pose.position.x, pose.pose.pose.position.y) #Transform pose_point to GPSPoint gpsData_point = pose_point.XYToGPS(origin) #Transform GPSPoint to GPSFix data ready to publish in topic gpsData.latitude = gpsData_point.latitude gpsData.longitude = gpsData_point.longitude #Get track from quaternion quaternion = pyQuaternion() quaternion.ROSQuaternionTransform(pose.pose.pose.orientation) #Transform quaternion to jaw rotation (normalized to 360.0) gpsData.track = XYPoint(0.0, 0.0).angleToBearing(quaternion.getJaw()) #rospy.loginfo(gpsData) pub.publish(gpsData) r.sleep()
def run(): expected_fleet_size = rospy.get_param('fleetSize', 1) receiving_freq = 10. #Set the speed of the transmission loops ################################################################################################### # Look for XBee USB port, to avoid conflicts with other USB devices ################################################################################################### rospy.init_node('fleetCoordinator', anonymous=True) rospy.loginfo("Looking for XBee...") context = pyudev.Context() usbPort = 'No XBee found' for device in context.list_devices(subsystem='tty'): if 'ID_VENDOR' in device and device['ID_VENDOR'] == 'FTDI': usbPort = device['DEVNAME'] ser = serial.Serial(usbPort, baudrate=57600, timeout=2) ################################################################################################### # Get local XBee ID (should be 0, convention for Coordinator) ################################################################################################### # Enter XBee command mode ser.write('+++') rospy.sleep(1.2) ser.read(10) # Get local XBee adress ser.write('ATMY\r') rospy.sleep(0.1) ans = ser.read(10) ID = eval(ans.split('\r')[0]) # Exit XBee "command mode" ser.write('ATCN\r') if ID == 0: rospy.loginfo("\nHello,\nI am Coordinator " + str(ID) + '\n') else: raise Exception( "This XBee device is not the coordinator of the network,\nlook for the XBee device stamped with 'C'." ) ################################################################################################### # Look for the boats connected in the XBee network ################################################################################################### # To check we have all the boats connected connected = [] #list of connected boats IDs rospy.loginfo("Expecting a fleet of " + str(expected_fleet_size) + " sailboats.") while not rospy.is_shutdown() and len(connected) < expected_fleet_size: #Read a connection message from a sailboat c = ser.read(1) line = '' while c != '#' and not rospy.is_shutdown(): c = ser.read(1) while c != '=' and not rospy.is_shutdown(): line += c c = ser.read(1) line += c #Transmission checks, details in the transmission loop part below check, msgReceived = is_valid(line) if check: words = msgReceived.split() IDboat = int(words[-1]) # if the sailboat is not connected yet, we connect it if IDboat not in connected: connected.append(IDboat) rospy.loginfo('|' + msgReceived + '|') fleetSize = len(connected) #link each ID to a minimal line number in the data storing structure linkDict = {connected[i]: i for i in range(fleetSize)} ser.write(str(fleetSize) + '@' + str(connected) + '@Connected' + '\n') rospy.loginfo("Got boats " + str(connected) + ' connected\n') sleep(5) ################################################################################################### # Initialisation ################################################################################################### #Variables storing the data received by the subscribers global targetString, modeString targetString = str({boat: (0, numpy.pi / 2) for boat in connected}) modeString = str({boat: 0 for boat in connected}) emission_freq = receiving_freq / fleetSize #Frequency of emission for the Coordinator rate = rospy.Rate(receiving_freq) ser.timeout = 0.1 compteur = 0 pub_send_connected = rospy.Publisher("xbee_send_connected", String, queue_size=1) for i in range(2): pub_send_connected.publish(String(data=str(connected))) sleep(2) pub_send_gps = [ rospy.Publisher("xbee_send_gps_" + str(i), GPSFix, queue_size=0) for i in connected ] pub_send_euler_angles = [ rospy.Publisher("xbee_send_euler_angles_" + str(i), Vector3, queue_size=0) for i in connected ] pub_send_line_begin = [ rospy.Publisher("xbee_send_line_begin_" + str(i), Pose2D, queue_size=0) for i in connected ] pub_send_line_end = [ rospy.Publisher("xbee_send_line_end_" + str(i), Pose2D, queue_size=0) for i in connected ] GPSdata = GPSFix() eulerAnglesData = Vector3() lineStartData, lineEndData = Pose2D(), Pose2D() ################################################################################################### #Subscribe to the topics that send the data to communicate to the sailboats. #This data comes from the operator's control systems (keyboard control...) ################################################################################################### # Receives the data relative to the target point # (depends on controlMode, common to all boats) rospy.Subscriber('commands', String, targetTransmission) # Receives the string indicator of the control mode rospy.Subscriber('controlMode', String, modeTransmission) ################################################################################################### # Transmission Loop ################################################################################################### #For statistics and synchronisation emission = -1 #Data storing structure received = ['ID@nothing@nothing@nothing@nothing@nothing@nothing' ] * fleetSize while not rospy.is_shutdown(): emission += 1 c = '' line = '' loopTime = time() #################################################################################################### # Receive useful data from the sailboats # Frame received: # "#####msgSize@ID@windForceString@windDirectionString@gpsString@eulerAnglesString@lineBeginString@lineEndString=====\n" #################################################################################################### #If available, read a line from the XBee while c != '#' and (time() - loopTime) < ( 1 / receiving_freq) and not rospy.is_shutdown(): c = ser.read(1) msgTime = time() if c == '#': while c != '=' and (time() - msgTime) < ( 1 / receiving_freq) and not rospy.is_shutdown(): c = ser.read(1) line += c if "Hello" in line: continue # rospy.loginfo('Line:\n|'+line+'|') # Check message syntax and checkSum and clean the message to use only the useful data check, msgReceived = is_valid(line) if check: # rospy.loginfo("Received\n|" +msgReceived+'|\n') compteur += 1 #Organise the incoming data in the storing structure msgData = msgReceived.split('@') IDboat = int(msgData[0]) received[linkDict[IDboat] - 1] = msgReceived GPSframe = msgData[3] tmpEuler = msgData[4].split(',') eulerAnglesData.x = float(tmpEuler[0]) eulerAnglesData.y = float(tmpEuler[1]) eulerAnglesData.z = float(tmpEuler[2]) tmpStartLine = msgData[5].split(',') lineStartData.x = float(tmpStartLine[0]) lineStartData.y = float(tmpStartLine[1]) lineStartData.theta = float(tmpStartLine[2]) tmpEndLine = msgData[6].split(',') lineEndData.x = float(tmpEndLine[0]) lineEndData.y = float(tmpEndLine[1]) lineEndData.theta = float(tmpEndLine[2]) data = GPSframe.split(',') if GPSframe != "nothing" and data[0] == '$GPGGA' and data[2] != '': #header now = rospy.get_rostime() GPSdata.header.stamp.secs = now.secs GPSdata.header.stamp.nsecs = now.nsecs GPSdata.header.frame_id = '/xbee_send_gps' status = 1 gps_time = float(data[1][0:2]) * 3600 + float( data[1][2:4]) * 60 + float(data[1][4:]) latitude = float(data[2][0:(len(data[2]) - 7)]) + float( data[2][(len(data[2]) - 7):]) / 60 if data[3] == 'S': latitude = -latitude longitude = float(data[4][0:(len(data[4]) - 7)]) + float( data[4][(len(data[4]) - 7):]) / 60 if data[5] == 'W': longitude = -longitude frame_type = float(data[6]) nbSat = float(data[7]) if data[8] != '': hdop = float(data[8]) altitude = float(data[9].split(',')[0]) #status GPSdata.status.header.seq = GPSdata.header.seq GPSdata.status.header.stamp.secs = now.secs GPSdata.status.header.stamp.nsecs = now.nsecs GPSdata.status.header.frame_id = 'GPGGA' GPSdata.status.satellites_used = nbSat GPSdata.status.status = status #autres GPSdata.latitude = latitude GPSdata.longitude = longitude GPSdata.altitude = altitude GPSdata.time = gps_time GPSdata.hdop = hdop pub_send_gps[linkDict[IDboat]].publish(GPSdata) if eulerAnglesData.x != -999: pub_send_euler_angles[linkDict[IDboat]].publish( eulerAnglesData) if lineStartData.x != -999: pub_send_line_begin[linkDict[IDboat]].publish(lineStartData) if lineEndData.x != -999: pub_send_line_end[linkDict[IDboat]].publish(lineEndData) if not check: # rospy.loginfo("Could not read\n|"+line+'|') pass if emission % fleetSize == 0: #We are supposed to have the data of every boat at this point. #Logically, only a transmission failure (simultaneous talk, ...) #can prevent that. ########################################################################################################################################## # Send useful data to the sailboats # Frame emitted: # "#####msgSize@ID1@windForceString1@windDirectionString1@gpsString1@eulerAnglesString1@lineBeginString1@lineEndString1@ID2@...@targetString@modeString=====\n" ########################################################################################################################################## #Collect the data from each boat and the operator and gather them in one string #Creating the core message receivedLines = '' for line in received: receivedLines += line + '@' msg = receivedLines + targetString + '@' + modeString #Generating the checkSum message control size = str(len(msg) + 5) for i in range(len(size), 4): size = '0' + size msg = "#####" + size + '@' + msg + "=====\n" #Emit the message ser.write(msg) # rospy.loginfo("Emitted\n|" + msg + '|') received = ['ID@nothing@nothing@nothing@nothing@nothing@nothing' ] * fleetSize # rospy.loginfo("Emission " + str(emission//fleetSize)) # rate.sleep() if fleetSize == 1: rospy.sleep(0.1 / emission_freq) else: rospy.sleep(0.4 / emission_freq) ################################################################### # Deconnection signal ################################################################### rospy.loginfo("End mission, disconnection of all connected boats\n") ser.write('#####**********=====\n') ser.write('#####**********=====\n') rospy.sleep(1.) ser.write('#####**********=====\n') ser.write('#####**********=====\n') ser.write('#####**********=====\n') rospy.loginfo("Emitted " + str(emission // fleetSize)) rospy.loginfo("Received " + str(compteur - 2))
gpstime.header.stamp = gpsVel.header.stamp gpstime.time_ref = convertNMEATimeToROS(fields[1]) longitude = float( fields[5][0:3]) + float(fields[5][3:]) / 60 if fields[6] == 'W': longitude = -longitude latitude = float( fields[3][0:2]) + float(fields[3][2:]) / 60 if fields[4] == 'S': latitude = -latitude #publish data navData.latitude = latitude navData.longitude = longitude navData.altitude = float('NaN') navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN if "$GPVTG" in lastmsg: fields = lastmsg["$GPVTG"] if fields[1] != '': # Conversion to radian navData.track = float(fields[1]) * math.pi / 180. # Conversion from knots to m/s navData.speed = float(fields[5]) * 0.514444444444 gpsVel.header.stamp = timeNow gpsVel.twist.linear.x = navData.speed * math.sin( navData.track) gpsVel.twist.linear.y = navData.speed * math.cos( navData.track)
while lat_lon_origin == [[],[]]: rospy.sleep(0.5) rospy.loginfo("[{}] Waiting GPS origin".format(node_name)) rospy.loginfo("[{}] Got GPS origin {}".format(node_name,lat_lon_origin)) while not rospy.is_shutdown(): xdot,delta_s=f(x,u) x = x + dt*xdot #rospy.loginfo("[{}] x : {}, y:{}".format(node_name,x[0,0],x[1,0])) #rospy.loginfo("[{}] theta : {}".format(node_name,x[2,0])) theta_msg.x = x[2,0] # heading xy_msg.x = x[0,0] # x pos xy_msg.y = x[1,0] # y pos xy_msg.z = delta_s wind_direction_msg.data = psi # wind direction wind_force_msg.data = awind # wind speed utm_to_latlon = utm.to_latlon(lat_lon_origin[1][0]-x[1,0],lat_lon_origin[1][1]+x[0,0], lat_lon_origin[1][2],lat_lon_origin[1][3]) gps_msg.latitude = utm_to_latlon[0] gps_msg.longitude = utm_to_latlon[1] gps_msg.speed = x[3,0] #speed pub_send_theta.publish(theta_msg) pub_send_xy.publish(xy_msg) pub_send_wind_direction.publish(wind_direction_msg) pub_send_wind_force.publish(wind_force_msg) pub_send_gps.publish(gps_msg) rate.sleep()
publish_time = True gpstime.header.stamp = gpsVel.header.stamp gpstime.time_ref = convertNMEATimeToROS(fields[1]) longitude = float(fields[5][0:3]) + float(fields[5][3:])/60 if fields[6] == 'W': longitude = -longitude latitude = float(fields[3][0:2]) + float(fields[3][2:])/60 if fields[4] == 'S': latitude = -latitude #publish data navData.latitude = latitude navData.longitude = longitude navData.altitude = float('NaN') navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN if "$GPVTG" in lastmsg: fields = lastmsg["$GPVTG"] if fields[1] != '': # Conversion to radian navData.track = float(fields[1])*math.pi/180. # Conversion from knots to m/s navData.speed = float(fields[5])*0.514444444444 gpsVel.header.stamp = timeNow gpsVel.twist.linear.x = navData.speed*math.sin(navData.track) gpsVel.twist.linear.y = navData.speed*math.cos(navData.track) publish_vel = True
# PointField('rgba', 12, PointField.UINT32, 1), ] scaled_polygon_pcl = pcl2.create_cloud(header, fields, all_points) labeled_pointcloud_pub.publish(scaled_polygon_pcl) bounding_box_pub.publish(marker) # gps/imu topic gen with open(gps_imu_file_paths[idx], 'rt') as fp: line = fp.readline().split(' ') quat_val = euler_to_quaternion(float(line[3]),float(line[4]),float(line[5])) navsat = GPSFix() navsat.header.frame_id = '/map' navsat.header.stamp = rospy.Time.now() navsat.latitude = float(line[0]) navsat.longitude = float(line[1]) navsat.altitude = float(line[2]) gps_pub.publish(navsat) imu = Imu() imu.header.frame_id = '/map' imu.header.stamp = rospy.Time.now() imu.orientation.x = quat_val[0] imu.orientation.y = quat_val[1] imu.orientation.z = quat_val[2] imu.orientation.w = quat_val[3] imu_pub.publish(imu) rate.sleep()