AMiRo-OS

      // Set basic valid map configuration

      setAttributes(global.testmap, 0, 1, 2, 1);

      setAttributes(global.testmap, 1, 2, 2, 0);

      setAttributes(global.testmap, 2, 1, 0, 0);

      setAttributes(global.testmap, 3, 0, 0, 0xff);

      chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());

      global.tcase++; // 1

      // Test map fail if first node is flagged with end

      // Test if node 2 is set as start node

      // Test if non reachable nodes will trigger invalid map

      global.tcase++; // 4

      // Test Reinitialization

      setAttributes(global.testmap, 0, 1, 2, 1);

      setAttributes(global.testmap, 1, 2, 2, 0);

      setAttributes(global.testmap, 2, 1, 0, 0);

      setAttributes(global.testmap, 3, 0, 0, 0xff);

      map.initialize();

      global.testres[global.tcase] = map.get_state()->valid;

      global.odometry.resetPosition();

      uint8_t ret = 0;

      global.tcase++; // 5

      // Test update under normal linefollowing without fixpoint

      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] = (ret == 0x4);

      global.odometry.setPosition(1.0, 0.0, 0.0);

      global.tcase++; // 6

      // Fixpoint on left side

      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      // global.odometry

      global.testres[global.tcase] = (ret == 0x1)

        && (map.get_state()->strategy == 0x01)

        && (map.get_state()->dist == 0)

        && (map.get_state()->current == 2);

      global.odometry.setPosition(1.5, 0.0, 0.0);

      global.tcase++; // 7

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] = (ret == 0x00)

        && (map.get_state()->strategy == 0x01);

        // && (map.get_state()->dist == 0);