def LidarHandler(self,channel,data): msg = rplidar_laser_t.decode(data) self.ranges = msg.ranges self.thetas = msg.thetas self.nranges = msg.nranges self.laser_times = msg.times self.intensities = msg.intensities if self.count == 0: return vel_avg = (self.vel_accum[0],self.vel_accum[1],self.vel_accum[2]) self.vel_accum = (0,0,0) self.count = 0 tmp_range = map(lambda ranges: 1000*ranges, self.ranges) tmp_thetas = map(lambda thetas: RAD2DEG*thetas, self.thetas) curr_position = self.slam.updateSlam(zip(tmp_range, tmp_thetas), vel_avg) slam_pos_msg = slam_pos_t() slam_pos_msg.abs_x = curr_position[1] slam_pos_msg.abs_y = curr_position[0] slam_pos_msg.abs_theta = curr_position[2] self.lc.publish("SLAM_POS", slam_pos_msg.encode()) if self.initialized_map: self.datamatrix.getRobotPos(curr_position) else: self.datamatrix.getRobotPos(curr_position, True) self.initialzed_map = True ret = self.slam.getBreezyMap() self.datamatrix.drawBreezyMap(ret) self.datamatrix.drawInset() self.slamPos = self.datamatrix.get_robot_abs()
def __init__(self): self.rel_pos = (0, 0, 0) self.count = 0 self.vel_accum = (0,0,0) self.initialzed_map = False self.currOdoVel = (0,0,0) self.ranges = 0 self.thetas = 0 self.nranges = 0 self.laser_times = 0 self.intensities = 0 self.map_count = 0 self.slam_pos_msg = slam_pos_t() self.lc = lcm.LCM() lcmOdoVelSub = self.lc.subscribe("BOT_ODO_VEL", self.OdoVelocityHandler) rpLidarSub = self.lc.subscribe("RPLIDAR_LASER", self.LidarHandler) self.laser = RPLidar(DIST_MIN, DIST_MAX) # lidar self.datamatrix = DataMatrix(**KWARGS) # handle map data self.slam = Slam(self.laser, **KWARGS) # do slam processing