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
