Ejemplo n.º 1
0
def main():

    with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as sbp_udp:
        sbp_udp.setblocking(1)
        sbp_udp.settimeout(None)
        sbp_udp.bind((DEFAULT_UDP_BIND_ADDRESS, DEFAULT_UDP_RECV_PORT))

        def handle_incoming(msg, **metadata):
            sbp_udp.sendto(msg.pack(),
                           (DEFAULT_UDP_SEND_ADDRESS, DEFAULT_UDP_SEND_PORT))

        with PySerialDriver(DEFAULT_SERIAL_PORT,
                            DEFAULT_SERIAL_BAUD) as driver:
            with Handler(Framer(driver.read, driver.write)) as handler:

                handler.add_callback(handle_incoming)

                print "***"
                print "*** Solo Relay Running"
                print "***"

                print "Sending to %s : %s" % (DEFAULT_UDP_SEND_ADDRESS,
                                              DEFAULT_UDP_SEND_PORT)
                print "Recving on %s : %s" % (DEFAULT_UDP_BIND_ADDRESS,
                                              DEFAULT_UDP_RECV_PORT)

                try:
                    while True:
                        data, addr = sbp_udp.recvfrom(4096)
                        driver.write(data)
                except KeyboardInterrupt:
                    pass
Ejemplo n.º 2
0
def reset(port='/dev/ttyUSB0', baud=115200):
    '''
    Resets the Multi.

    Args:
        port: serial port [[default='/dev/ttyUSB0']
        baud: baud rate [default=115200]

    Returns:
        None
    '''

    print('Reading from {} at {}'.format(port, baud))

    verify = raw_input('Are you sure want to reset the Multi? Y/[n]: ') or 'n'
    if verify == 'Y':
        print('Retting the Multi ...')
        with PySerialDriver(port, baud) as driver:
            reset_sbp = SBP(SBP_MSG_RESET)
            reset_sbp.payload = ''
            reset_sbp = reset_sbp.pack()
            driver.write(reset_sbp)
        print('Resetted the Multi.')
    else:
        print('Skipped resetting.')
Ejemplo n.º 3
0
def get_driver(use_ftdi=False,
               port=SERIAL_PORT,
               baud=SERIAL_BAUD,
               file=False,
               rtscts=False):
    """
  Get a driver based on configuration options

  Parameters
  ----------
  use_ftdi : bool
    For serial driver, use the pyftdi driver, otherwise use the pyserial driver.
  port : string
    Serial port to read.
  baud : int
    Serial port baud rate to set.
  """
    try:
        if use_ftdi:
            return PyFTDIDriver(baud)
        if file:
            return open(port, 'rb')
    # HACK - if we are on OSX and the device appears to be a CDC device, open as a binary file
        for each in serial.tools.list_ports.comports():
            if port == each[0]:
                if each[1].startswith("Gadget Serial"):
                    print "opening a file driver"
                    return CdcDriver(open(port, 'w+b', 0))
        return PySerialDriver(port, baud, rtscts=rtscts)
    # if finding the driver fails we should exit with a return code
    # currently sbp's py serial driver raises SystemExit, so we trap it
    # here
    except SystemExit:
        sys.exit(1)
Ejemplo n.º 4
0
  def run(self):
    '''Thread loop here'''
    # Problems? See: https://pylibftdi.readthedocs.org/en/latest/troubleshooting.html
    # with PyFTDIDriver(self.sbp_baud) as driver:
    try:
      with PySerialDriver(self.sbp_port, baud=self.sbp_baud) as driver:
        self.driver = driver
        self.framer = Framer(driver.read, driver.write)
        with Handler(self.framer) as handler:

          self.handler = handler
          handler.add_callback(self.handle_basestation_obs, msg_type=[SBP_MSG_OBS, SBP_MSG_BASE_POS_LLH])
          handler.add_callback(self.handle_surveying, msg_type=[SBP_MSG_POS_LLH])
          
          self.ready()

          try:
            while not self.wait_on_stop(1.0):
              pass 
          except KeyboardInterrupt:
            raise
          except socket.error:
            raise
          except SystemExit:
            print "Exit Forced. We're dead."
            return


    except:
      traceback.print_exc()
      self.ready()
Ejemplo n.º 5
0
def main():
    '''Retreives data from GNSS module and sends it to a node'''
    publisher = rospy.Publisher('swiftnav-gnss', String, queue_size=10)
    rospy.init_node('swiftnav')
    with PySerialDriver(PORT, baud=1000000) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            while not rospy.is_shutdown():
                for msg, _ in source.filter(SBP_MSG_BASELINE_NED):
                    publisher.publish('{},{},{}'.format(
                        msg.n * 1e-3, msg.e * 1e-3, msg.d * 1e-3))
Ejemplo n.º 6
0
def get_imu():
	with PySerialDriver(SERIAL_PORT_IN, baud=SERIAL_BAUD_IN) as driver:
		with Handler(Framer(driver.read, None, verbose=True)) as source:
			try:
				for msg, metadata in source.filter(SBP_MSG_IMU_RAW):
					rate = msg.acc_x/4095.75, msg.acc_y/4095.75, msg.acc_z/4095.75
					break
			except KeyboardInterrupt:
				pass
	return rate
Ejemplo n.º 7
0
    def simple_read():
        with PySerialDriver(USB_Piksi, baud=1000000) as driver:
            with Handler(Framer(driver.read, None, verbose=False)) as source:
                for msg, metadata in source.filter(SBP_MSG_POS_LLH):
                    # Store the latitude, longitude and number of satellites
                    #print "Lat:", msg.lat, "Lon:", msg.lon, "nSats:", msg.n_sats

                    SBPComm.lat, SBPComm.lon = msg.lat, msg.lon
                    SBPComm.height = msg.height
                    SBPComm.n_sats = msg.n_sats
Ejemplo n.º 8
0
def get_position():
	with PySerialDriver(SERIAL_PORT_IN, baud=SERIAL_BAUD_IN) as driver:
		with Handler(Framer(driver.read, None, verbose=True)) as source:
			try:
				for msg, metadata in source.filter(SBP_MSG_POS_LLH):
					pos = msg.lat, msg.lon, msg.flags, msg.n_sats
					break
			except KeyboardInterrupt:
				pass
	return pos
Ejemplo n.º 9
0
 def begin():
     SBPComm._driver = PySerialDriver(SBPComm.port, baud=1000000)
     SBPComm._handler = Handler(
         Framer(SBPComm._driver.read, None, verbose=False))
     msg_types = [
         SBP_MSG_POS_LLH, SBP_MSG_VEL_NED, SBP_MSG_DOPS,
         SBP_MSG_BASELINE_NED, SBP_MSG_BASELINE_HEADING, SBP_MSG_HEARTBEAT
     ]
     SBPComm._handler.add_callback(SBPComm.sbp_cb, msg_types)
     SBPComm._handler.start()
Ejemplo n.º 10
0
 def __init__(self):
     self.data_lock = threading.Lock()
     self.data = None
     # Set up the Duro
     # Assumes the Duro is plugged into the core via serial
     self.d = PySerialDriver('/dev/ttyS0')
     self.f = Framer(self.d.read, None, verbose=True)
     self.h = Handler(self.f)
     self.h.add_callback(self._update_gps_callback, SBP_MSG_POS_LLH_DEP_A)
     self.h.add_callback(self._update_gps_callback, SBP_MSG_POS_LLH)
     self.h.start()
Ejemplo n.º 11
0
def main():
  args = get_args()

  with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
    with Handler(driver.read, driver.write) as link:
      with ByteLogger(args.filename[0]) as logger:
        link.add_callback(logger)
        link.start

        try:
          while True:
            time.sleep(0.1)
        except KeyboardInterrupt:
          pass
Ejemplo n.º 12
0
    def run_application(self):
        
        self.port = Window.port
        self.logFile = Window.logFile
        self.buad = 115200

        # Open a connection to Piksi using the default baud rate (115200) and port (/dev/ttyUSB0)
        driver = PySerialDriver(self.port, self.buad)
        source = Handler(Framer(driver.read, None, verbose=True))
                         
        # Use SBP built in callback function to create callback for posLLH messages
        source.add_callback(self.posLLH, SBP_MSG_POS_LLH)
        #source.add_callback(baselineCallback, SBP_MSG_BASELINE_NED)
        source.start()
Ejemplo n.º 13
0
def main():
    args = get_args()

    with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
        with Handler(driver.read, driver.write) as handler:
            with closing(socket.socket(socket.AF_INET,
                                       socket.SOCK_DGRAM)) as udp:
                handler.add_callback(send_udp_callback_generator(udp, args))
                handler.start()

                try:
                    while True:
                        time.sleep(0.1)
                except KeyboardInterrupt:
                    pass
Ejemplo n.º 14
0
def main():
    args = get_args()
    address = args.address[0]
    udp_port = args.udp_port[0]

    with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
        with Handler(driver.read, driver.write) as handler:
            with UdpLogger(address, udp_port) as udp:
                handler.add_callback(udp)

                try:
                    while True:
                        time.sleep(0.1)
                except KeyboardInterrupt:
                    pass
Ejemplo n.º 15
0
def get_driver(use_ftdi=False, port=SERIAL_PORT, baud=SERIAL_BAUD):
  """
  Get a driver based on configuration options

  Parameters
  ----------
  use_ftdi : bool
    For serial driver, use the pyftdi driver, otherwise use the pyserial driver.
  port : string
    Serial port to read.
  baud : int
    Serial port baud rate to set.
  """
  if use_ftdi:
    return PyFTDIDriver(baud)
  return PySerialDriver(port, baud)
Ejemplo n.º 16
0
def main():
    parser = argparse.ArgumentParser(
        description="Swift Navigation SBP Example.")
    parser.add_argument("-p",
                        "--port",
                        default=['/dev/ttyUSB0'],
                        nargs=1,
                        help="specify the serial port to use.")
    parser.add_argument("-b",
                        "--baud",
                        default=[115200],
                        nargs=1,
                        help="specify the baud rate")
    parser.add_argument("-f",
                        "--filename",
                        default=[DEFAULT_LOG_FILENAME],
                        nargs=1,
                        help="specify the name of the log file")
    args = parser.parse_args()
    print args

    # Open a connection to Piksi using the default baud rate (1Mbaud)
    # driver =  PySerialDriver(args.port[0], args.baud[0])
    # source =  Handler(Framer(driver.read, None, verbose=True))

    # source.add_callback(posLLHCallback, SBP_MSG_POS_LLH)
    # source.add_callback(baselineCallback, SBP_MSG_BASE_POS_LLH)
    # source.start()
    # print SBP_MSG_BASE_POS_LLH

    # Open a connection to Piksi using the default baud rate (1Mbaud)
    with PySerialDriver(args.port[0], args.baud[0]) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                for msg, metadata in source.filter(SBP_MSG_BASELINE_NED):
                    # Print out the N, E, D coordinates of the baseline
                    print msg

            except KeyboardInterrupt:
                pass

    try:
        while (1):
            time.sleep(.01)
    except KeyboardInterrupt:
        pass
Ejemplo n.º 17
0
def main():
    parser = argparse.ArgumentParser(description="Swift Navigation SBP Example.")
    parser.add_argument("-p", "--port",
                      default=['/dev/ttyUSB0'], nargs=1,
                      help="specify the serial port to use.")
    args = parser.parse_args()

    # Open a connection to Piksi using the default baud rate (1Mbaud)
    with PySerialDriver(args.port[0], baud=1000000) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                #for msg, metadata in source.filter(SBP_MSG_BASELINE_NED):
                for msg, metadata in source:
                    # Print out the N, E, D coordinates of the baseline
                    #print "%.4f,%.4f,%.4f" % (msg.n * 1e-3, msg.e * 1e-3, msg.d * 1e-3)
                    print msg

            except KeyboardInterrupt:
                pass
Ejemplo n.º 18
0
def main():
    """Simple command line interface for running the udp bridge to
    forward observation messages

    """
    args = get_args()
    port = int(args.udp_port[0])
    address = args.address[0]
    with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
        with Handler(Framer(driver.read, driver.write)) as handler:
            with UdpLogger(address, port) as udp:
                handler.add_callback(udp, OBS_MSGS)
                # Note, we may want to send the ephemeris message in the future
                # but the message is too big for MAVProxy right now
                try:
                    while True:
                        time.sleep(0.1)
                except KeyboardInterrupt:
                    pass
def radio_corrections(q_radio):
    global radio_sender  # get radio sender id
    with PySerialDriver(RADIO_PORT, baud=RADIO_BAUDRATE) as driver:
        print(driver.read)
        with Handler(Framer(driver.read, None, verbose=False)) as source:
            try:
                for sbp_msg, metadata in source.filter():
                    if radio_sender is None:
                        radio_sender = sbp_msg.sender  # get ntrip sender id

                    if sbp_msg.msg_type == sbp.observation.SBP_MSG_OBS:
                        q_radio.put(sbp_msg)
                    else:
                        if ntrip_sender is not None:
                            sbp_msg.sender = ntrip_sender
                        udp.call(sbp_msg)

            except KeyboardInterrupt:
                pass
Ejemplo n.º 20
0
def test_tcp_logger():
  handler = tcp_handler(MsgPrint(text='abc').to_binary())
  ip, port = tcp_server(handler)
  port = "socket://%s:%s" % (ip, port)
  baud = 115200
  t0 = time.time()
  sleep = 0.1
  def assert_logger(s):
    assert s.preamble==0x55
    assert s.msg_type==0x10
    assert s.sender==66
    assert s.length==3
    assert s.payload=='abc'
    assert s.crc==0xDAEE
  with PySerialDriver(port, baud) as driver:
    with Handler(driver.read, driver.write, verbose=False) as link:
      link.add_callback(assert_logger)
      while True:
        if (time.time() - t0) < sleep:
          break
Ejemplo n.º 21
0
def main():
    global source, exitFlag

    rospy.init_node("dGPS_Data", anonymous=True)

    parser = argparse.ArgumentParser(
        description="Swift Navigation SBP Example.")
    parser.add_argument("-p",
                        "--port",
                        default=['/dev/ttyUSB0'],
                        nargs=1,
                        help="specify the serial port to use.")
    args = parser.parse_args()

    # Open a connection to Piksi using the default baud rate (1Mbaud)
    with PySerialDriver(args.port[0], baud=1000000) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:

                t1 = threading.Thread(name='baseline_NED', target=baseline_NED)
                t2 = threading.Thread(name='Vel_NED', target=velocity_NED)
                t3 = threading.Thread(name='dop_Info', target=dop_Info)
                t4 = threading.Thread(name='gps_Pos', target=gps_Pos)

                dataCollectThread = threading.Thread(name='dataCollect',
                                                     target=dataCollect)

                t1.start()
                t2.start()
                t3.start()
                t4.start()

                dataCollectThread.start()

                rospy.spin()

            except KeyboardInterrupt:
                exitFlag = True
                raise SystemExit
Ejemplo n.º 22
0
def get_driver(use_ftdi=False, port=SERIAL_PORT, baud=SERIAL_BAUD):
    """
  Get a driver based on configuration options

  Parameters
  ----------
  use_ftdi : bool
    For serial driver, use the pyftdi driver, otherwise use the pyserial driver.
  port : string
    Serial port to read.
  baud : int
    Serial port baud rate to set.
  """
    try:
        if use_ftdi:
            return PyFTDIDriver(baud)
        return PySerialDriver(port, baud)
    # if finding the driver fails we should exit with a return code
    # currently sbp's py serial driver raises SystemExit, so we trap it
    # here
    except SystemExit:
        sys.exit(1)
Ejemplo n.º 23
0
def main():
    args = get_args()
    app = args.app[0]
    key = args.key[0]
    secret = args.secret[0]
    port = args.port[0]
    baud = args.baud[0]
    rx_channel, rx_event = args.rx_channel_event[0].split(':')
    tx_channel, tx_event = args.tx_channel_event[0].split(':')
    push = pusher.Pusher(app_id=app,
                         key=key,
                         secret=secret,
                         ssl=True,
                         port=443)
    push_client = pusherclient.Pusher(key, secret=secret)
    with PySerialDriver(port, baud) as driver:
        with Handler(Framer(driver.read, driver.write)) as handler:

            def push_it(sbp_msg):
                push.trigger(tx_channel, tx_event, sbp_msg.to_json_dict())

            def pull_it(data):
                handler(SBP.from_json(data))

            def connect_it(data):
                push_client.subscribe(rx_channel).bind(rx_event, pull_it)

            push_client.connection.bind('pusher:connection_established',
                                        connect_it)
            push_client.connect()

            try:
                for msg, metadata in handler.filter(OBS_MSG_LIST):
                    push_it(msg)
            except KeyboardInterrupt:
                pass
Ejemplo n.º 24
0
    def navigate(self):
        # Select Driver
        if self.args.u is False:
            driver = TCPDriver('10.0.0.223', '55555')
        else:
            driver = PySerialDriver('/dev/ttyUSB0',
                                    baud=115200)  # 68,72,73,74,65535

        # Location of rover, from the top.
        # driver.read is the foundliteral output from the tcp driver
        # framer turns bytes into SBP messages (swift binary protocol)
        # handler is an iterable to read the messages
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            # reads all the messages in a loop as they are received
            for i in range(10):
                msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
                self.history[i] = msg

            # Search for a tennis ball
            bt = BallTracker.BallTracker()
            print bt

            # Start Navigation Loop
            while True:
                # Get Next Driver Message
                msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
                for i in range(1, 10):
                    self.history[i - 1] = self.history[i]
                self.history[9] = msg
                # Shouldn't We need an elevation?.. that's msg.height
                # int of accuracy is [h/v]_accuracy also there is n_sats
                self.last_rover_latitude = self.history[0].lat
                self.last_rover_longitude = self.history[0].lon
                self.rover_latitude = math.radians(msg.lat)
                self.rover_longitude = math.radians(msg.lon)

                # Get target_longitude, target_latitude,217
                # (not yet...height, and number of satellites) - need it in degrees
                if self.target_bearing is None and self.distance is None:
                    self.target_longitude = math.radians(
                        float(self.target_longitude))
                    self.target_latitude = math.radians(
                        float(self.target_latitude))

                # If target_bearing and distance were supplied, get target_latitude and target_longitude in degrees
                else:
                    self.distance = float(self.distance)
                    self.target_bearing = self.get_target_bearing()
                    self.target_latitude, self.target_longitude = self.get_gps_coordinates(
                    )
                    self.target_latitude = math.radians(self.target_latitude)
                    self.target_longitude = math.radians(self.target_longitude)

                self.distance = self.get_distance()
                self.leg_distance = self.distance
                self.distance_traveled = (self.leg_distance -
                                          self.distance) / self.leg_distance
                theta = self.get_target_bearing()
                delta_theta = self.get_rover_bearing() - theta

                # Get velocity in North, East, Down format
                msg2, metadata = source.filter(SBP_MSG_VEL_NED).next()

                # Velocity is in mm/s, so convert it to m/s
                vel__north = msg2.n * 1000
                vel__east = msg2.e * 1000

                #print("North: ", vel__north, " East: ", vel__east)

                # Calculate rover_heading using the atan2 function
                rover_heading = math.atan2(vel__east, vel__north)

                # TODO: turn(target_bearing) # something like: if math.abs(target_bearing) > tolerance
                # TODO: then turn else drive (distance) drive(distance)

                # Need to fix this to output the proper value of theta
                #print("Bearing: ", math.degrees(theta), ", Distance: ", self.distance,
                #     ",Turn:", math.degrees(delta_theta), "My Bearing:", math.degrees(rover_heading))
                #print "Ball found: " + str(bt.hasFound()) + ", ball angle: " + str(bt.getAngle()) + ", ball dist: " + str(bt.distance)
                if self.distance_traveled > 0.75:
                    found = bt.hasFound()
                    if found is True:
                        print "ball found"
                        # Give control to the ball tracker
                        bt.setControl(True)
                        self.hasControl = False
                    else:
                        #Give control to the GPS code
                        bt.setControl(False)
                        self.hasControl = True

                if self.hasControl is True or True:
                    # Drive the rover via GPS
                    self.angle = math.degrees(delta_theta)
                    #self.speed = 25
                    # If we're within 2 meters of the destination
                    if self.get_distance() <= 2:
                        print(
                            "Distance is within 2 meters of the destination.")
                        # A tennis ball wasn't found on the way to the destination.
                        # Signal that the rover122.41883 has reached the target destination.
                        self.speed = 0
                    else:
                        print("distance is " + str(self.get_distance()) +
                              " angle is " + str(math.degrees(self.angle)))
                        self.speed = 25

                if bt.hasFound() is True and False:
                    print "BT has controll"
                    # Drive the rover via Ball Tracker
                    self.angle = bt.getAngle()
                    # If we're within 2 meters of the destination
                    if bt.distance <= 2:
                        print(
                            "Distance is within 2 meters of the destination.")
                        # A tennis ball wasn't found on the way to the destination.
                        # Signal that the rover has reached the target destination.
                        self.speed = 0
                    else:
                        self.speed = 25
Ejemplo n.º 25
0
    def __init__(self):
        # Print info.
        rospy.sleep(0.5)  # wait for a while for init to complete before printing
        rospy.loginfo(rospy.get_name() + " start")
        rospy.loginfo("libsbp version currently used: " + sbp.version.get_git_version())

        if Piksi.LIB_SBP_VERSION != sbp.version.get_git_version():
            rospy.logwarn("Lib SBP version in usage (%s) is different than the one used to test this driver (%s)!" % (
                sbp.version.get_git_version(), Piksi.LIB_SBP_VERSION))

        # Open a connection to Piksi.
        serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
        baud_rate = rospy.get_param('~baud_rate', 1000000)

        try:
            self.driver = PySerialDriver(serial_port, baud=baud_rate)
        except SystemExit:
            rospy.logerr("Piksi not found on serial port '%s'", serial_port)

        # Create a handler to connect Piksi driver to callbacks.
        self.framer = Framer(self.driver.read, self.driver.write, verbose=True)
        self.handler = Handler(self.framer)

        self.debug_mode = rospy.get_param('~debug_mode', False)
        if self.debug_mode:
            rospy.loginfo("Piksi driver started in debug mode, every available topic will be published.")
        else:
            rospy.loginfo("Piksi driver started in normal mode.")

        # Corrections over WiFi settings.
        self.base_station_mode = rospy.get_param('~base_station_mode', False)
        self.udp_broadcast_addr = rospy.get_param('~broadcast_addr', '255.255.255.255')
        self.udp_port = rospy.get_param('~broadcast_port', 26078)
        self.base_station_ip_for_latency_estimation = rospy.get_param(
            '~base_station_ip_for_latency_estimation',
            '10.10.50.1')
        # Subscribe to OBS messages and relay them via UDP if in base station mode.
        if self.base_station_mode:
            rospy.loginfo("Starting in base station mode")
            self._multicaster = UdpHelpers.SbpUdpMulticaster(self.udp_broadcast_addr, self.udp_port)

            self.handler.add_callback(self.callback_sbp_obs, msg_type=SBP_MSG_OBS)
            self.handler.add_callback(self.callback_sbp_obs_dep_a, msg_type=SBP_MSG_OBS_DEP_A)
            self.handler.add_callback(self.callback_sbp_obs_dep_b, msg_type=SBP_MSG_OBS_DEP_B)
            # not sure if SBP_MSG_BASE_POS_LLH or SBP_MSG_BASE_POS_ECEF is better?
            self.handler.add_callback(self.callback_sbp_base_pos_llh, msg_type=SBP_MSG_BASE_POS_LLH)
            self.handler.add_callback(self.callback_sbp_base_pos_ecef, msg_type=SBP_MSG_BASE_POS_ECEF)
        else:
            rospy.loginfo("Starting in client station mode")
            self._multicast_recv = UdpHelpers.SbpUdpMulticastReceiver(self.udp_port, self.multicast_callback)

        # Navsatfix settings.
        self.var_spp = rospy.get_param('~var_spp', [25.0, 25.0, 64.0])
        self.var_rtk_float = rospy.get_param('~var_rtk_float', [25.0, 25.0, 64.0])
        self.var_rtk_fix = rospy.get_param('~var_rtk_fix', [0.0049, 0.0049, 0.01])
        self.navsatfix_frame_id = rospy.get_param('~navsatfix_frame_id', 'gps')

        # Local ENU frame settings
        self.origin_enu_set = False
        self.latitude0 = 0.0
        self.longitude0 = 0.0
        self.altitude0 = 0.0
        self.initial_ecef_x = 0.0
        self.initial_ecef_y = 0.0
        self.initial_ecef_z = 0.0
        self.ecef_to_ned_matrix = np.eye(3)
        self.enu_frame_id = rospy.get_param('~enu_frame_id', 'enu')
        self.transform_child_frame_id = rospy.get_param('~transform_child_frame_id', 'gps_receiver')

        if rospy.has_param('~latitude0_deg') and rospy.has_param('~longitude0_deg') and rospy.has_param(
                '~altitude0_deg'):
            latitude0 = rospy.get_param('~latitude0_deg')
            longitude0 = rospy.get_param('~longitude0_deg')
            altitude0 = rospy.get_param('~altitude0_deg')

            # Set origin ENU frame to coordinate specified by rosparam.
            self.init_geodetic_reference(latitude0, longitude0, altitude0)
            rospy.loginfo("Origin ENU frame set by rosparam.")

        # Advertise topics.
        self.publishers = self.advertise_topics()

        # Create callbacks.
        self.create_callbacks()

        # Init messages with "memory".
        self.receiver_state_msg = self.init_receiver_state_msg()
        self.num_wifi_corrections = self.init_num_corrections_msg()

        # corrections over wifi message, if we are not the base station.
        if not self.base_station_mode:
            # start new thread to periodically ping base station.
            threading.Thread(target=self.ping_base_station_over_wifi).start()

        self.handler.start()

        # Spin.
        rospy.spin()
Ejemplo n.º 26
0
        pub_piksidebug.publish(debug_msg)

# Main function.    
if __name__ == '__main__':   
    rospy.init_node('piksi')

    #Print info
    rospy.sleep(0.5) #wait for a while for init to complete before printing
    rospy.loginfo("init_node")
    rospy.loginfo("libsbp version currently used: " + sbp.version.get_git_version())

    # Open a connection to Piksi using the default baud rate (1Mbaud)
    serial_port = rospy.get_param('~serial_port')

    try:
        driver = PySerialDriver(serial_port, baud=1000000)
    except SystemExit:
        rospy.logerr("Piksi not found on serial port '%s'", serial_port)
    
    # Create a handler to connect Piksi driver to callbacks
    handler = Handler(Framer(driver.read, driver.write, verbose=True))

    # Read settings
    var_spp_x = rospy.get_param('~var_spp_x')
    var_spp_y = rospy.get_param('~var_spp_y')
    var_spp_z = rospy.get_param('~var_spp_z')
    var_rtk_float_x = rospy.get_param('~var_rtk_float_x')
    var_rtk_float_y = rospy.get_param('~var_rtk_float_y')
    var_rtk_float_z = rospy.get_param('~var_rtk_float_z')
    var_rtk_fix_x = rospy.get_param('~var_rtk_fix_x')
    var_rtk_fix_y = rospy.get_param('~var_rtk_fix_y')
Ejemplo n.º 27
0
def main():
    args = get_args()
    rover_lat = 0.0
    rover_long = 0.0
    if args.u is False:
        driver = TCPDriver('192.168.1.222', '55555')
    else:
        driver = PySerialDriver('/dev/ttyUSB0', baud=115200)  # 68,72,73,74,65535
    # Location of rover, from the top.
    # driver.read is the literal output from the tcp driver
    # framer turns bytes into SBP messages (swift binary protocol)
    # handler is an iterable to read the messages
    with Handler(Framer(driver.read, None, verbose=True)) as source:
        # reads all the messages in a loop as they are received
        for i in range(1, 10):
            msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
            history[i] = msg
        while True:
            msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
            for i in range(1, 10):
                history[i-1] = history [i]
            history[9] = msg
            # Shouldnt We need an elevation?.. that's msg.height
            # int of acuracy is [h/v]_acuracy also there is n_sats
            last_lat = math.radians(history[0].lat)
            last_long = math.radians(history[0].lon)
            rover_lat = math.radians(msg.lat)
            rover_long = math.radians(msg.lon)

            # Get longitude, latitude, (not yet...height, and number of satellites) - need it in degrees
            if args.bearing is None and args.distance is None:
                longitude = math.radians(float(args.longitude))
                latitude = math.radians(float(args.latitude))

            # If bearing and distance were supplied, get latitude and longitude in degrees
            else:
                bearing = float(args.bearing)
                distance = float(args.distance)
                latitude, longitude = get_gps_coordinate(bearing, distance, rover_lat, rover_long)
                latitude = math.radians(latitude)
                longitude = math.radians(longitude)

            distance = get_distance(longitude, latitude, rover_lat, rover_long)
            theta = get_bearing(longitude, latitude, rover_lat, rover_long)
            delta_theta = get_my_bearing(rover_lat, rover_long, last_lat, last_long) - theta

            # Get velocity in North, East, Down format
            msg2, metadata = source.filter(SBP_MSG_VEL_NED).next()

            # Velocity is in mm/s, so convert it to m/s
            vel__north = msg2.n * 1000
            vel__east = msg2.e * 1000

            print("North: ", vel__north, " East: ", vel__east)

            # Calculate my_heading using the atan2 function
            my_heading = math.atan2(vel__east, vel__north)

            # TODO: turn(bearing) # something like: if math.abs(bearing) > tolerance then turn else drive (distance)
            # TODO: drive(distance)

            # Need to fix this to output the proper value of theta
            print("Bearing: ", math.degrees(theta), ", Distance: ", distance, ",Turn:", math.degrees(delta_theta),
                  "My Bearing:", math.degrees(my_heading))

            # If we're within 2 meters of the destination
            if distance <= 2:
                print("Distance is within 2 meters of the destination.")
Ejemplo n.º 28
0
def read_rtk(port='/dev/ttyUSB0', baud=115200):
    '''
    Reads the RTK output from SwiftNav Piksi, parses the messege and prints.
    Piksi's must be configured to give RTK message through the serial port.
    NOTE: Current official sbp drivers only support python-2

    Args:
        port: serial port [[default='/dev/ttyUSB0']
        baud: baud rate [default=115200]

    Returns:
        None
    '''

    print('Reading from {} at {}'.format(port, baud))

    m = RtkMessage()
    # t_now = datetime.now().strftime('%Y%m%d%H%M%S')
    # out_file = 'GPS_' + t_now + '.txt'

    # open a connection to Piksi
    # with open(out_file, 'w') as f:
    with PySerialDriver(port, baud) as driver:
        with Handler(Framer(driver.read, None, verbose=True)) as source:
            try:
                msg_list = [
                    SBP_MSG_BASELINE_NED, SBP_MSG_POS_LLH, SBP_MSG_VEL_NED,
                    SBP_MSG_GPS_TIME
                ]
                for msg, metadata in source.filter(msg_list):

                    # LLH position in deg-deg-m
                    if msg.msg_type == 522:
                        m.lat = msg.lat
                        m.lon = msg.lon
                        m.h = msg.height

                    # RTK position in mm (from base to rover)
                    elif msg.msg_type == 524:
                        m.n = msg.n
                        m.e = msg.e
                        m.d = msg.d
                        m.flag = msg.flags

                    # RTK velocity in mm/s
                    elif msg.msg_type == 526:
                        m.v_n = msg.n
                        m.v_e = msg.e
                        m.v_d = msg.d

                    # GPS time
                    elif msg.msg_type == 258:
                        m.wn = msg.wn
                        m.tow = msg.tow  # in millis

                    else:
                        pass

                    print(m.whole_string())
                    # f.write(line)
                    # f.write('\n')

            except KeyboardInterrupt:
                pass

    return
Ejemplo n.º 29
0
class PiksiMulti:
    LIB_SBP_VERSION_MULTI = '2.2.1'  # SBP version used for Piksi Multi.

    # Geodetic Constants.
    kSemimajorAxis = 6378137
    kSemiminorAxis = 6356752.3142
    kFirstEccentricitySquared = 6.69437999014 * 0.001
    kSecondEccentricitySquared = 6.73949674228 * 0.001
    kFlattening = 1 / 298.257223563

    def __init__(self):

        # Print info.
        rospy.sleep(
            0.5)  # Wait for a while for init to complete before printing.
        rospy.loginfo(rospy.get_name() + " start")
        rospy.loginfo("libsbp version currently used: " +
                      sbp.version.get_git_version())

        # Check for correct SBP library version dependent on Piksi device.
        if PiksiMulti.LIB_SBP_VERSION_MULTI != sbp.version.get_git_version():
            rospy.logwarn(
                "Lib SBP version in usage (%s) is different than the one used to test this driver (%s)!"
                % (sbp.version.get_git_version(),
                   PiksiMulti.LIB_SBP_VERSION_MULTI))

        # Open a connection to Piksi.
        serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
        baud_rate = rospy.get_param('~baud_rate', 115200)

        try:
            self.driver = PySerialDriver(serial_port, baud=baud_rate)
        except SystemExit:
            rospy.logerr("Piksi not found on serial port '%s'", serial_port)
            raise

        # Create a handler to connect Piksi driver to callbacks.
        self.framer = Framer(self.driver.read, self.driver.write, verbose=True)
        self.handler = Handler(self.framer)

        self.debug_mode = rospy.get_param('~debug_mode', False)
        if self.debug_mode:
            rospy.loginfo(
                "Piksi driver started in debug mode, every available topic will be published."
            )
        else:
            rospy.loginfo("Piksi driver started in normal mode.")

        # Corrections over WiFi settings.
        self.base_station_mode = rospy.get_param('~base_station_mode', False)
        self.udp_broadcast_addr = rospy.get_param('~broadcast_addr',
                                                  '255.255.255.255')
        self.udp_port = rospy.get_param('~broadcast_port', 26078)
        self.base_station_ip_for_latency_estimation = rospy.get_param(
            '~base_station_ip_for_latency_estimation', '10.10.10.1')
        self.multicaster = []
        self.multicast_recv = []

        # Navsatfix settings.
        self.var_spp = rospy.get_param('~var_spp', [25.0, 25.0, 64.0])
        self.var_rtk_float = rospy.get_param('~var_rtk_float',
                                             [25.0, 25.0, 64.0])
        self.var_rtk_fix = rospy.get_param('~var_rtk_fix',
                                           [0.0049, 0.0049, 0.01])
        self.navsatfix_frame_id = rospy.get_param('~navsatfix_frame_id', 'gps')

        # Local ENU frame settings.
        self.origin_enu_set = False
        self.latitude0 = 0.0
        self.longitude0 = 0.0
        self.altitude0 = 0.0
        self.initial_ecef_x = 0.0
        self.initial_ecef_y = 0.0
        self.initial_ecef_z = 0.0
        self.ecef_to_ned_matrix = np.eye(3)
        self.enu_frame_id = rospy.get_param('~enu_frame_id', 'enu')
        self.transform_child_frame_id = rospy.get_param(
            '~transform_child_frame_id', 'gps_receiver')

        if rospy.has_param('~latitude0_deg') and rospy.has_param(
                '~longitude0_deg') and rospy.has_param('~altitude0'):
            latitude0 = rospy.get_param('~latitude0_deg')
            longitude0 = rospy.get_param('~longitude0_deg')
            altitude0 = rospy.get_param('~altitude0')

            # Set origin ENU frame to coordinate specified by rosparam.
            self.init_geodetic_reference(latitude0, longitude0, altitude0)
            rospy.loginfo("Origin ENU frame set by rosparam.")

        # Advertise topics.
        self.publishers = self.advertise_topics()

        # Create callbacks.
        self.create_callbacks()

        # Init messages with "memory".
        self.receiver_state_msg = self.init_receiver_state_msg()
        self.num_wifi_corrections = self.init_num_corrections_msg()

        # Corrections over wifi message, if we are not the base station.
        if not self.base_station_mode:
            # Start new thread to periodically ping base station.
            threading.Thread(target=self.ping_base_station_over_wifi).start()

        self.handler.start()

        # Reset service.
        self.reset_piksi_service = rospy.Service(
            rospy.get_name() + '/reset_piksi', std_srvs.srv.SetBool,
            self.reset_piksi_service_callback)

        # Watchdog timer info
        self.watchdog_time = rospy.get_rostime()
        self.messages_started = False

        # Only have start-up reset in base station mode
        if self.base_station_mode:
            # Things have 30 seconds to start or we will kill node
            rospy.Timer(rospy.Duration(30), self.watchdog_callback, True)

        # Spin.
        rospy.spin()

    def create_callbacks(self):
        # Callbacks implemented "manually".
        self.handler.add_callback(self.pos_llh_callback,
                                  msg_type=SBP_MSG_POS_LLH)
        self.handler.add_callback(self.heartbeat_callback,
                                  msg_type=SBP_MSG_HEARTBEAT)
        self.handler.add_callback(self.tracking_state_callback,
                                  msg_type=SBP_MSG_TRACKING_STATE)
        self.handler.add_callback(self.uart_state_callback,
                                  msg_type=SBP_MSG_UART_STATE)

        # Callbacks generated "automatically".
        self.init_callback('baseline_ecef_multi', BaselineEcef,
                           SBP_MSG_BASELINE_ECEF, MsgBaselineECEF, 'tow', 'x',
                           'y', 'z', 'accuracy', 'n_sats', 'flags')
        self.init_callback('baseline_ned_multi', BaselineNed,
                           SBP_MSG_BASELINE_NED, MsgBaselineNED, 'tow', 'n',
                           'e', 'd', 'h_accuracy', 'v_accuracy', 'n_sats',
                           'flags')
        self.init_callback('dops_multi', DopsMulti, SBP_MSG_DOPS, MsgDops,
                           'tow', 'gdop', 'pdop', 'tdop', 'hdop', 'vdop',
                           'flags')
        self.init_callback('gps_time_multi', GpsTimeMulti, SBP_MSG_GPS_TIME,
                           MsgGPSTime, 'wn', 'tow', 'ns_residual', 'flags')
        self.init_callback('utc_time_multi', UtcTimeMulti, SBP_MSG_UTC_TIME,
                           MsgUtcTime, 'flags', 'tow', 'year', 'month', 'day',
                           'hours', 'minutes', 'seconds', 'ns')
        self.init_callback('pos_ecef_multi', PosEcef, SBP_MSG_POS_ECEF,
                           MsgPosECEF, 'tow', 'x', 'y', 'z', 'accuracy',
                           'n_sats', 'flags')
        self.init_callback('vel_ecef', VelEcef, SBP_MSG_VEL_ECEF, MsgVelECEF,
                           'tow', 'x', 'y', 'z', 'accuracy', 'n_sats', 'flags')
        self.init_callback('vel_ned', VelNed, SBP_MSG_VEL_NED, MsgVelNED,
                           'tow', 'n', 'e', 'd', 'h_accuracy', 'v_accuracy',
                           'n_sats', 'flags')
        self.init_callback('imu_raw', ImuRawMulti, SBP_MSG_IMU_RAW, MsgImuRaw,
                           'tow', 'tow_f', 'acc_x', 'acc_y', 'acc_z', 'gyr_x',
                           'gyr_y', 'gyr_z')
        self.init_callback('imu_aux', ImuAuxMulti, SBP_MSG_IMU_AUX, MsgImuAux,
                           'imu_type', 'temp', 'imu_conf')
        self.init_callback('log', Log, SBP_MSG_LOG, MsgLog, 'level', 'text')

        # do not publish llh message, prefer publishing directly navsatfix_spp or navsatfix_rtk_fix.
        # self.init_callback('pos_llh', PosLlh,
        #                   SBP_MSG_POS_LLH, MsgPosLLH,
        #                   'tow', 'lat', 'lon', 'height', 'h_accuracy', 'v_accuracy', 'n_sats', 'flags')

        # Subscribe to OBS messages and relay them via UDP if in base station mode.
        if self.base_station_mode:
            rospy.loginfo("Starting in base station mode")
            self.multicaster = UdpHelpers.SbpUdpMulticaster(
                self.udp_broadcast_addr, self.udp_port)

            self.handler.add_callback(self.callback_sbp_obs,
                                      msg_type=SBP_MSG_OBS)
            # not sure if SBP_MSG_BASE_POS_LLH or SBP_MSG_BASE_POS_ECEF is better?
            #self.handler.add_callback(self.callback_sbp_base_pos_llh, msg_type=SBP_MSG_BASE_POS_LLH)
            self.handler.add_callback(self.callback_sbp_base_pos_ecef,
                                      msg_type=SBP_MSG_BASE_POS_ECEF)
        else:
            rospy.loginfo("Starting in client station mode")
            self.multicast_recv = UdpHelpers.SbpUdpMulticastReceiver(
                self.udp_port, self.multicast_callback)

    def init_num_corrections_msg(self):
        num_wifi_corrections = InfoWifiCorrections()
        num_wifi_corrections.header.seq = 0
        num_wifi_corrections.received_corrections = 0
        num_wifi_corrections.latency = -1

        return num_wifi_corrections

    def init_receiver_state_msg(self):
        receiver_state_msg = ReceiverState()
        receiver_state_msg.num_sat = 0  # Unkown.
        receiver_state_msg.rtk_mode_fix = False  # Unkown.
        receiver_state_msg.sat = []  # Unkown.
        receiver_state_msg.cn0 = []  # Unkown.
        receiver_state_msg.tracking_running = []  # Unkown.
        receiver_state_msg.system_error = 255  # Unkown.
        receiver_state_msg.io_error = 255  # Unkown.
        receiver_state_msg.swift_nap_error = 255  # Unkown.
        receiver_state_msg.external_antenna_present = 255  # Unkown.

        return receiver_state_msg

    def advertise_topics(self):
        """
        Adverties topics.
        :return: python dictionary, with topic names used as keys and publishers as values.
        """
        publishers = {}

        publishers['rtk_fix'] = rospy.Publisher(rospy.get_name() +
                                                '/navsatfix_rtk_fix',
                                                NavSatFix,
                                                queue_size=10)
        publishers['spp'] = rospy.Publisher(rospy.get_name() +
                                            '/navsatfix_spp',
                                            NavSatFix,
                                            queue_size=10)
        publishers['heartbeat'] = rospy.Publisher(rospy.get_name() +
                                                  '/heartbeat',
                                                  Heartbeat,
                                                  queue_size=10)
        publishers['tracking_state'] = rospy.Publisher(rospy.get_name() +
                                                       '/tracking_state',
                                                       TrackingState,
                                                       queue_size=10)
        publishers['receiver_state'] = rospy.Publisher(rospy.get_name() +
                                                       '/debug/receiver_state',
                                                       ReceiverState,
                                                       queue_size=10)
        publishers['uart_state_multi'] = rospy.Publisher(rospy.get_name() +
                                                         '/debug/uart_state',
                                                         UartState,
                                                         queue_size=10)
        # Do not publish llh message, prefer publishing directly navsatfix_spp or navsatfix_rtk_fix.
        # publishers['pos_llh'] = rospy.Publisher(rospy.get_name() + '/pos_llh',
        #                                        PosLlh, queue_size=10)
        publishers['vel_ned'] = rospy.Publisher(rospy.get_name() + '/vel_ned',
                                                VelNed,
                                                queue_size=10)
        publishers['log'] = rospy.Publisher(rospy.get_name() + '/log',
                                            Log,
                                            queue_size=10)
        # Points in ENU frame.
        publishers['enu_pose_fix'] = rospy.Publisher(rospy.get_name() +
                                                     '/enu_pose_fix',
                                                     PoseWithCovarianceStamped,
                                                     queue_size=10)
        publishers['enu_point_fix'] = rospy.Publisher(rospy.get_name() +
                                                      '/enu_point_fix',
                                                      PointStamped,
                                                      queue_size=10)
        publishers['enu_transform_fix'] = rospy.Publisher(rospy.get_name() +
                                                          '/enu_transform_fix',
                                                          TransformStamped,
                                                          queue_size=10)
        publishers['enu_pose_spp'] = rospy.Publisher(rospy.get_name() +
                                                     '/enu_pose_spp',
                                                     PoseWithCovarianceStamped,
                                                     queue_size=10)
        publishers['enu_point_spp'] = rospy.Publisher(rospy.get_name() +
                                                      '/enu_point_spp',
                                                      PointStamped,
                                                      queue_size=10)
        publishers['enu_transform_spp'] = rospy.Publisher(rospy.get_name() +
                                                          '/enu_transform_spp',
                                                          TransformStamped,
                                                          queue_size=10)
        publishers['gps_time_multi'] = rospy.Publisher(rospy.get_name() +
                                                       '/gps_time',
                                                       GpsTimeMulti,
                                                       queue_size=10)
        publishers['baseline_ned_multi'] = rospy.Publisher(rospy.get_name() +
                                                           '/baseline_ned',
                                                           BaselineNed,
                                                           queue_size=10)
        publishers['utc_time_multi'] = rospy.Publisher(rospy.get_name() +
                                                       '/utc_time',
                                                       UtcTimeMulti,
                                                       queue_size=10)
        publishers['imu_raw_multi'] = rospy.Publisher(rospy.get_name() +
                                                      '/imu_raw',
                                                      ImuRawMulti,
                                                      queue_size=10)
        publishers['imu_aux_multi'] = rospy.Publisher(rospy.get_name() +
                                                      '/debug/imu_aux',
                                                      ImuAuxMulti,
                                                      queue_size=10)
        # Topics published only if in "debug mode".
        if self.debug_mode:
            publishers['rtk_float'] = rospy.Publisher(rospy.get_name() +
                                                      '/navsatfix_rtk_float',
                                                      NavSatFix,
                                                      queue_size=10)
            publishers['vel_ecef'] = rospy.Publisher(rospy.get_name() +
                                                     '/vel_ecef',
                                                     VelEcef,
                                                     queue_size=10)
            publishers['enu_pose_float'] = rospy.Publisher(
                rospy.get_name() + '/enu_pose_float',
                PoseWithCovarianceStamped,
                queue_size=10)
            publishers['enu_point_float'] = rospy.Publisher(rospy.get_name() +
                                                            '/enu_point_float',
                                                            PointStamped,
                                                            queue_size=10)
            publishers['enu_transform_float'] = rospy.Publisher(
                rospy.get_name() + '/enu_transform_float',
                TransformStamped,
                queue_size=10)
            publishers['baseline_ecef_multi'] = rospy.Publisher(
                rospy.get_name() + '/baseline_ecef',
                BaselineEcef,
                queue_size=10)
            publishers['dops_multi'] = rospy.Publisher(rospy.get_name() +
                                                       '/dops',
                                                       DopsMulti,
                                                       queue_size=10)
            publishers['pos_ecef_multi'] = rospy.Publisher(rospy.get_name() +
                                                           '/pos_ecef',
                                                           PosEcef,
                                                           queue_size=10)

        if not self.base_station_mode:
            publishers['wifi_corrections'] = rospy.Publisher(
                rospy.get_name() + '/debug/wifi_corrections',
                InfoWifiCorrections,
                queue_size=10)

        return publishers

    def ping_base_station_over_wifi(self):
        """
        Ping base station periodically without blocking the driver.
        """
        ping_deadline_seconds = 3
        interval_between_pings_seconds = 5

        while not rospy.is_shutdown():
            # Send ping command.
            command = [
                "ping",
                "-w",
                str(ping_deadline_seconds),  # deadline before stopping attempt
                "-c",
                "1",  # number of pings to send
                self.base_station_ip_for_latency_estimation
            ]
            ping = subprocess.Popen(command, stdout=subprocess.PIPE)

            out, error = ping.communicate()
            # Search for 'min/avg/max/mdev' round trip delay time (rtt) numbers.
            matcher = re.compile("(\d+.\d+)/(\d+.\d+)/(\d+.\d+)/(\d+.\d+)")

            if matcher.search(out) == None:
                # No ping response within ping_deadline_seconds.
                # In python write and read operations on built-in type are atomic,
                # there's no need to use mutex.
                self.num_wifi_corrections.latency = -1
            else:
                groups_rtt = matcher.search(out).groups()
                avg_rtt = groups_rtt[1]
                # In python write and read operations on built-in type are atomic,
                # there's no need to use mutex.
                self.num_wifi_corrections.latency = float(avg_rtt)

            time.sleep(interval_between_pings_seconds)

    def make_callback(self, sbp_type, ros_message, pub, attrs):
        """
        Dynamic generator for callback functions for message types from
        the SBP library.
        Inputs: 'sbp_type' name of SBP message type.
                'ros_message' ROS message type with SBP format.
                'pub' ROS publisher for ros_message.
                'attrs' array of attributes in SBP/ROS message.
        Returns: callback function 'callback'.
        """
        def callback(msg, **metadata):
            sbp_message = sbp_type(msg)
            ros_message.header.stamp = rospy.Time.now()
            for attr in attrs:
                if attr == 'flags':
                    # Least significat three bits of flags indicate status.
                    if (msg.flags & 0x07) == 0:
                        return  # Invalid message, do not publish it.

                setattr(ros_message, attr, getattr(sbp_message, attr))
            pub.publish(ros_message)

        return callback

    def init_callback(self, topic_name, ros_datatype, sbp_msg_type,
                      callback_data_type, *attrs):
        """
        Initializes the callback function  for an SBP
        message type.
        Inputs: 'topic_name' name of ROS topic for publisher
                'ros_datatype' ROS custom message type
                'sbp_msg_type' name of SBP message type for callback function
                'callback_data_type' name of SBP message type for SBP library
                '*attrs' array of attributes in ROS/SBP message
        """
        # Check that required topic has been advertised.
        if topic_name in self.publishers:
            ros_message = ros_datatype()

            # Add callback function.
            pub = self.publishers[topic_name]
            callback_function = self.make_callback(callback_data_type,
                                                   ros_message, pub, attrs)
            self.handler.add_callback(callback_function, msg_type=sbp_msg_type)

    def callback_sbp_obs(self, msg, **metadata):
        # rospy.logwarn("CALLBACK SBP OBS")
        self.multicaster.sendSbpPacket(msg)

    def callback_sbp_obs_dep_a(self, msg, **metadata):
        # rospy.logwarn("CALLBACK SBP OBS DEP A")
        self.multicaster.sendSbpPacket(msg)

    def callback_sbp_obs_dep_b(self, msg, **metadata):
        # rospy.logwarn("CALLBACK SBP OBS DEP B")
        self.multicaster.sendSbpPacket(msg)

    def callback_sbp_base_pos_llh(self, msg, **metadata):
        # rospy.logwarn("CALLBACK SBP OBS BASE LLH")
        self.multicaster.sendSbpPacket(msg)

    def callback_sbp_base_pos_ecef(self, msg, **metadata):
        # rospy.logwarn("CALLBACK SBP OBS BASE LLH")
        self.multicaster.sendSbpPacket(msg)

    def multicast_callback(self, msg, **metadata):
        # rospy.logwarn("MULTICAST Callback")
        if self.framer:
            self.framer(msg, **metadata)

            # Publish debug message about wifi corrections, if enabled.
            self.num_wifi_corrections.header.seq += 1
            self.num_wifi_corrections.header.stamp = rospy.Time.now()
            self.num_wifi_corrections.received_corrections += 1
            if not self.base_station_mode:
                self.publishers['wifi_corrections'].publish(
                    self.num_wifi_corrections)

        else:
            rospy.logwarn(
                "Received external SBP msg, but Piksi not connected.")

    def watchdog_callback(self, event):
        if ((rospy.get_rostime() - self.watchdog_time).to_sec() > 10.0):
            rospy.logwarn("Heartbeat failed, watchdog triggered.")

            if self.base_station_mode:
                rospy.signal_shutdown(
                    "Watchdog triggered, was gps disconnected?")

    def pos_llh_callback(self, msg_raw, **metadata):
        msg = MsgPosLLH(msg_raw)

        # Invalid messages.
        if msg.flags == PosLlhMulti.FIX_MODE_INVALID:
            return
        # SPP GPS messages.
        elif msg.flags == PosLlhMulti.FIX_MODE_SPP:
            self.publish_spp(msg.lat, msg.lon, msg.height)

        #TODO: Differential GNSS (DGNSS)
        #elif msg.flags == PosLlhMulti.FIX_MODE_DGNSS

        # RTK GPS messages.
        elif msg.flags == PosLlhMulti.FIX_MODE_FLOAT_RTK and self.debug_mode:
            self.publish_rtk_float(msg.lat, msg.lon, msg.height)
        elif msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK:
            # Use first RTK fix to set origin ENU frame, if it was not set by rosparam.
            if not self.origin_enu_set:
                self.init_geodetic_reference(msg.lat, msg.lon, msg.height)

            self.publish_rtk_fix(msg.lat, msg.lon, msg.height)
        # Update debug msg and publish.
        self.receiver_state_msg.rtk_mode_fix = True if (
            msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK) else False
        self.publish_receiver_state_msg()

    def publish_spp(self, latitude, longitude, height):
        self.publish_gps_point(latitude, longitude, height, self.var_spp,
                               NavSatStatus.STATUS_FIX, self.publishers['spp'],
                               self.publishers['enu_pose_spp'],
                               self.publishers['enu_point_spp'],
                               self.publishers['enu_transform_spp'])

    def publish_rtk_float(self, latitude, longitude, height):
        self.publish_gps_point(latitude, longitude, height, self.var_rtk_float,
                               NavSatStatus.STATUS_GBAS_FIX,
                               self.publishers['rtk_float'],
                               self.publishers['enu_pose_float'],
                               self.publishers['enu_point_float'],
                               self.publishers['enu_transform_float'])

    def publish_rtk_fix(self, latitude, longitude, height):
        self.publish_gps_point(latitude, longitude, height, self.var_rtk_fix,
                               NavSatStatus.STATUS_GBAS_FIX,
                               self.publishers['rtk_fix'],
                               self.publishers['enu_pose_fix'],
                               self.publishers['enu_point_fix'],
                               self.publishers['enu_transform_fix'])

    def publish_gps_point(self, latitude, longitude, height, variance, status,
                          pub_navsatfix, pub_pose, pub_point, pub_transform):
        # Navsatfix message.
        navsatfix_msg = NavSatFix()
        navsatfix_msg.header.stamp = rospy.Time.now()
        navsatfix_msg.header.frame_id = self.navsatfix_frame_id
        navsatfix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
        navsatfix_msg.status.service = NavSatStatus.SERVICE_GPS
        navsatfix_msg.latitude = latitude
        navsatfix_msg.longitude = longitude
        navsatfix_msg.altitude = height
        navsatfix_msg.status.status = status
        navsatfix_msg.position_covariance = [
            variance[0], 0, 0, 0, variance[1], 0, 0, 0, variance[2]
        ]
        # Local Enu coordinate.
        (east, north, up) = self.geodetic_to_enu(latitude, longitude, height)

        # Pose message.
        pose_msg = PoseWithCovarianceStamped()
        pose_msg.header.stamp = navsatfix_msg.header.stamp
        pose_msg.header.frame_id = self.enu_frame_id
        pose_msg.pose = self.enu_to_pose_msg(east, north, up, variance)

        # Point message.
        point_msg = PointStamped()
        point_msg.header.stamp = navsatfix_msg.header.stamp
        point_msg.header.frame_id = self.enu_frame_id
        point_msg.point = self.enu_to_point_msg(east, north, up)

        # Transform message.
        transform_msg = TransformStamped()
        transform_msg.header.stamp = navsatfix_msg.header.stamp
        transform_msg.header.frame_id = self.enu_frame_id
        transform_msg.child_frame_id = self.transform_child_frame_id
        transform_msg.transform = self.enu_to_transform_msg(east, north, up)

        # Publish.
        pub_navsatfix.publish(navsatfix_msg)
        pub_pose.publish(pose_msg)
        pub_point.publish(point_msg)
        pub_transform.publish(transform_msg)

    def heartbeat_callback(self, msg_raw, **metadata):
        msg = MsgHeartbeat(msg_raw)

        # Let watchdag know messages are still arriving
        self.watchdog_time = rospy.get_rostime()

        # Start watchdog with 10 second timeout to ensure we keep getting gps
        if (not self.messages_started):
            self.messages_started = True
            rospy.Timer(rospy.Duration(10), self.watchdog_callback)

        heartbeat_msg = Heartbeat()
        heartbeat_msg.header.stamp = rospy.Time.now()
        heartbeat_msg.system_error = msg.flags & 0x01
        heartbeat_msg.io_error = msg.flags & 0x02
        heartbeat_msg.swift_nap_error = msg.flags & 0x04
        heartbeat_msg.sbp_minor_version = (msg.flags & 0xFF00) >> 8
        heartbeat_msg.sbp_major_version = (msg.flags & 0xFF0000) >> 16
        heartbeat_msg.external_antenna_present = (msg.flags & 0x80000000) >> 31

        self.publishers['heartbeat'].publish(heartbeat_msg)

        # Update debug msg and publish.
        self.receiver_state_msg.system_error = heartbeat_msg.system_error
        self.receiver_state_msg.io_error = heartbeat_msg.io_error
        self.receiver_state_msg.swift_nap_error = heartbeat_msg.swift_nap_error
        self.receiver_state_msg.external_antenna_present = heartbeat_msg.external_antenna_present
        self.publish_receiver_state_msg()

    def tracking_state_callback(self, msg_raw, **metadata):
        msg = MsgTrackingState(msg_raw)

        tracking_state_msg = TrackingState()
        tracking_state_msg.header.stamp = rospy.Time.now()
        tracking_state_msg.state = []
        tracking_state_msg.sat = []
        tracking_state_msg.code = []
        tracking_state_msg.cn0 = []

        for single_tracking_state in msg.states:
            # Take only running tracking.
            track_running = single_tracking_state.state & 0x01
            if track_running:
                tracking_state_msg.state.append(single_tracking_state.state)
                tracking_state_msg.sat.append(single_tracking_state.sid.sat)
                tracking_state_msg.code.append(single_tracking_state.sid.code)
                tracking_state_msg.cn0.append(single_tracking_state.cn0)

        # Publish if there's at least one element in each array.
        if len(tracking_state_msg.state) \
                and len(tracking_state_msg.sat) \
                and len(tracking_state_msg.code) \
                and len(tracking_state_msg.cn0):

            self.publishers['tracking_state'].publish(tracking_state_msg)

            # Update debug msg and publish.
            self.receiver_state_msg.num_sat = 0  # Count number of satellites used to track.
            for tracking_running in tracking_state_msg.state:
                self.receiver_state_msg.num_sat += tracking_running

            self.receiver_state_msg.sat = tracking_state_msg.sat
            self.receiver_state_msg.cn0 = tracking_state_msg.cn0
            self.receiver_state_msg.tracking_running = tracking_state_msg.state
            self.publish_receiver_state_msg()

#     def utc_time_callback(self, msg_raw, **metadata):
#         msg = MsgUtcTime(msg_raw)
#
#         # check i message is valid
#         if msg.flags & 0x01 == True: # msg valid TODO: use bitmask instead
#             # TODO: calc delta_t to rospy.Time.now()
#             # delta_t_vec.append(delta_t)
#             # self.delta_t_MA = moving_average_filter(delta_t_vec, N)
#             return
#         else: # msg invalid
#             return

    def publish_receiver_state_msg(self):
        self.receiver_state_msg.header.stamp = rospy.Time.now()
        self.publishers['receiver_state'].publish(self.receiver_state_msg)

    def uart_state_callback(self, msg_raw, **metadata):
        msg = MsgUartState(msg_raw)

        uart_state_msg = UartState()
        uart_state_msg.header.stamp = rospy.Time.now()

        uart_state_msg.uart_a_tx_throughput = msg.uart_a.tx_throughput
        uart_state_msg.uart_a_rx_throughput = msg.uart_a.rx_throughput
        uart_state_msg.uart_a_crc_error_count = msg.uart_a.crc_error_count
        uart_state_msg.uart_a_io_error_count = msg.uart_a.io_error_count
        uart_state_msg.uart_a_tx_buffer_level = msg.uart_a.tx_buffer_level
        uart_state_msg.uart_a_rx_buffer_level = msg.uart_a.rx_buffer_level

        uart_state_msg.uart_b_tx_throughput = msg.uart_b.tx_throughput
        uart_state_msg.uart_b_rx_throughput = msg.uart_b.rx_throughput
        uart_state_msg.uart_b_crc_error_count = msg.uart_b.crc_error_count
        uart_state_msg.uart_b_io_error_count = msg.uart_b.io_error_count
        uart_state_msg.uart_b_tx_buffer_level = msg.uart_b.tx_buffer_level
        uart_state_msg.uart_b_rx_buffer_level = msg.uart_b.rx_buffer_level

        uart_state_msg.uart_ftdi_tx_throughput = msg.uart_ftdi.tx_throughput
        uart_state_msg.uart_ftdi_rx_throughput = msg.uart_ftdi.rx_throughput
        uart_state_msg.uart_ftdi_crc_error_count = msg.uart_ftdi.crc_error_count
        uart_state_msg.uart_ftdi_io_error_count = msg.uart_ftdi.io_error_count
        uart_state_msg.uart_ftdi_tx_buffer_level = msg.uart_ftdi.tx_buffer_level
        uart_state_msg.uart_ftdi_rx_buffer_level = msg.uart_ftdi.rx_buffer_level

        uart_state_msg.latency_avg = msg.latency.avg
        uart_state_msg.latency_lmin = msg.latency.lmin
        uart_state_msg.latency_lmax = msg.latency.lmax
        uart_state_msg.latency_current = msg.latency.current

        uart_state_msg.obs_period_avg = msg.obs_period.avg
        uart_state_msg.obs_period_pmin = msg.obs_period.pmin
        uart_state_msg.obs_period_pmax = msg.obs_period.pmax
        uart_state_msg.obs_period_current = msg.obs_period.current

        self.publishers['uart_state_multi'].publish(uart_state_msg)

    def init_geodetic_reference(self, latitude, longitude, altitude):
        if self.origin_enu_set:
            return

        self.latitude0 = math.radians(latitude)
        self.longitude0 = math.radians(longitude)
        self.altitude0 = altitude

        (self.initial_ecef_x, self.initial_ecef_y,
         self.initial_ecef_z) = self.geodetic_to_ecef(latitude, longitude,
                                                      altitude)
        # Compute ECEF to NED.
        phiP = math.atan2(
            self.initial_ecef_z,
            math.sqrt(
                math.pow(self.initial_ecef_x, 2) +
                math.pow(self.initial_ecef_y, 2)))
        self.ecef_to_ned_matrix = self.n_re(phiP, self.longitude0)

        self.origin_enu_set = True

        rospy.loginfo("Origin ENU frame set to: %.6f, %.6f, %.2f" %
                      (latitude, longitude, altitude))

    def geodetic_to_ecef(self, latitude, longitude, altitude):
        # Convert geodetic coordinates to ECEF.
        # http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
        lat_rad = math.radians(latitude)
        lon_rad = math.radians(longitude)
        xi = math.sqrt(1 - PiksiMulti.kFirstEccentricitySquared *
                       math.sin(lat_rad) * math.sin(lat_rad))
        x = (PiksiMulti.kSemimajorAxis / xi +
             altitude) * math.cos(lat_rad) * math.cos(lon_rad)
        y = (PiksiMulti.kSemimajorAxis / xi +
             altitude) * math.cos(lat_rad) * math.sin(lon_rad)
        z = (PiksiMulti.kSemimajorAxis / xi *
             (1 - PiksiMulti.kFirstEccentricitySquared) +
             altitude) * math.sin(lat_rad)

        return x, y, z

    def ecef_to_ned(self, x, y, z):
        # Converts ECEF coordinate position into local-tangent-plane NED.
        # Coordinates relative to given ECEF coordinate frame.
        vect = np.array([0.0, 0.0, 0.0])
        vect[0] = x - self.initial_ecef_x
        vect[1] = y - self.initial_ecef_y
        vect[2] = z - self.initial_ecef_z
        ret = self.ecef_to_ned_matrix.dot(vect)
        n = ret[0]
        e = ret[1]
        d = -ret[2]

        return n, e, d

    def geodetic_to_enu(self, latitude, longitude, altitude):
        # Geodetic position to local ENU frame
        (x, y, z) = self.geodetic_to_ecef(latitude, longitude, altitude)
        (north, east, down) = self.ecef_to_ned(x, y, z)

        # Return East, North, Up coordinate.
        return east, north, -down

    def n_re(self, lat_radians, lon_radians):
        s_lat = math.sin(lat_radians)
        s_lon = math.sin(lon_radians)
        c_lat = math.cos(lat_radians)
        c_lon = math.cos(lon_radians)

        ret = np.eye(3)
        ret[0, 0] = -s_lat * c_lon
        ret[0, 1] = -s_lat * s_lon
        ret[0, 2] = c_lat
        ret[1, 0] = -s_lon
        ret[1, 1] = c_lon
        ret[1, 2] = 0.0
        ret[2, 0] = c_lat * c_lon
        ret[2, 1] = c_lat * s_lon
        ret[2, 2] = s_lat

        return ret

    def enu_to_pose_msg(self, east, north, up, variance):
        pose_msg = PoseWithCovariance()

        # Fill covariance using variance parameter of GPS.
        pose_msg.covariance[6 * 0 + 0] = variance[0]
        pose_msg.covariance[6 * 1 + 1] = variance[1]
        pose_msg.covariance[6 * 2 + 2] = variance[2]

        # Fill pose section.
        pose_msg.pose.position.x = east
        pose_msg.pose.position.y = north
        pose_msg.pose.position.z = up

        # GPS points do not have orientation
        pose_msg.pose.orientation.x = 0.0
        pose_msg.pose.orientation.y = 0.0
        pose_msg.pose.orientation.z = 0.0
        pose_msg.pose.orientation.w = 1.0

        return pose_msg

    def enu_to_point_msg(self, east, north, up):
        point_msg = Point()

        # Fill pose section.
        point_msg.x = east
        point_msg.y = north
        point_msg.z = up

        return point_msg

    def enu_to_transform_msg(self, east, north, up):
        transform_msg = Transform()

        # Fill message.
        transform_msg.translation.x = east
        transform_msg.translation.y = north
        transform_msg.translation.z = up

        # Set orientation to unit quaternion as it does not really metter.
        transform_msg.rotation.x = 0.0
        transform_msg.rotation.y = 0.0
        transform_msg.rotation.z = 0.0
        transform_msg.rotation.w = 1.0

        return transform_msg

    def reset_piksi_service_callback(self, request):
        response = std_srvs.srv.SetBoolResponse()

        if request.data:
            # Send reset message.
            reset_sbp = SBP(SBP_MSG_RESET)
            reset_sbp.payload = ''
            reset_msg = reset_sbp.pack()
            self.driver.write(reset_msg)

            rospy.logwarn("Piksi hard reset via rosservice call.")

            # Init messages with "memory".
            self.receiver_state_msg = self.init_receiver_state_msg()
            self.num_wifi_corrections = self.init_num_corrections_msg()

            response.success = True
            response.message = "Piksi reset command sent."
        else:
            response.success = False
            response.message = "Piksi reset command not sent."

        return response
Ejemplo n.º 30
0
    def __init__(self):

        # Print info.
        rospy.sleep(
            0.5)  # Wait for a while for init to complete before printing.
        rospy.loginfo(rospy.get_name() + " start")
        rospy.loginfo("libsbp version currently used: " +
                      sbp.version.get_git_version())

        # Check for correct SBP library version dependent on Piksi device.
        if PiksiMulti.LIB_SBP_VERSION_MULTI != sbp.version.get_git_version():
            rospy.logwarn(
                "Lib SBP version in usage (%s) is different than the one used to test this driver (%s)!"
                % (sbp.version.get_git_version(),
                   PiksiMulti.LIB_SBP_VERSION_MULTI))

        # Open a connection to Piksi.
        serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
        baud_rate = rospy.get_param('~baud_rate', 115200)

        try:
            self.driver = PySerialDriver(serial_port, baud=baud_rate)
        except SystemExit:
            rospy.logerr("Piksi not found on serial port '%s'", serial_port)
            raise

        # Create a handler to connect Piksi driver to callbacks.
        self.framer = Framer(self.driver.read, self.driver.write, verbose=True)
        self.handler = Handler(self.framer)

        self.debug_mode = rospy.get_param('~debug_mode', False)
        if self.debug_mode:
            rospy.loginfo(
                "Piksi driver started in debug mode, every available topic will be published."
            )
        else:
            rospy.loginfo("Piksi driver started in normal mode.")

        # Corrections over WiFi settings.
        self.base_station_mode = rospy.get_param('~base_station_mode', False)
        self.udp_broadcast_addr = rospy.get_param('~broadcast_addr',
                                                  '255.255.255.255')
        self.udp_port = rospy.get_param('~broadcast_port', 26078)
        self.base_station_ip_for_latency_estimation = rospy.get_param(
            '~base_station_ip_for_latency_estimation', '10.10.10.1')
        self.multicaster = []
        self.multicast_recv = []

        # Navsatfix settings.
        self.var_spp = rospy.get_param('~var_spp', [25.0, 25.0, 64.0])
        self.var_rtk_float = rospy.get_param('~var_rtk_float',
                                             [25.0, 25.0, 64.0])
        self.var_rtk_fix = rospy.get_param('~var_rtk_fix',
                                           [0.0049, 0.0049, 0.01])
        self.navsatfix_frame_id = rospy.get_param('~navsatfix_frame_id', 'gps')

        # Local ENU frame settings.
        self.origin_enu_set = False
        self.latitude0 = 0.0
        self.longitude0 = 0.0
        self.altitude0 = 0.0
        self.initial_ecef_x = 0.0
        self.initial_ecef_y = 0.0
        self.initial_ecef_z = 0.0
        self.ecef_to_ned_matrix = np.eye(3)
        self.enu_frame_id = rospy.get_param('~enu_frame_id', 'enu')
        self.transform_child_frame_id = rospy.get_param(
            '~transform_child_frame_id', 'gps_receiver')

        if rospy.has_param('~latitude0_deg') and rospy.has_param(
                '~longitude0_deg') and rospy.has_param('~altitude0'):
            latitude0 = rospy.get_param('~latitude0_deg')
            longitude0 = rospy.get_param('~longitude0_deg')
            altitude0 = rospy.get_param('~altitude0')

            # Set origin ENU frame to coordinate specified by rosparam.
            self.init_geodetic_reference(latitude0, longitude0, altitude0)
            rospy.loginfo("Origin ENU frame set by rosparam.")

        # Advertise topics.
        self.publishers = self.advertise_topics()

        # Create callbacks.
        self.create_callbacks()

        # Init messages with "memory".
        self.receiver_state_msg = self.init_receiver_state_msg()
        self.num_wifi_corrections = self.init_num_corrections_msg()

        # Corrections over wifi message, if we are not the base station.
        if not self.base_station_mode:
            # Start new thread to periodically ping base station.
            threading.Thread(target=self.ping_base_station_over_wifi).start()

        self.handler.start()

        # Reset service.
        self.reset_piksi_service = rospy.Service(
            rospy.get_name() + '/reset_piksi', std_srvs.srv.SetBool,
            self.reset_piksi_service_callback)

        # Watchdog timer info
        self.watchdog_time = rospy.get_rostime()
        self.messages_started = False

        # Only have start-up reset in base station mode
        if self.base_station_mode:
            # Things have 30 seconds to start or we will kill node
            rospy.Timer(rospy.Duration(30), self.watchdog_callback, True)

        # Spin.
        rospy.spin()