#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
class EKFLocalizationKinect(object):
'''
listens to topic emotions, turns on/off different LED'S of the mbot head
to mimic the desired emotion
'''
def __init__(self):
'''
Constructor (gets executed only once at the moment of object creation)
'''
rospy.Subscriber("/robot_0/odom", Odometry, self.odomCallback, queue_size=1)
rospy.Subscriber("/robot_0/base_scan_1", LaserScan, self.laserCallback, queue_size=1)
self.odom_msg_received = False
self.laser_msg_received = False
self.odom_msg = None
self.laser_msg = None
self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 10.0))
self.updated_robot_pose = None # or set to initial robot pose?
self.whatever_name_i_want = 3.0
def odomCallback(self, msg):
'''
gets executed every time you receive a new msg on topic /robot_0/odom
'''
self.odom_msg = msg
self.odom_msg_received = True
def laserCallback(self, msg):
'''
gets executed every time you receive a new msg on topic /robot_0/base_scan_1
'''
self.laser_msg = msg
self.laser_msg_received = True
def compare_timestamps(self):
'''
ensure that no big time difference exists between your msgs for sync purposes
'''
# get timestamps of both (from header)
# compare
# if bigger than a threshold then return falsem otherwise true
return True # HACK for now to do it later
def ekf_update(self):
'''
perform one EKF update
'''
self.updated_robot_pose = call_kalman(args...)
def start_ekf_loc(self):
'''
main loop
'''
while not rospy.is_shutdown():
if self.odom_msg_received == True:
rospy.loginfo('odom msg received!')
if self.laser_msg_received == True:
rospy.loginfo('laser msg received!')
# lower flags
self.laser_msg_received = False
self.odom_msg_received = False
# compare timestamps
if self.compare_timestamps()
# update eyes emotion
self.ekf_update()
else:
rospy.logwarn('odom and laser msgs are not in sync')
# sleep to control node frequency
self.loop_rate.sleep()
if __name__ == '__main__':
rospy.init_node('my_ekf_localication_kinect', anonymous=False)
# create object of the EKF class (constructor gets executed)
my_ekf_localication_kinect = EKFLocalizationKinect()
# call main function
my_ekf_localication_kinect.start_ekf_loc()
