nav_publisher

#!/usr/bin/env python

import rospy
from tf import TransformListener
from time import sleep

from heron_nmea.nmea_helpers import TxHelper
from geometry_msgs.msg import TwistStamped, Vector3Stamped
from nav_msgs.msg import Odometry
from math import degrees, floor, fabs, atan2, hypot, pi
from heron_nmea.rpy_helpers import sea_rpy_from_quaternion
from sensor_msgs.msg import Imu, NavSatFix


def nmea_deg(decimal_degrees):
  dd = fabs(decimal_degrees)
  return (floor(dd) * 100) + (dd % 1.0 * 60.0)

def nmea_NS(decimal_degrees):
  return 'N' if decimal_degrees > 0 else 'S'

def nmea_EW(decimal_degrees):
  return 'E' if decimal_degrees > 0 else 'W'


class Nav(TxHelper):
  SENTENCE = "NVG"

  def __init__(self):
    rospy.loginfo("Waiting for ENU to_fix service.")
    rospy.loginfo("Service found, setting up NVG publisher.")
    self.sub_fix = rospy.Subscriber("navsat/fix", NavSatFix, self._fix_cb)
    self.sub_odom = rospy.Subscriber("odom", Odometry, self._cb)
    self.sub_vel = rospy.Subscriber("navsat/vel", TwistStamped, self._vel)
    self.vel = None
    self.fix = None

  def _vel(self, msg):
    self.vel = msg

  def _fix_cb(self, msg):
    self.fix = msg

  def _cb(self, msg):
    if self.fix is None:
        return

    roll, pitch, heading = sea_rpy_from_quaternion(msg.pose.pose.orientation)

    # Override heading with value from GPS, when we have sufficient speed.
    if self.vel and hypot(self.vel.twist.linear.y, self.vel.twist.linear.x) > 0.2:
      heading = (pi/2.0) - atan2(self.vel.twist.linear.y, self.vel.twist.linear.x)
      if heading < 0: heading += 2*pi

    self.tx(self.gps_time(),
        nmea_deg(self.fix.latitude), nmea_NS(self.fix.latitude),
        nmea_deg(self.fix.longitude), nmea_EW(self.fix.longitude),
        1, # Pos quality. 1 = GPS, for now we don't publish unless there are fixes.
        0, # Altitude from bottom
        0, # Depth from surface
        degrees(heading), # Heading
        degrees(roll), # Roll
        degrees(pitch), # Pitch
        self.gps_time(msg.header.stamp))


class VelRate(TxHelper):
  SENTENCE = "NVR"

  def __init__(self, tf_listener):
    self.sub_imu = rospy.Subscriber("imu/data", Imu, self._cb)
    self.sub_vel = rospy.Subscriber("navsat/vel", TwistStamped, self._vel)
    self.vel = None
    self.tf_listener = tf_listener

  def _vel(self, msg):
    self.vel = msg

  def _cb(self, msg):
    if self.vel:
      base_av = self.tf_listener.transformVector3(namespace + "base_link",
              Vector3Stamped(msg.header, msg.angular_velocity))
      self.tx(self.gps_time(),
          self.vel.twist.linear.x,
          self.vel.twist.linear.y,
          0,  # Vertical component of transit velocity
          -degrees(base_av.vector.y),  # Pitch rate
          degrees(base_av.vector.x), # Roll rate
          -degrees(base_av.vector.z)) # Yaw rate


namespace = ""
if __name__ == "__main__":
  rospy.init_node('nmea_nav')

  namespace = rospy.get_param("~namespace", "") + "/"

  tf_listener = TransformListener()
  tf_listener.waitForTransform(namespace + "base_link", namespace + "imu_link", rospy.Time(), rospy.Duration(10.0))
  sleep(1.0)
  Nav()
  VelRate(tf_listener)
  rospy.spin()