class FilterVel(object): def __init__(self): self.filterL = KalmanFilter(1, 30) self.filterR = KalmanFilter(1, 30) self.filter_vw_r = 0.0 self.filter_vw_l = 0.0 def updateVW_L(self, v_l): self.filterL.input_latest_noisy_measurement(v_l) self.filter_vw_l = self.filterL.get_latest_estimated_measurement() def updateVW_R(self, v_r): self.filterR.input_latest_noisy_measurement(v_r) self.filter_vw_r = self.filterR.get_latest_estimated_measurement()
class WheelVelFilter:
def __init__(self):
rospy.init_node('wheelVel_filter_node', anonymous=True)
self.odomFreq = float(rospy.get_param("~odom_freq", "20"))
self.sub_vw_r = rospy.Subscriber("v_wheel_right", Float32,
self.updateVW_R)
self.sub_vw_l = rospy.Subscriber("v_wheel_left", Float32,
self.updateVW_L)
# self.sub_imu = rospy.Subscriber("/imu", Float32, self.updateIMU)
# self.pub_raw_vel = rospy.Publisher("/raw_vel", Float32MultiArray, queue_size=1)
self.pub_filter_vel = rospy.Publisher("filter_vel",
Float32MultiArray,
queue_size=1)
# self.pub_filter_yaw = rospy.Publisher("/filter_yaw", Float32, queue_size=1)
self.timer_v_wheel = rospy.Timer(rospy.Duration(1.0 / self.odomFreq),
self.timerVWheelCB)
self.filterR = KalmanFilter(1, 30)
self.filterL = KalmanFilter(1, 30)
# self.filterYaw = KalmanFilter(0.01, 2)
self.filter_yaw = 0.0
self.yaw = 0.0
self.vw_r = 0.0
self.vw_l = 0.0
self.filter_vw_r = 0.0
self.filter_vw_l = 0.0
# def updateIMU(self, data):
# self.yaw = data.data
# self.filterYaw.input_latest_noisy_measurement(self.yaw)
# self.filter_yaw = self.filterYaw.get_latest_estimated_measurement()
def updateVW_L(self, data):
self.vw_l = data.data
self.filterL.input_latest_noisy_measurement(self.vw_l)
self.filter_vw_l = self.filterL.get_latest_estimated_measurement()
def updateVW_R(self, data):
self.vw_r = data.data
self.filterR.input_latest_noisy_measurement(self.vw_r)
self.filter_vw_r = self.filterR.get_latest_estimated_measurement()
def timerVWheelCB(self, event):
# self.filter_vw_l = self.filter_vw_l * 0.075 / 100
# self.filter_vw_r = self.filter_vw_r * 0.075 / 100
# vl = float(str(round(self.filter_vw_l, 5)))
# vr = float(str(round(self.filter_vw_r, 5)))
# print(vl, vr)
raw_vel_array = Float32MultiArray(data=[self.vw_l, self.vw_r])
filter_vel_array = Float32MultiArray(
data=[self.filter_vw_l, self.filter_vw_r])
# self.pub_raw_vel.publish(raw_vel_array)
self.pub_filter_vel.publish(filter_vel_array)
