def laser_callback(self, msg): ''' Function called each time a LaserScan message with topic "scan" arrives. ''' # Project LaserScan to points in space rng = np.array(msg.ranges) ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) points = np.vstack((rng * np.cos(ang), rng * np.sin(ang))) msg.range_max = 3 self.pub_laser.publish(msg) # Filter long ranges points = points[:, rng < msg.range_max] # Use split and merge to obtain lines and publish lines = splitandmerge(points) self.counter += 1 new_scan = 'scan_line' + str(self.counter) #publish_lines(lines, self.pub_line, frame=msg.header.frame_id, #time=msg.header.stamp, ns=new_scan, color=(1,0,0)) # Publish results publish_lines(lines, self.pub_line, frame=msg.header.frame_id, time=msg.header.stamp, ns='scan_line', color=(1,0,0))
def lines_callback(self, msg): """Callback to a LaserScan message with topic "scan".""" # Save time self.linestime = msg.header.stamp # Get the lines if len(msg.points) > 0: # Structure for the lines self.lines = np.zeros((len(msg.points) / 2, 4)) for i in range(0, len(msg.points) / 2): # Get start and end points pt1 = msg.points[2 * i] pt2 = msg.points[2 * i + 1] # Transform to robot frame pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0]) pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0]) # Append to line list self.lines[i, :2] = pt1R[:2] self.lines[i, 2:] = pt2R[:2] # Flag self.new_laser = True # Publish publish_lines(self.lines, self.pub_lines, frame='/robot', time=msg.header.stamp, ns='lines_robot', color=(0, 0, 1))
def publish_results(self): ''' Publishes all results from the filter. ''' # Ground turth of the map of the room publish_lines(self.map, self.pub_map_gt, frame='/world', ns='gt_map', color=(0.3, 0.3, 0.3)) odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf) publish_lines(room_map, self.pub_map, frame='/world', ns='map', color=(0, 1, 0)) self.pub_odom.publish(odom) self.pub_uncertainity.publish(ellipse) self.tfBroad.sendTransform(translation=trans, rotation=rot, time=self.time, child='/robot', parent='/world')
def cbk_lines(self, msg): """Republish the laser scam in the /robot frame.""" # Save time self.linestime = msg.header.stamp # Get the lines if len(msg.points) > 0: # Structure for the lines self.lines = np.zeros((len(msg.points) / 2, 4)) for i in range(0, len(msg.points)/2): # Get start and end points pt1 = msg.points[2*i] pt2 = msg.points[2*i+1] # Transform to robot frame pt1R = funcs.comp(self.robot2sensor, [pt1.x, pt1.y, 0.0]) pt2R = funcs.comp(self.robot2sensor, [pt2.x, pt2.y, 0.0]) # Append to line list self.lines[i, :2] = pt1R[:2] self.lines[i, 2:] = pt2R[:2] # Flag self.new_laser = True # Publish funcs.publish_lines(self.lines, self.pub_lines, frame='robot', time=msg.header.stamp, ns='lines_robot', color=(0, 0, 1))
def publish_results(self, lines): ''' Publishes all results from the filter. ''' if self.time is not None: time = rospy.Time.now() # Map of the room map_lines = get_dataset3_map() publish_lines(map_lines, self.pub_map, frame='/world', ns='map', color=(0,1,0)) # Particles and biggest weighted particle msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(self.part_filter, time) self.pub_particles.publish(msg) self.pub_big_particle.publish(msg_mean) self.pub_odom.publish(msg_odom) self.pub_wei.publish(msg_wei) self.tfBroad.sendTransform(translation = trans, rotation = rot, time = time, parent = 'world', child = 'mean_particle') # Publish scanned lines if lines is not None: publish_lines(lines, self.pub_lines_mean, frame='mean_particle', time=time, ns='scan_lines_mean', color=(0,0,1))
def lines_callback(self, msg): ''' Function called each time a LaserScan message with topic "scan" arrives. ''' # Save time self.linestime = msg.header.stamp # Get the lines if len(msg.points) > 0: # Structure for the lines self.lines = np.zeros((len(msg.points) / 2, 4)) for i in range(0, len(msg.points)/2): # Get start and end points pt1 = msg.points[2*i] pt2 = msg.points[2*i+1] # Transform to robot frame pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0]) pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0]) # Append to line list self.lines[i, :2] = pt1R[:2] self.lines[i, 2:] = pt2R[:2] # Flag self.new_laser = True # Publish publish_lines(self.lines, self.pub_lines, frame = '/robot', time = msg.header.stamp, ns = 'lines_robot', color = (0,0,1))
def laser_callback(self, msg): ''' Function called each time a LaserScan message with topic "scan" arrives. ''' # Project LaserScan to points in space rng = np.array(msg.ranges) ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) points = np.vstack((rng * np.cos(ang), rng * np.sin(ang))) msg.range_max = 3 self.pub_laser.publish(msg) # Filter long ranges points = points[:, rng < msg.range_max] # Use split and merge to obtain lines and publish lines = splitandmerge(points) # # Publish results # publish_lines(lines, self.pub_line, frame=msg.header.frame_id, # time=msg.header.stamp, ns='scan_line', color=(1,0,0)) self.i = self.i + 1 nss = 'scan_line' + str(self.i) #msg.header.frame_id = "d" + str(self.i) #msg.header.frame_id = 'world' publish_lines(lines, self.pub_line, frame=msg.header.frame_id, time=msg.header.stamp, ns=nss, color=(1,0,0))
def publish_results(self): """Publishe results from the filter.""" # Map of the room (ground truth) funcs.publish_lines(self.ekf.map, self.pub_lines, frame='world', ns='map', color=(0, 1, 0)) # Get filter data odom, ellipse, trans, rot, dummy = funcs.get_ekf_msgs(self.ekf) # Publish results self.pub_odom.publish(odom) self.pub_uncertainity.publish(ellipse) self.tfBroad.sendTransform(translation=trans, rotation=rot, time=self.time, child='robot', parent='world')
def publish_results(self): ''' Publishes all results from the filter. ''' # Ground turth of the map of the room publish_lines(self.map, self.pub_map_gt, frame='/world', ns='gt_map', color=(0.3,0.3,0.3)) odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf) publish_lines(room_map, self.pub_map, frame='/world', ns='map', color=(0,1,0)) self.pub_odom.publish(odom) self.pub_uncertainity.publish(ellipse) self.tfBroad.sendTransform(translation = trans, rotation = rot, time = self.time, child = '/robot', parent = '/world')
def publish_results(self, lines): ''' Publishes all results from the filter. ''' if self.time is not None: time = rospy.Time.now() # Map of the room map_lines = get_dataset3_map() publish_lines(map_lines, self.pub_map, frame='/world', ns='map', color=(0, 1, 0)) # Particles and biggest weighted particle msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs( self.part_filter, time) self.pub_particles.publish(msg) self.pub_big_particle.publish(msg_mean) self.pub_odom.publish(msg_odom) self.pub_wei.publish(msg_wei) self.tfBroad.sendTransform(translation=trans, rotation=rot, time=time, parent='world', child='mean_particle') # Publish scanned lines if lines is not None: publish_lines(lines, self.pub_lines_mean, frame='mean_particle', time=time, ns='scan_lines_mean', color=(0, 0, 1))