def test_WrapToPi(self):
""" Testcase for WrapToPi against MATLAB's function """
filename = "./test_data/WrapToPiTest.mat"
data = sio.loadmat(filename, struct_as_record=True)
data = np.hstack((data['input'], data['output'].T))
for (inp, out) in data:
assert_almost_equal(ekf.wrapToPi(inp), out, decimal=12)
def test_WrapToPi(self):
""" Testcase for WrapToPi against MATLAB's function """
filename = "./test_data/WrapToPiTest.mat"
data = sio.loadmat(filename,struct_as_record=True)
data = np.hstack((data['input'],data['output'].T))
for (inp,out) in data:
assert_almost_equal(ekf.wrapToPi(inp),out,decimal=12)
def __init__(self):
# Load Parameters
self.odom_used = rospy.get_param("~odom_used",True)
self.imu_used = rospy.get_param("~imu_used",True)
self.gps_used = rospy.get_param("~gps_used",True)
self.cutters_used = rospy.get_param("~cutters_used",False)
self.decimate_ahrs = rospy.get_param("~decimate_ahrs_by_factor",0)
self.publish_rate = 1.0/rospy.get_param("~output_publish_rate", 25)
self.time_delay = rospy.get_param("~time_delay",0.0)
self.output_frame = rospy.get_param("~output_frame","odom_combined")
self.output_states = rospy.get_param("~output_states",False)
self.output_states_dir = rospy.get_param("~output_states_dir","~/.ros/")
self.adaptive_encoders = rospy.get_param("~adaptive_encoders",False)
self.adaptive_cutters = rospy.get_param("~adaptive_cutters",False)
self.publish_states = rospy.get_param("~publish_states",False)
if self.output_states:
import time
self.output_states_file = self.output_states_dir + "ekf_states-" + \
time.strftime('%F-%H-%M') + ".csv"
self.output_file = open(self.output_states_file,'w')
self.location_initilized = False
self.heading_initilized = False
self.ahrs_count = 0
# Class Variables
self.ekf = AutomowEKF.fromDefault()
self.v = 0
self.w = 0
self.filter_time = rospy.Time.now()
self.cutter_l = 0
self.cutter_r = 0
# Subscribers
if self.odom_used:
rospy.loginfo("Adding Encoders to the EKF")
self.enc_sub = rospy.Subscriber("encoders",
StampedEncoders,
self.encoders_cb)
else:
rospy.loginfo("You aren't using the wheel encoders, expect bad results")
rospy.loginfo("Adding IMU to the EKF")
self.imu_sub = rospy.Subscriber("imu/data",
Imu,
self.imu_cb)
rospy.loginfo("Adding GPS to the EKF")
self.gps_sub = rospy.Subscriber("gps/odometry",
Odometry,
self.gps_cb)
if self.cutters_used or self.adaptive_cutters:
rospy.loginfo("Subscribing to the cutter status")
self.cut_sub = rospy.Subscriber("/CutterControl",
CutterControl,
self.cutter_cb)
if self.adaptive_cutters:
rospy.loginfo("P matrix will change on cutter status")
# Publishers
self.odom_pub = rospy.Publisher("ekf/odom",Odometry)
self.odometry_timer = threading.Timer(self.publish_rate, self.odometry_cb)
self.odometry_timer.start()
if self.publish_states:
self.states_pub = rospy.Publisher("ekf/states",States)
def __init__(self):
# Load Parameters
self.odom_used = rospy.get_param("~odom_used", True)
self.imu_used = rospy.get_param("~imu_used", True)
self.gps_used = rospy.get_param("~gps_used", True)
self.decimate_ahrs = rospy.get_param("~decimate_ahrs_by_factor", 0)
self.publish_rate = 1.0 / rospy.get_param("~output_publish_rate", 25)
self.time_delay = rospy.get_param("~time_delay", 0.0)
self.output_tf = rospy.get_param("~output_tf", True)
self.output_frame = rospy.get_param("~output_frame", "odom_combined")
self.adaptive_encoders = rospy.get_param("~adaptive_encoders", False)
self.publish_states = rospy.get_param("~publish_states", False)
self.encoder_resolution = 1000
self.location_initilized = False
self.heading_initilized = False
self.ahrs_count = 0
# Class Variables
self.ekf = AutomowEKF.fromDefault()
self.v = 0
self.w = 0
self.filter_time = rospy.Time.now()
self.cutter_l = 0
self.cutter_r = 0
# Subscribers
if self.odom_used:
rospy.loginfo("Adding Encoders to the EKF")
self.enc_sub = rospy.Subscriber("/encoders",
StampedEncoders,
self.encoders_cb)
self.enc_prev_time = rospy.Time.now()
if self.imu_used:
rospy.loginfo("Adding IMU to the EKF")
self.imu_sub = rospy.Subscriber("/imu/data",
Imu,
self.imu_cb)
else:
rospy.loginfo("IMU will not be used.")
self.ekf.measurementUpdateAHRS(0)
self.filter_time = rospy.Time.now()
self.heading_initilized = True
if self.gps_used:
rospy.loginfo("Adding GPS to the EKF")
self.gps_sub = rospy.Subscriber("/magellan_dg14/odometry",
Odometry,
self.gps_cb)
self.gps_fix_sub = rospy.Subscriber("/magellan_dg14/utm_fix",
UTMFix,
self.gps_fix_cb)
self.gps_bad = False
else:
rospy.loginfo("GPS will not be used")
y = np.array([0, 0], dtype=np.double)
covar = np.diag(np.array([0.001, 0.001]))
self.ekf.measurementUpdateGPS(y, covar)
self.filter_time = rospy.Time.now()
self.location_initilized = True
# Publishers
if self.publish_states:
self.states_pub = rospy.Publisher("ekf/states", States)
self.odom_pub = rospy.Publisher("ekf/odom", Odometry)
self.odometry_timer = threading.Timer(self.publish_rate,
self.odometry_cb)
self.odometry_timer.start()
