Node example, class template for your project

[oscar-lima]

#!/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()