def __init__(self, **kwargs):
super(ROS_GPS, self).__init__()
self.gps = GPS_Filter( Ts = 0.005 )
self.publisher = dict(
state = rospy.Publisher('/ardrone/gps', Odometry)
)
self.subscriber = dict(
raw_gps = rospy.Subscriber('/fix', NavSatFix, callback = self.ReceiveGPS ),
navdata = rospy.Subscriber('/ardrone/navdata', Navdata, callback = self.ReceiveNavdata)
)
class ROS_GPS(ROS_Object, object):
"""docstring for ROS_IMU"""
def __init__(self, **kwargs):
super(ROS_GPS, self).__init__()
self.gps = GPS_Filter( Ts = 0.005 )
self.publisher = dict(
state = rospy.Publisher('/ardrone/gps', Odometry)
)
self.subscriber = dict(
raw_gps = rospy.Subscriber('/fix', NavSatFix, callback = self.ReceiveGPS ),
navdata = rospy.Subscriber('/ardrone/navdata', Navdata, callback = self.ReceiveNavdata)
)
def ReceiveNavdata(self, navdata):
self.gps.measure_navdata(navdata)
#dummy input
self.gps.position.set_properties( yaw = navdata.rotZ * pi/180 )
self.gps.predict()
self.Talk()
def ReceiveGPS(self, gps_raw):
self.gps.measure_gps(gps_raw)
self.gps.correct()
def Talk( self ):
msg = Odometry()
msg.header.stamp = rospy.Time.now()
msg.pose.pose.position.x = self.gps.position.x
msg.pose.pose.position.y = self.gps.position.y
msg.pose.pose.position.z = self.gps.position.z
self.publisher['state'].publish(msg)
