surveyor slam建图中需要的信息参数-breezyslam的应用

在github中找到一个关于slam入门级别建图的学习，这里简单的梳理一下。

chasis 底盘中可以采集传感器的信息，包括BNO055 的imu信息，还有左右两轮子的转速信息。由于DIY这个slam建图的工程师是在乐高EV3上实现的

并且使用java 开发的，本人java水平有限，而且对乐高接口不熟悉，连猜带蒙写一些理解。

有两个比较重要的类，一个是 MapTracker ，提供 位姿信息，位姿包括当前位姿，目标位姿，位姿差，航向角，目标航向。

public static enum PlanState{NONE, DIRECT, PLAN};
    final static float[] speedMult = {1.0f, 0.75f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    PoseProvider imuPose;
    Pose deltaPose = new Pose();
    public Pose targetPose = new Pose();
    float heading = 0.0f;
    public float targetSpeed = 0.0f;
    public PlanState planState;

另一个类是 LineFollower  提供 底盘的当前速度，目标速度，加速度值

  protected Chassis chassis;
    protected MapTracker tracker;
    protected float P, I, D;
    protected FollowState state = FollowState.STOPPED;
    protected Thread thread;
    //protected static final float MAX_STEER = 125.0f;
    protected static final float MAX_STEER = 10.0f;
    protected static final long LOOP_TIME = 100;
    protected float curSpeed = 0.0f;
    protected float targetSpeed = curSpeed;
    protected float acceleration = 50.0f;
    protected float outputVal;

有以上两个类生成 StatusTracker，

 public StatusTracker(LineFollowerChassis chassis, LineFollower follower, Pose p, String name, String host, Power batt, SampleProvider ir, MapTracker tracker) throws UnknownHostException, IOException
    {
        this.chassis = chassis;
        this.name = name;
        odom = chassis.getPoseProvider();
        imu = chassis.getIMUPoseProvider();
        battery = batt;
        infraRed = ir;
        this.follower = follower;
        this.tracker = tracker;
        setPose(p);
        sock = new Socket(host, 2446);
        dos = new DataOutputStream(sock.getOutputStream());
        dis = new DataInputStream(sock.getInputStream());
        dos.writeUTF(name);
        Thread thread = new Thread(this);
        thread.setDaemon(true);
        thread.start();
    }

最终通过lidar 捕获一帧一帧的数据 写入log

    public Lidar(Port motorPort, Port sensorPort, RegulatedMotor left, RegulatedMotor right, PoseProvider robotPose, PoseProvider pose2, String logName, StatusTracker status) throws IOException
    {
        System.out.println("Starting Lidar");
        log = new PrintWriter(new FileWriter(logName));
        motor = new EV3LargeRegulatedMotor(motorPort);
        laser = new I2CSensor(sensorPort, ADDRESS, I2CPort.TYPE_HIGHSPEED);
        this.left = left;
        this.right = right;
        this.robotPose = robotPose;//对应的pose,有bno055提供的
        this.robotPose2 = pose2;//对应的pose2
        this.status = status;
        running = true;
        Thread scanThread = new Thread(
                new Runnable() {
                    public void run() {
                        runCaptureScans();
                    }
                }
            );
        Thread processThread = new Thread(
                new Runnable() {
                    public void run() {
                        runProcessScans();
                    }
                }
            );
        scanThread.setDaemon(true);
        scanThread.setPriority(Thread.MAX_PRIORITY);
        processThread.setDaemon(true);
        processThread.start();
        scanThread.start();
    }
最终处理一帧汇总后，

protected void runProcessScans()
    {
        System.out.println("Lidar process thread running");
        while (running)
        {
            LidarScan scan = waitForScan();
            if (scan == null)
            {
                System.err.println("Missing lidar scan");
                break;
            }
            log.print(scan.timestamp*1000 + " " + scanCnt++ + " " + scan.leftTacho + " " + scan.rightTacho + " " + 
                    scan.pose.getX() + " " + scan.pose.getY() + " " + scan.pose.getHeading() + " " +
                    scan.pose2.getX() + " " + scan.pose2.getY() + " " + scan.pose2.getHeading() + 
                        " 0 0 0 0 0 0 0 0 0 0 0 0 0 0");
            int zero = 0;
            for(int i = 0; i < scan.ranges.length; i++)
            {
                log.print(" " + scan.ranges[i]*10);
                if (scan.ranges[i] == 0) zero++;
            }
            /*
            for(int i = 0; i < scan.ranges.length; i++)
            {
               if (scan.poses[i] != null)
                    log.print(" " + scan.poses[i].getX() + " " + scan.poses[i].getY() + " " + scan.poses[i].getHeading());
                else
                    log.print(" 0.0 0.0 0.0");
            }
            */
            log.println();
            //System.out.println("scan " + scanCnt + " missed " + missed + " retry " + retry + " error " + error + " zero " + zero);
            status.setScan(scan);
        }
    }

log.dat 如下

heading 经过了归一化处理。范围在-180 -180 

while (newPose < -180)
                newPose += 360;
              while (newPose > 180)
                newPose -= 360;

先不用考虑乐高的底层是怎么计算出来的，只要在我们的底盘中能够获取正确的传感器信息，并把这些数据 按照图中的格式写入log中，下一步就可以slam建图了。