class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.model_noise_rate = 0.15 self.d_thresh = .2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # ????? # rospy.Subscriber('/simple_odom', Odometry, self.process_odom) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received, queue_size=10) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # [.0] * 3 # self.initial_particles = self.initial_list_builder() # self.particle_cloud = self.initialize_particle_cloud() print(self.particle_cloud) # self.current_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map mapa = obter_mapa() self.occupancy_field = OccupancyField(mapa) # self.update_particles_with_odom(msg) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(new_odom_xy_theta) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) for p in self.particle_cloud: p.x += delta[0] p.y += delta[1] p.theta += delta[2] self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # ???? # TODO: modify particles using delta for p in self.particle_cloud: r = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2)) p.theta += r % 360 p.x += d * math.cos(p.theta) + normal(0, .1) p.y += d * math.sin(p.theta) + normal(0, .1) p.theta += (delta[2] - r + normal(0, .1)) % 360 # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized # TODO: fill out the rest of the implementation self.particle_cloud = ParticleFilter.weighted_values(self.particle_cloud, [p.w for p in self.particle_cloud], len(self.particle_cloud)) for p in particle_cloud: p.w = 1 / len(self.particle_cloud) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this for r in msg.ranges: for p in self.particle_cloud: p.w = 1 self.occupancy_field.get_particle_likelyhood(p, r, self.model_noise_rate) self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) print(size, bins) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # TODO create particles self.particle_cloud = self.initial_list_builder(xy_theta) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this w_sum = 0 for p in self.particle_cloud: w_sum += p.w for p in self.particle_cloud: p.w /= w_sum def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete # print 1 return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node print 2 return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node print 3 return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(self.current_odom_xy_theta) # Essa list não está sendo "refeita" / preenchida print(new_odom_xy_theta) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) print(math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]), "hi") elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! ''' É AQUI!!!! ''' print('jorge') self.update_particles_with_odom(msg) # update based on odometry - FAZER self.update_particles_with_laser(msg) # update based on laser scan - FAZER self.update_robot_pose() # update robot's pose """ abaixo """ self.resample_particles() # resample particles to focus on areas of high density - FAZER self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.Time.now(), self.odom_frame, self.map_frame) def initial_list_builder(self, xy_theta): ''' Creates the initial particles list, using the super advanced methods provided to us by the one and only John Cena ''' initial_particles = [] for i in range(self.n_particles): p = Particle() p.x = gauss(xy_theta[0], 1) p.y = gauss(xy_theta[1], 1) p.theta = gauss(xy_theta[2], (math.pi / 2)) p.w = 1.0 / self.n_particles initial_particles.append(p) return initial_particles
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ xy_theta = [] def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 250 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.model_noise_rate = 0.15 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) print() # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField rospy.wait_for_service('static_map') grid = rospy.ServiceProxy('static_map',GetMap) my_map = grid().map # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.field = OccupancyField(my_map) self.initialized = True def create_initial_particle_list(self,xy_theta): init_particle_list = [] n = self.n_particles for i in range(self.n_particles): w = 1.0/n x = gauss(xy_theta[0],0.5) y = gauss(xy_theta[1],0.5) theta = gauss(xy_theta[2],((math.pi)/2)) particle = Particle(x,y,theta,w) init_particle_list.append(particle) print("init_particle_list") return init_particle_list def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin x = 0 y = 0 theta = 0 angles = [] for particle in self.particle_cloud: x += particle.x * particle.w y += particle.y * particle.w v = [particle.w * math.cos(math.radians(particle.theta)), particle.w * math.sin(math.radians(particle.theta))] angles.append(v) theta = sum_vectors(angles) orientation = tf.transformations.quaternion_from_euler(0,0,theta) self.robot_pose = Pose(position=Point(x=x,y=y),orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ print('update_w_odom') new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta for particle in self.particle_cloud: parc = (math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]) % 360 particle.x += (math.sqrt((delta[0]**2) + (delta[1]**2)))* math.cos(parc) particle.y += (math.sqrt((delta[0]**2) + (delta[1]**2))) * math.sin(parc) particle.theta += delta[2] else: self.current_odom_xy_theta = new_odom_xy_theta return #DONE # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_random_particle = ParticleFilter.weighted_values(values,probs,self.n_particles) new_particles = [] for i in new_random_particle: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append(Particle(x=s_p.x+gauss(0,.025),y=s_p.y+gauss(0,.05),theta=s_p.theta+gauss(0,.05))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ print('update_w_laser') readings = msg.ranges for particle in self.particle_cloud: for read in range(0,len(readings),3): self.field.get_particle_likelyhood(particle,readings[read],self.model_noise_rate,read) self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)-1] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ print('draw_random_sample') values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) #self.particle_cloud = [] # TODO create particles self.particle_cloud = self.create_initial_particle_list(xy_theta) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ w_sum = sum([p.w for p in self.particle_cloud]) for particle in self.particle_cloud: particle.w /= w_sum # TODO: implement this def publish_particles(self, msg): print('publish_particles') particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ print('scan_received') if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return print('broadcast') self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.Time.now(), self.odom_frame, self.map_frame)