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 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 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 start_subscriber_spin(): rospy.init_node("GPS_Parser", anonymous=True) gps_pub = rospy.Publisher("GPS", GPSFix, queue_size=100) gps_port = serial.Serial("/dev/ttyUSB0", 57600) while not rospy.is_shutdown(): line = gps_port.readline() tokens = line.split(",") if (tokens[0] != "$GPGGA" or int(tokens[6]) is 0): continue lat = tokens[2] lat = int(lat[0:2]) + float(lat[2:]) / 60.0 n = tokens[3] if n is not "N": lat = -lat lon = tokens[4] lon = int(lon[0:3]) + float(lon[3:]) / 60.0 w = tokens[5] if n is "W": lon = -lon m = GPSFix(latitude=lat, longitude=lon) gps_pub.publish(m) pass
def testInvalidGPSFix(self): rospy.init_node('test_invalid_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_NO_FIX gps_msg.header.stamp = msg_stamp # There are two ways in which initialize_origin.py could fail due to getting # an invalid fix: if it publishes an origin despite not getting a valid fix # or if it unsubscribes from the gps & fix topics without ever publishing # an origin. # This will test for those conditions by waiting until the node has # subscribed to the topic, then failing if either ROS shuts down, which # our subscriber will do if it gets an origin message, or if the number of # connections drops to zero, which means initialize_origin.py subscribed # but did not publish a message. r = rospy.Rate(100.0) timeout = time.time() + 2 # time out after 2 seconds, which should be plenty node_attached = False while not rospy.is_shutdown() and time.time() < timeout: if not node_attached and gps_pub.get_num_connections() > 0: node_attached = True if node_attached and gps_pub.get_num_connections() == 0: break gps_pub.publish(gps_msg) r.sleep() self.assertFalse(self.got_message, "initialize_origin should not have published an origin.") self.assertFalse(node_attached and gps_pub.get_num_connections() == 0, "initialize_origin unsubscribed without getting a valid fix.")
def __init__(self): rospy.init_node("GPS") rospy.on_shutdown(self.shutdown) rospy.loginfo("Starting node...") self.rate = 1.0 self.heading = 0.0 self.latitude = 0.0 self.longitude = 0.0 self.pub_gps_fix = rospy.Publisher('gps_fix', GPSFix, queue_size=1) self.pub_gps_initial_fix = rospy.Publisher('gps_initial_fix', GPSFix, queue_size=1, latch=True) self.sub_gps_navsat = rospy.Subscriber('gps_navsat', NavSatFix, self.gps_navsat_callback) self.sub_odom = rospy.Subscriber('odom_wheel', Odometry, self.odom_callback) self.gps_msg = GPSFix() self.initial_heading_degrees = rospy.get_param( '~initial_heading_degrees') self.count = 0
def __init__(self): rospy.init_node("GPSCONV") rospy.on_shutdown(self.shutdown) rospy.loginfo("Starting GPS convertion...") self.rate = 1.0 self.heading = 0.0 self.latitude = 0.0 self.altitude = 0.0 self.longitude = 0.0 self.pub_gps_fix = rospy.Publisher('gps_fix', GPSFix, queue_size=1) self.pub_gps_initial_fix = rospy.Publisher('gps_initial_fix', GPSFix, queue_size=1, latch=True) self.sub_gps_navsat = rospy.Subscriber('gps_navsat', NavSatFix, self.gps_navsat_callback) self.sub_gps_heading = rospy.Subscriber('gps_heading', Int32, self.gps_heading_callback) self.gps_msg = GPSFix() self.count = 0
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 __init__(self, device="/dev/ttyUSB0", baudrate=115200): self.sb = SerialBuffer(device, baudrate) if not self.sb.is_open(): print "Cannot open port" exit() self.reset_srv = rospy.Service('rtk_gps/reset', Trigger, self.reset_callback) self.pub = rospy.Publisher('/rtk_gps/gps', GPSFix) self.msg = GPSFix()
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 ground_truth_callback(data): global viz_grndtruth_pub degrees_from_north = -math.degrees(data.heading_rad) + 90 rospy.loginfo("Got Ground Truth message: (%f, %f, %f)", data.latitude_deg, data.longitude_deg, degrees_from_north) viz_msg_groundtruth = GPSFix(latitude=data.latitude_deg, longitude=data.longitude_deg, track=degrees_from_north) viz_grndtruth_pub.publish(viz_msg_groundtruth) pass
def setUp(self): self.start_time = time.time() rospy.Subscriber('/final_waypoints', Lane, self.recv_lane) rospy.Subscriber('/vehicle/perfect_gps/enhanced_fix', GPSFix, self.recv_fix) self.waypoints = [] self.fix = GPSFix() self.received_waypoints = False self.received_fix = False self.max_error = 0 self.max_speed = 0
def run(self): counter = 0 rospy.loginfo("Simulation started") while not rospy.is_shutdown(): self.q_pub.publish(Quaternion(self.theta, self.omega, self.bias_yaw, self.delta_mag)) now = rospy.Time.now() rpy = Vector3Stamped() rpy.header.stamp = now rpy.vector.z = self.theta - self.bias_yaw + self.noise(self.noise_yaw) self.rpy_pub.publish(rpy) mag = Vector3Stamped() mag.header = rpy.header magnitude = 150 + self.noise(self.noise_magnitude_mag) theta = self.theta - self.delta_mag + self.noise(self.noise_mag) mag.vector.x = magnitude * math.cos(theta) mag.vector.y = magnitude * math.sin(theta) self.mag_pub.publish(mag) imu = Imu() imu.header = mag.header imu.angular_velocity.z = self.omega + self.noise(self.noise_omega) self.imu_pub.publish(imu) if (counter % 20) == 0: gps = GPSFix() gps.header = rpy.header gps.speed = 1.0 gps.track = self.theta + self.noise(self.noise_gps) gps.track = norm_angle(gps.track) self.gps_pub.publish(gps) counter += 1 self.bias_index += self.bias_speed * self.dt self.bias_yaw = 2 * math.pi * math.cos(self.bias_index) self.theta += self.omega * self.dt self.theta = norm_angle(self.theta) rospy.sleep(self.dt)
def waypoints_publisher(): waypoints_pub = rospy.Publisher('WAYPOINTS_VIZ', GPSFix, queue_size=10) # read from waypoints file, parse JSON, publish waypoints = open( "../RoboBuggy/Software/real_time/ROS_RoboBuggy/src/robobuggy/config/waypoints.txt", 'r') line = waypoints.readline() time.sleep(5) while line: data = json.loads(line) viz_msg_waypoint = GPSFix(latitude=data['latitude'], longitude=data['longitude']) waypoints_pub.publish(viz_msg_waypoint) time.sleep(.01) line = waypoints.readline() waypoints.close() pass
def command_callback(data): global viz_command_pub global last_latitude global last_longitude global last_heading_deg #steer_cmd should be degrees rospy.loginfo("got Steering Command msg: %f degrees", math.degrees(data.steer_cmd_rad)) viz_msg_steering = GPSFix(latitude=last_latitude, longitude=last_longitude, track=last_heading_deg - math.degrees(data.steer_cmd_rad)) viz_command_pub.publish(viz_msg_steering) pass
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 pose_callback(data): global viz_pose_pub global last_latitude global last_longitude global last_heading_deg #data.heading_rad is in Radians from north clockwise, but GPSFix requires degrees from north degrees_from_north = math.degrees(data.heading_rad) last_latitude = data.latitude_deg last_longitude = data.longitude_deg last_heading_deg = -degrees_from_north + 90 rospy.loginfo("got Pose msg: %f degrees lat, %f degrees long, %f bearing", last_latitude, last_longitude, last_heading_deg) viz_msg_heading = GPSFix(latitude=last_latitude, longitude=last_longitude, track=last_heading_deg) viz_pose_pub.publish(viz_msg_heading)
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 location_listener_node(): rospy.init_node("location_listener_node", anonymous=True) global position_publisher, marker_publisher global drone_marker, position drone_marker = Marker() initDroneMarker() position = GPSFix() position_publisher = rospy.Publisher('drone_position', GPSFix, queue_size=10) marker_publisher = rospy.Publisher('drone_marker', Marker, queue_size=10) rospy.Subscriber("mavros/global_position/local", Odometry, updateAttitude) rospy.Subscriber("mavros/global_position/global", NavSatFix, updateLocation) while not rospy.is_shutdown(): rospy.spin()
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 reset_vars(self): self.imu_msg = Imu() self.imu_msg.orientation_covariance = (-1., ) * 9 self.imu_msg.angular_velocity_covariance = (-1., ) * 9 self.imu_msg.linear_acceleration_covariance = (-1., ) * 9 self.pub_imu = False self.gps_msg = NavSatFix() self.xgps_msg = GPSFix() self.pub_gps = False self.vel_msg = TwistStamped() self.pub_vel = False self.mag_msg = Vector3Stamped() self.pub_mag = False self.temp_msg = Float32() self.pub_temp = False self.press_msg = Float32() self.pub_press = False self.anin1_msg = UInt16() self.pub_anin1 = False self.anin2_msg = UInt16() self.pub_anin2 = False self.pub_diag = False
def waypoints_publisher(): waypoints_pub = rospy.Publisher('WAYPOINTS_VIZ', GPSFix, queue_size=10) # Get location of waypoint file by getting package path and then moving from there rospack = rospkg.RosPack() robobuggy_path = rospack.get_path("robobuggy") config_file_loc = "/config/waypoints.txt" waypoint_file = robobuggy_path + config_file_loc # read from waypoints file, parse JSON, publish waypoints = open(waypoint_file) line = waypoints.readline() time.sleep(5) while line: data = json.loads(line) viz_msg_waypoint = GPSFix(latitude=data['latitude'], longitude=data['longitude']) waypoints_pub.publish(viz_msg_waypoint) time.sleep(.01) line = waypoints.readline() waypoints.close() pass
def run(): global device #dev_logger = utils.enable_logger("digi.xbee.devices", logging.DEBUG) dev_logger = utils.disable_logger("digi.xbee.devices") ################################################################################################### # 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'] device = XBeeDevice(usbPort, 57600) #device = XBeeDevice("/dev/ttyUSB0", 57600) device.open() print("Current timeout: %d seconds" % device.get_sync_ops_timeout()) device.set_sync_ops_timeout(0.1) ################################################################################################### # Get local XBee ID (should be 0, convention for Coordinator) ################################################################################################### ID = utils.bytes_to_int(device.get_16bit_addr().address) 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'.") ################################################################################################### # Initialisation ################################################################################################### #Variables storing the data received by the subscribers global remote_devices, pubGps, pubEuler, pubLB, pubLE, lastDataStr, lastData remote_devices = {} lastDataStr = {} lastData = {} pubGps = [] pubEuler = [] pubLB = [] pubLE = [] xnet = device.get_network() xnet.add_device_discovered_callback(network_callback) xnet.add_discovery_process_finished_callback(network_finished) device.add_data_received_callback(data_received) rate = rospy.Rate(10) GPSdata = GPSFix() eulerAnglesData = Vector3() lineStartData, lineEndData = Pose2D(), Pose2D() global chosen, mode, cmd mode = 0 chosen = 0 cmd = Twist() pygame.init() screen = pygame.display.set_mode( (640,480) ) pygame.display.set_caption('Python numbers') ################################################################################################### # Transmission Loop ################################################################################################### while not rospy.is_shutdown(): if(xnet.is_discovery_running() is False): xnet.start_discovery_process() display(screen) send_command() rate.sleep() if(xnet.is_discovery_running()): xnet.stop_discovery_process() device.close()
def __init__(self): self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) self.vel_pub = rospy.Publisher('vel', TwistStamped, queue_size=1) self.extend_fix_pub = rospy.Publisher('extend_fix', GPSFix, queue_size=1) self.heading_pub = rospy.Publisher('heading', QuaternionStamped, queue_size=1) self.time_ref_pub = rospy.Publisher('time_reference', TimeReference, queue_size=1) self.time_ref_source = rospy.get_param('~time_ref_source', None) self.use_RMC = rospy.get_param('~useRMC', False) self.valid_fix = False # epe = estimated position error self.default_epe_quality0 = rospy.get_param('~epe_quality0', 1000000) self.default_epe_quality1 = rospy.get_param('~epe_quality1', 4.0) self.default_epe_quality2 = rospy.get_param('~epe_quality2', 0.1) self.default_epe_quality4 = rospy.get_param('~epe_quality4', 0.02) self.default_epe_quality5 = rospy.get_param('~epe_quality5', 4.0) self.default_epe_quality9 = rospy.get_param('~epe_quality9', 3.0) self.using_receiver_epe = False self.lon_std_dev = float("nan") self.lat_std_dev = float("nan") self.alt_std_dev = float("nan") """Format for this dictionary is the fix type from a GGA message as the key, with each entry containing a tuple consisting of a default estimated position error, a NavSatStatus value, and a NavSatFix covariance value.""" self.gps_qualities = { # Unknown -1: [ self.default_epe_quality0, NavSatStatus.STATUS_NO_FIX, NavSatFix.COVARIANCE_TYPE_UNKNOWN ], # Invalid 0: [ self.default_epe_quality0, NavSatStatus.STATUS_NO_FIX, NavSatFix.COVARIANCE_TYPE_UNKNOWN ], # SPS 1: [ self.default_epe_quality1, NavSatStatus.STATUS_FIX, NavSatFix.COVARIANCE_TYPE_APPROXIMATED ], # DGPS 2: [ self.default_epe_quality2, NavSatStatus.STATUS_SBAS_FIX, NavSatFix.COVARIANCE_TYPE_APPROXIMATED ], # RTK Fix 4: [ self.default_epe_quality4, NavSatStatus.STATUS_GBAS_FIX, NavSatFix.COVARIANCE_TYPE_APPROXIMATED ], # RTK Float 5: [ self.default_epe_quality5, NavSatStatus.STATUS_GBAS_FIX, NavSatFix.COVARIANCE_TYPE_APPROXIMATED ], # WAAS 9: [ self.default_epe_quality9, NavSatStatus.STATUS_GBAS_FIX, NavSatFix.COVARIANCE_TYPE_APPROXIMATED ] } self.extend_fix = GPSFix() self.star_map_gps = [] self.star_map_bd = [] self.star_use_gps = [] self.star_use_bd = []
gpspub = rospy.Publisher('fix', NavSatFix) egpspub = rospy.Publisher('extfix', GPSFix) gpsVelPub = rospy.Publisher('vel',TwistStamped) gpstimePub = rospy.Publisher('time_reference', TimeReference) #Init GPS port GPSport = rospy.get_param('~port','/dev/ttyUSB0') GPSrate = rospy.get_param('~baud',4800) frame_id = rospy.get_param('~frame_id','gps') sleep_ms = int(rospy.get_param('~sleep_ms',5)) if frame_id[0] != "/": frame_id = addTFPrefix(frame_id) time_ref_source = rospy.get_param('~time_ref_source', frame_id) trigger = "$GP" + rospy.get_param('~trigger', "GGA") GPSLock = False navData = GPSFix() try: GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2) lastmsg = {} gpstxt = "" #Read in GPS while not rospy.is_shutdown(): #read GPS line c = GPS.read(1) if not c: # Nothing, keep waiting continue #anything useful will take at least that long to arrive rospy.sleep(sleep_ms/1000.) gpstxt += c + GPS.read(GPS.inWaiting()) if not ("\n" in gpstxt):
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))
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 spin_once(self): def baroPressureToHeight(value): c1 = 44330.0 c2 = 9.869232667160128300024673081668e-6 c3 = 0.1901975534856 intermediate = 1 - math.pow(c2 * value, c3) height = c1 * intermediate return height # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp_data = data.get('Temperature') # float orient_data = data.get('Orientation Data') velocity_data = data.get('Velocity') position_data = data.get('Latlon') altitude_data = data.get('Altitude') acc_data = data.get('Acceleration') gyr_data = data.get('Angular Velocity') mag_data = data.get('Magnetic') pressure_data = data.get('Pressure') time_data = data.get('Timestamp') gnss_data = data.get('GNSS') # debug the data from the sensor # print(data) # print("\n") # create messages and default values "Temp message" temp_msg = Temperature() pub_temp = False "Imu message supported with Modes 1 & 2" imuraw_msg = Imu() pub_imuraw = False imuins_msg = Imu() pub_imuins = False "SensorSample message supported with Mode 2" ss_msg = sensorSample() pub_ss = False "Mag message supported with Modes 1 & 2" mag_msg = Vector3Stamped() pub_mag = False "Baro in meters" baro_msg = FluidPressure() height_msg = baroSample() pub_baro = False "GNSS message supported only with MTi-G-7xx devices" "Valid only for modes 1 and 2" gnssPvt_msg = gnssSample() gps1_msg = NavSatFix() gps2_msg = GPSFix() pub_gnssPvt = False gnssSatinfo_msg = GPSStatus() pub_gnssSatinfo = False # All filter related outputs "Supported in mode 3" ori_msg = orientationEstimate() pub_ori = False "Supported in mode 3 for MTi-G-7xx devices" vel_msg = velocityEstimate() pub_vel = False "Supported in mode 3 for MTi-G-7xx devices" pos_msg = positionEstimate() pub_pos = False # first getting the sampleTimeFine # note if we are not using ros time, the we should replace the header # with the time supplied by the GNSS unit if time_data and not self.use_rostime: time = time_data['SampleTimeFine'] h.stamp.secs = 100e-6 * time h.stamp.nsecs = 1e5 * time - 1e9 * math.floor(h.stamp.secs) # temp data if temp_data: temp_msg.temperature = temp_data['Temp'] pub_temp = True # acceleration data if acc_data: if 'accX' in acc_data: # found acceleration pub_imuraw = True imuraw_msg.linear_acceleration.x = acc_data['accX'] imuraw_msg.linear_acceleration.y = acc_data['accY'] imuraw_msg.linear_acceleration.z = acc_data['accZ'] if 'freeAccX' in acc_data: # found free acceleration pub_imuins = True imuins_msg.linear_acceleration.x = acc_data['freeAccX'] imuins_msg.linear_acceleration.y = acc_data['freeAccY'] imuins_msg.linear_acceleration.z = acc_data['freeAccZ'] if 'Delta v.x' in acc_data: # found delta-v's pub_ss = True ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] #else: # raise MTException("Unsupported message in XDI_AccelerationGroup.") # gyro data if gyr_data: if 'gyrX' in gyr_data: # found rate of turn pub_imuraw = True imuraw_msg.angular_velocity.x = gyr_data['gyrX'] imuraw_msg.angular_velocity.y = gyr_data['gyrY'] imuraw_msg.angular_velocity.z = gyr_data['gyrZ'] # note we do not force publishing the INS if we do not use the free acceleration imuins_msg.angular_velocity.x = gyr_data['gyrX'] imuins_msg.angular_velocity.y = gyr_data['gyrY'] imuins_msg.angular_velocity.z = gyr_data['gyrZ'] if 'Delta q0' in gyr_data: # found delta-q's pub_ss = True ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] #else: # raise MTException("Unsupported message in XDI_AngularVelocityGroup.") # magfield if mag_data: ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX'] ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY'] ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ'] pub_mag = True if pressure_data: pub_baro = True baro_msg.fluid_pressure = pressure_data['Pressure'] height = baroPressureToHeight(pressure_data['Pressure']) height_msg.height = ss_msg.internal.baro.height = height # gps fix message if gnss_data and 'lat' in gnss_data: pub_gnssPvt = True # A "3" means that the MTi-G is using the GPS data. # A "1" means that the MTi-G was using GPS data and is now coasting/dead-reckoning the # position based on the inertial sensors (the MTi-G is not using GPS data in this mode). # This is done for 45 seconds, before the MTi-G Mode drops to "0". # A "0" means that the MTi-G doesn't use GPS data and also that it # doesn't output position based on the inertial sensors. if gnss_data['fix'] < 2: gnssSatinfo_msg.status = NavSatStatus.STATUS_NO_FIX # no fix gps1_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix gps1_msg.status.service = 0 else: gnssSatinfo_msg.status = NavSatStatus.STATUS_FIX # unaugmented gps1_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented gps1_msg.status.service = NavSatStatus.SERVICE_GPS # lat lon alt gps1_msg.latitude = gnss_data['lat'] gps1_msg.longitude = gnss_data['lon'] gps1_msg.altitude = gnss_data['hEll'] # covariances gps1_msg.position_covariance[0] = math.pow(gnss_data['horzAcc'], 2) gps1_msg.position_covariance[4] = math.pow(gnss_data['horzAcc'], 2) gps1_msg.position_covariance[8] = math.pow(gnss_data['vertAcc'], 2) gps1_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # custom message gnssPvt_msg.itow = gnss_data['iTOW'] gnssPvt_msg.fix = gnss_data['fix'] gnssPvt_msg.latitude = gnss_data['lat'] gnssPvt_msg.longitude = gnss_data['lon'] gnssPvt_msg.hEll = gnss_data['hEll'] gnssPvt_msg.hMsl = gnss_data['hMsl'] gnssPvt_msg.vel.x = gnss_data['velE'] gnssPvt_msg.vel.y = gnss_data['velN'] gnssPvt_msg.vel.z = gnss_data['velD'] gnssPvt_msg.hAcc = gnss_data['horzAcc'] gnssPvt_msg.vAcc = gnss_data['vertAcc'] gnssPvt_msg.sAcc = gnss_data['speedAcc'] gnssPvt_msg.pDop = gnss_data['PDOP'] gnssPvt_msg.hDop = gnss_data['HDOP'] gnssPvt_msg.vDop = gnss_data['VDOP'] gnssPvt_msg.numSat = gnss_data['nSat'] gnssPvt_msg.heading = gnss_data['heading'] gnssPvt_msg.headingAcc = gnss_data['headingAcc'] if orient_data: if 'Q0' in orient_data: pub_imuraw = True imuraw_msg.orientation.w = orient_data['Q0'] imuraw_msg.orientation.x = orient_data['Q1'] imuraw_msg.orientation.y = orient_data['Q2'] imuraw_msg.orientation.z = orient_data['Q3'] pub_imuins = True imuins_msg.orientation.w = orient_data['Q0'] imuins_msg.orientation.x = orient_data['Q1'] imuins_msg.orientation.y = orient_data['Q2'] imuins_msg.orientation.z = orient_data['Q3'] elif 'Roll' in orient_data: pub_ori = True ori_msg.roll = orient_data['Roll'] ori_msg.pitch = orient_data['Pitch'] ori_msg.yaw = orient_data['Yaw'] else: raise MTException( 'Unsupported message in XDI_OrientationGroup') if velocity_data: pub_vel = True vel_msg.velE = velocity_data['velX'] vel_msg.velN = velocity_data['velY'] vel_msg.velU = velocity_data['velZ'] if position_data: pub_pos = True pos_msg.latitude = position_data['lat'] pos_msg.longitude = position_data['lon'] if altitude_data: pub_pos = True tempData = altitude_data['ellipsoid'] pos_msg.hEll = tempData[0] # publish available information if pub_imuraw: imuraw_msg.header = h self.imuraw_pub.publish(imuraw_msg) if pub_imuins: imuins_msg.header = h self.imuins_pub.publish(imuins_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: temp_msg.header = h self.temp_pub.publish(temp_msg) if pub_ss: ss_msg.header = h self.ss_pub.publish(ss_msg) if pub_baro: baro_msg.header = h height_msg.header = h self.baro_pub.publish(baro_msg) self.height_pub.publish(height_msg) if pub_gnssPvt: gnssPvt_msg.header = h gps1_msg.header = h self.gnssPvt_pub.publish(gnssPvt_msg) self.gps1_pub.publish(gps1_msg) #if pub_gnssSatinfo: # gnssSatinfo_msg.header = h # self.gnssSatinfo_pub.publish(gnssSatinfo_msg) if pub_ori: ori_msg.header = h self.ori_pub.publish(ori_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_pos: pos_msg.header = h self.pos_pub.publish(pos_msg)
# t -> topic to which read data from (sim is from simulation, gps for real data) #default sim gpsTopic = "sim" #limits for lineal velocity & turn maxVel = 2.0 #2.0 m/s minVel = 0.3 #1.8 m/s maxTurn = 0.8 #0.6 rad/s Kv = 0.2 #Constant for linear velocity Kt = 2 #Constant for turn velocity gpsError = 5 #Max error possible for GPS #GPSFix to cache the GPS data gpsData = GPSFix() #GPSPoint to cache the GPS data gpsDataPoint = GPSPoint() #GPSPoint to cache the GPSTarget target = GPSPoint() #GPSPoint to cache the GPSTarget finalTarget = GPSPoint() #flag to indicate if final goal has been reached goal = False #ball controller status # 0 ball tracker takes control
def spin_once(self): def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi * orient['roll'] / 180., pi * orient['pitch'] / 180, pi * orient['yaw'] / 180.) except KeyError: pass try: m = identity_matrix() m[:3, :3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp = data.get('Temp') # float raw_data = data.get('RAW') imu_data = data.get('Calib') orient_data = data.get('Orient') velocity_data = data.get('Vel') position_data = data.get('Pos') rawgps_data = data.get('RAWGPS') status = data.get('Stat') # int # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False # fill information where it's due # start by raw information that can be overriden if raw_data: # TODO warn about data not calibrated pub_imu = True pub_vel = True pub_mag = True pub_temp = True # acceleration imu_msg.linear_acceleration.x = raw_data['accX'] imu_msg.linear_acceleration.y = raw_data['accY'] imu_msg.linear_acceleration.z = raw_data['accZ'] imu_msg.linear_acceleration_covariance = (0., ) * 9 # gyroscopes imu_msg.angular_velocity.x = raw_data['gyrX'] imu_msg.angular_velocity.y = raw_data['gyrY'] imu_msg.angular_velocity.z = raw_data['gyrZ'] imu_msg.angular_velocity_covariance = (0., ) * 9 vel_msg.twist.angular.x = raw_data['gyrX'] vel_msg.twist.angular.y = raw_data['gyrY'] vel_msg.twist.angular.z = raw_data['gyrZ'] # magnetometer mag_msg.vector.x = raw_data['magX'] mag_msg.vector.y = raw_data['magY'] mag_msg.vector.z = raw_data['magZ'] # temperature # 2-complement decoding and 1/256 resolution x = raw_data['temp'] if x & 0x8000: temp_msg.data = (x - 1 << 16) / 256. else: temp_msg.data = x / 256. if rawgps_data: if rawgps_data['bGPS'] < self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT'] * 1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data[ 'LON'] * 1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT'] * 1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2 * rawgps_data['Hacc'] * 1e-3 xgps_msg.err_vert = 2 * rawgps_data['Vacc'] * 1e-3 self.old_bGPS = rawgps_data['bGPS'] if temp is not None: pub_temp = True temp_msg.data = temp if imu_data: try: x = imu_data['gyrX'] y = imu_data['gyrY'] z = imu_data['gyrZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.angular_velocity.x = v[0] imu_msg.angular_velocity.y = v[1] imu_msg.angular_velocity.z = v[2] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = v[0] vel_msg.twist.angular.y = v[1] vel_msg.twist.angular.z = v[2] pub_vel = True except KeyError: pass try: x = imu_data['accX'] y = imu_data['accY'] z = imu_data['accZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.linear_acceleration.x = v[0] imu_msg.linear_acceleration.y = v[1] imu_msg.linear_acceleration.z = v[2] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: x = imu_data['magX'] y = imu_data['magY'] z = imu_data['magZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) mag_msg.vector.x = v[0] mag_msg.vector.y = v[1] mag_msg.vector.z = v[2] pub_mag = True except KeyError: pass if velocity_data: pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] if orient_data: pub_imu = True orient_quat = quat_from_orient(orient_data) imu_msg.orientation.x = orient_quat[0] imu_msg.orientation.y = orient_quat[1] imu_msg.orientation.z = orient_quat[2] imu_msg.orientation.w = orient_quat[3] imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) if position_data: pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] if status is not None: if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) if pub_gps: if status & 0b0100: gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg)
gpspub = rospy.Publisher('fix', NavSatFix) egpspub = rospy.Publisher('extfix', GPSFix) gpsVelPub = rospy.Publisher('vel', TwistStamped) gpstimePub = rospy.Publisher('time_reference', TimeReference) #Init GPS port GPSport = rospy.get_param('~port', '/dev/ttyUSB0') GPSrate = rospy.get_param('~baud', 4800) frame_id = rospy.get_param('~frame_id', 'gps') sleep_ms = int(rospy.get_param('~sleep_ms', 5)) if frame_id[0] != "/": frame_id = addTFPrefix(frame_id) time_ref_source = rospy.get_param('~time_ref_source', frame_id) trigger = "$GP" + rospy.get_param('~trigger', "GGA") GPSLock = False navData = GPSFix() try: GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2) lastmsg = {} gpstxt = "" #Read in GPS while not rospy.is_shutdown(): #read GPS line c = GPS.read(1) if not c: # Nothing, keep waiting continue #anything useful will take at least that long to arrive rospy.sleep(sleep_ms / 1000.) gpstxt += c + GPS.read(GPS.inWaiting()) if not ("\n" in gpstxt):
def __init__(self): self.fix_pub = rospy.Publisher('GPS/fix', GPSFix, queue_size=1) self.nmea_pub = rospy.Publisher('GPS/NMEA', String, queue_size=1) self.valid_fix = False self.use_RMC = rospy.get_param('~useRMC', False) self.current_fix = GPSFix() self.current_nmea = String() # epe = estimated position error self.default_epe_quality0 = rospy.get_param('~epe_quality0', 1000000) self.default_epe_quality1 = rospy.get_param('~epe_quality1', 4.0) self.default_epe_quality2 = rospy.get_param('~epe_quality2', 0.1) self.default_epe_quality4 = rospy.get_param('~epe_quality4', 0.02) self.default_epe_quality5 = rospy.get_param('~epe_quality5', 4.0) self.default_epe_quality9 = rospy.get_param('~epe_quality9', 3.0) self.using_receiver_epe = False self.lon_std_dev = float("nan") self.lat_std_dev = float("nan") self.alt_std_dev = float("nan") """Format for this dictionary is the fix type from a GGA message as the key, with each entry containing a tuple consisting of a default estimated position error, a NavSatStatus value, and a NavSatFix covariance value.""" self.gps_qualities = { # Unknown -1: [ self.default_epe_quality0, GPSStatus.STATUS_NO_FIX, GPSFix.COVARIANCE_TYPE_UNKNOWN ], # Invalid 0: [ self.default_epe_quality0, GPSStatus.STATUS_NO_FIX, GPSFix.COVARIANCE_TYPE_UNKNOWN ], # SPS 1: [ self.default_epe_quality1, GPSStatus.STATUS_FIX, GPSFix.COVARIANCE_TYPE_APPROXIMATED ], # DGPS 2: [ self.default_epe_quality2, GPSStatus.STATUS_SBAS_FIX, GPSFix.COVARIANCE_TYPE_APPROXIMATED ], # RTK Fix 4: [ self.default_epe_quality4, GPSStatus.STATUS_GBAS_FIX, GPSFix.COVARIANCE_TYPE_APPROXIMATED ], # RTK Float 5: [ self.default_epe_quality5, GPSStatus.STATUS_GBAS_FIX, GPSFix.COVARIANCE_TYPE_APPROXIMATED ], # WAAS 9: [ self.default_epe_quality9, GPSStatus.STATUS_GBAS_FIX, GPSFix.COVARIANCE_TYPE_APPROXIMATED ] }
class Controller: name = '' looprate = 10 controller = 0 windMsg = Pose2D() gpsMsg = GPSFix() imuMsg = Imu() velMsg = Twist() sailAngle = 0.0 rudderAngle = 0.0 def wakeup(self): self.publishMSG("M") def loop(self): control = self.control() if (control): self.publishCMD(control) self.publishOdom() self.rate.sleep() def gps(self, data): self.gpsMsg = data def imu(self, data): self.imuMsg = data def wind(self, data): self.windMsg = data def sail(self, data): self.sailAngle = data.data def rudder(self, data): self.rudderAngle = data.data def vel(self, data): self.velMsg = data def publishOdom(self): odom_msg = Odometry() odom_msg.pose.pose.position.x = self.gpsMsg.latitude odom_msg.pose.pose.position.y = self.gpsMsg.longitude odom_msg.pose.pose.position.z = self.gpsMsg.altitude odom_msg.pose.pose.orientation = self.imuMsg.orientation odom_msg.twist.twist = self.velMsg self.odomMsg.publish(odom_msg) def publishCMD(self, cmd): self.pubCmd.publish(cmd) def publishMSG(self, msg): self.pubMsg.publish(msg) def __init__(self, name, looprate, mode): self.name = name self.looprate = looprate self.controller = mode rospy.init_node(self.name, anonymous=True) self.rate = rospy.Rate(self.looprate) rospy.Timer(rospy.Duration(30.0), self.wakeup) self.pubCmd = rospy.Publisher('/sailboat/sailboat_cmd', Twist, queue_size=100) self.pubMsg = rospy.Publisher('/sailboat/sailboat_msg', String, queue_size=10) self.odomMsg = rospy.Publisher('/sailboat/odom', Odometry, queue_size=100) self.gpsSub = rospy.Subscriber('/sailboat/GPS', GPSFix, self.gps) self.imuSub = rospy.Subscriber('/sailboat/IMU', Imu, self.imu) self.windSub = rospy.Subscriber('/sailboat/wind', Pose2D, self.wind) self.sailSub = rospy.Subscriber('/sailboat/sail', Float32, self.sail) self.rudderSub = rospy.Subscriber('/sailboat/rudder', Float32, self.rudder) self.velSub = rospy.Subscriber('/sailboat/IMU_Dv', Twist, self.vel) time.sleep(1) self.publishMSG("C" + str(mode))
def spin_once(self): def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi*orient['roll']/180., pi*orient['pitch']/180, pi*orient['yaw']/180.) except KeyError: pass try: m = identity_matrix() m[:3,:3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp = data.get('Temp') # float raw_data = data.get('RAW') imu_data = data.get('Calib') orient_data = data.get('Orient') velocity_data = data.get('Velocity') position_data = data.get('Position') rawgps_data = data.get('RAWGPS') status = data.get('Status') # int sample = data.get('Sample') # TODO by @demmeln: use sample counter for timestamp # correction. Watch out for counter wrap. # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False # fill information where it's due # start by raw information that can be overriden if raw_data: # TODO warn about data not calibrated pub_imu = True pub_vel = True pub_mag = True pub_temp = True # acceleration imu_msg.linear_acceleration.x = raw_data['accX'] imu_msg.linear_acceleration.y = raw_data['accY'] imu_msg.linear_acceleration.z = raw_data['accZ'] imu_msg.linear_acceleration_covariance = (0., )*9 # gyroscopes imu_msg.angular_velocity.x = raw_data['gyrX'] imu_msg.angular_velocity.y = raw_data['gyrY'] imu_msg.angular_velocity.z = raw_data['gyrZ'] imu_msg.angular_velocity_covariance = (0., )*9 vel_msg.twist.angular.x = raw_data['gyrX'] vel_msg.twist.angular.y = raw_data['gyrY'] vel_msg.twist.angular.z = raw_data['gyrZ'] # magnetometer mag_msg.vector.x = raw_data['magX'] mag_msg.vector.y = raw_data['magY'] mag_msg.vector.z = raw_data['magZ'] # temperature # 2-complement decoding and 1/256 resolution x = raw_data['temp'] if x&0x8000: temp_msg.data = (x - 1<<16)/256. else: temp_msg.data = x/256. if rawgps_data: if rawgps_data['bGPS']<self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3 self.old_bGPS = rawgps_data['bGPS'] if temp is not None: pub_temp = True temp_msg.data = temp if imu_data: try: imu_msg.angular_velocity.x = imu_data['gyrX'] imu_msg.angular_velocity.y = imu_data['gyrY'] imu_msg.angular_velocity.z = imu_data['gyrZ'] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = imu_data['gyrX'] vel_msg.twist.angular.y = imu_data['gyrY'] vel_msg.twist.angular.z = imu_data['gyrZ'] pub_vel = True except KeyError: pass try: imu_msg.linear_acceleration.x = imu_data['accX'] imu_msg.linear_acceleration.y = imu_data['accY'] imu_msg.linear_acceleration.z = imu_data['accZ'] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: mag_msg.vector.x = imu_data['magX'] mag_msg.vector.y = imu_data['magY'] mag_msg.vector.z = imu_data['magZ'] pub_mag = True except KeyError: pass if velocity_data: pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] if orient_data: pub_imu = True orient_quat = quat_from_orient(orient_data) imu_msg.orientation.x = orient_quat[0] imu_msg.orientation.y = orient_quat[1] imu_msg.orientation.z = orient_quat[2] imu_msg.orientation.w = orient_quat[3] imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) if position_data: pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] if status is not None: if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) if pub_gps: if status & 0b0100: gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg)