SEVT case study

1. Mô tả bài toán

Sơ đồ mô phỏng nhà máy SEVT như hình bên dưới

AMR sẽ xuất phát tại điểm STANDBY_1, chờ nhận nhiệm vụ từ server.

Với mỗi nhiệm vụ, server sẽ gửi 2 danh sách bao gồm 1 danh sách chứa các line để lấy hàng (UNLOAD) và 1 danh sách chứa các line để trả hàng (LOAD). Hai danh sách này có độ dài bằng nhau và thứ tự xếp theo từng cặp line lấy hàng và line trả hàng

Vị trí các LINE lấy hàng được ký hiệu là : Lxx1, Lxx2 trên bản đồ. Trong đó xx là id của line, hậu tố 1, 2 tương ứng với slot 1, 2 của line. Mỗi line có 2 slot để có thể lấy hàng.

Vị trí các line trả hàng được ký hiệu là: Pxx trên bản đồ. Trong đó xx là id của line.

Khi nhận nhiệm vụ, robot xuất phát từ STANDBY đi tới điểm lấy hàng (Lxx1 hoặc Lxx2). Khi gần tới vị trí Lxx, robot giảm tốc độ để tiếp cận các băng tải của Lxx cho chính xác. Robot dừng tại Lxx và tiến hành lấy hàng. Sau khi lấy hàng xong, robot tiếp tục đi tới các điểm lấy hàng (Lxx) tiếp theo. Khi đã lấy hàng xong ở tất cả các điểm lấy hàng trong danh sách, robot sẽ đi tới các điểm trả hàng Pxx. Robot sẽ lần lượt di chuyển tới các điểm trả hàng và trả hàng vào đúng line (theo cặp như trong danh sách).

Lưu ý, số lượng từng cặp line lấy hàng và trả hàng có thể thay đổi tùy theo mỗi nhiệm vụ. Ví dụ, có nhiệm vụ thì chỉ lấy hàng và trả hàng 1 lần, nhưng có nhiệm vụ có thể là lấy 2 line và trả 2 line, hoặc lấy 3 line và trả 3 line.

2. Soạn TVC-NAV Mission

2.1. Phần line lấy hàng

Với mỗi line lấy hàng, quy trình di chuyển của robot như sau:

1. Di chuyển tốc độ bình thường tới gần line
2. Di chuyển tốc độ chậm, bám chính xác đường để tiếp cận chính xác băng tải
3. Dừng lại để gọi băng tải lấy hàng

Tùy vào số lượng line lấy hàng, quy trình này sẽ lặp lại tương ứng. Các action tương ứng với quy trình này như sau:

1. Đầu tiên, thêm các biến runtime-variables vào để cập nhật dữ liệu từ ACS lên cho TVC-NAV Mission, các runtime-variables như sau:

• Biến currentUnloadMarkerId dùng để cập nhật marker hiện tại mà robot đang đứng
• Biến approachMarkerId cập nhật vị trí gần với line đích. Robot sẽ di chuyển tốc độ bình thường từ currentUnloadMarkerId tới approachMarkerId
• Biến lineMarkerId cập nhật vị trí line đích.

2. Thêm action While-Do để lặp lại nhiệm vụ lấy hàng bên trên cho tới khi xử lý hết line lấy hàng trong danh sách

3. Trong phần kiểm tra điều kiện của action WHILE DO, kiểm tra xem biến currentUnloadMarkerId có rỗng không, nếu rỗng có nghĩa là đã xử lý hết các line lấy hàng.

4. Tiếp theo trong phần action con của action WHILE DO, ta có action FOLLOW PATH, action này cho robot đi từ vị trí marker hiện tại currentUnloadMarkerId tới approachMarkerId, chế độ di chuyển bình thường (tốc độ bình thường, độ chính xác bám đường khoảng 20cm)

5. Action tiếp theo là action FOLLOW PATH nhưng với chế độ Craw to stop signal, ở chế độ này robot sẽ đi chậm, bám đường với độ chính xác cao (2cm) và dừng lại tại đúng vị trí bằng chuyền cần ghép. Khi kết thúc action này (robot ghép chính xác vào vị trí băng chuyền lấy hàng), TVC-NAV sẽ gọi 1 RESTFUL API (http://localhost:40000/handle-unload khai báo trong ô Webhook host) để robot thực hiện lấy hàng từ băng chuyền. Do đó sau action FOLLOW PATH này, ta sẽ để 1 action PAUSE để robot đứng chờ callback thực hiện xong, sau khi callback thực hiện xong việc lấy hàng sẽ gọi service unpause để tiếp tục nhiệm vụ.

2.2. Phần line trả hàng

Tương tự như phần line lấy hàng, soạn action như sau:

Config điều khiện cho WHILE-DO:

Config cho action FOLLOW-PATH:

2.3. Toàn bộ mission:

3. Code client

• Ở bước trên, ta đã soạn ra mission cho TVC-NAV. Tuy nhiên để TVC-NAV mission hoạt động được thì ta cần có 1 module gửi dữ liệu (tùy theo từng nhiệm vụ) cho TVC-NAV thông qua các runtime-variables đã định nghĩa bên trên.
• Dưới đây là pseudo-code (javascript) mô tả cách viết 1 module điểu khiển hoạt động của TVC-NAV mission đã soạn ở trên với dữ liệu nhận từ ACS.

3.1. Xử lý dữ liệu nhận từ ACS

• Ở bước này SEVT Client nhận bản tin nhiệm vụ từ ACS
• SEVT Client cần xác định các marker tương ứng các line trong nhiệm vụ, lưu các dữ liệu marker này vào bộ nhớ để dùng lúc thực hiện TVC-NAV Mission

Code mẫu javascript dưới đấy sẽ lấy input là bản tin từ ACS acsMessage, sau đó xử lý và lưu ra 2 mảng packageUnloadDestinations và packageLoadDestinations, để lưu các dữ liệu cho TVC-NAV dùng trong 2 vòng lặp WHILE DO như mission đã được soạn ở phía trên.

async prepareMissionData(acsMessage) {
		// Từ bản tin ACS, tính ra các marker cho line lấy hàng (Unload) và trả hàng (Load), sắp xếp các marker theo thứ tự đường đi của Robot
  	const {sortedUnloadDestinations, sortedLoadDestinations} =
          findDestinationFromMessage(acsMessage);

  	const currentMarkerName = standbyName;
  	let targetName;
  	let beginStraightlineMarkerName;

    // Xử lý và lưu dữ liệu cho các line lấy hàng
    sortedUnloadDestinations.forEach((destination) => {

      // Tìm ra vị trí marker line của robot
      targetName = getTargetLineMarkerName(destination);

      // Tìm ra vị trí bắt đầu vào line chính xác
      beginStraightlineMarkerName = getBeginStraightlineMarkerName(destination);

      // Lưu dữ liệu markerId vào trong bộ nhớ
      packageUnloadDestinations.push({
          currentUnloadMarkerId: webappAPI.getMarkerIdByName(markers, currentMarkerName),
          approachMarkerId: webappAPI.getMarkerIdByName(
            markers,
            beginStraightlineMarkerName
          ),
          lineMarkerId: webappAPI.getMarkerIdByName(markers, destination),
          slotNumber: Number(destination.at(3)) - 1,
      });

      currentMarkerName = destination;
    });

  	// Xử lý và lưu dữ liệu cho các line trả hàng
    sortedLoadDestinations.forEach((destination) => {

			// Tìm ra vị trí marker line của robot
      targetName = getTargetLineMarkerName(destination);

      // Tìm ra vị trí bắt đầu vào line chính xác
      beginStraightlineMarkerName = getBeginStraightlineMarkerName(destination);

      packageLoadDestinations.push({
        currentMarkerName,
        beginStraightlineMarkerName,
        destination,
        currentLoadMarkerId: webappAPI.getMarkerIdByName(markers, currentMarkerName),
        approachMarkerId: webappAPI.getMarkerIdByName(
          markers,
          beginStraightlineMarkerName
        ),
        lineMarkerId: webappAPI.getMarkerIdByName(markers, destination),
        slotNumber: webappAPI.getLoadLineSlot(destination),
      });
      currentMarkerName = destination;

    });
  }

3.2. Code gọi thực hiện Start Mission

• Do TVC-NAV Mission khi chạy sẽ cần tới dữ liệu để chạy tới line lấy hàng đầu tiên, nên khi gọi API start mission cho TVC-NAV, ta cần gửi cùng với dữ liệu của line đầu tiên

  async startMission() {
    // Kiểm tra xem dữ liệu được chuẩn bị đã được lưu chưa, nếu chưa có dữ liệu thì sẽ kết thúc ngay
    if (!packageLoadDestinations || packageLoadDestinations.length == 0)
      return;

    // Lấy ra dữ liệu của Line lấy hàng đầu tiên
    const initialVariable = packageUnloadDestinations[0];
    const initialVariables = Object.keys(initialVariable).map((key) => ({key, value: initialVariable[key]}));

    // Gọi bắt đầu TVC-NAV mission với dữ liệu trên
    executedMission = (await webappAPI.executeMission({ initialVariables}))?.data;

    // Lưu trạng thái để biết là đang xử lý ở Line nào
    currentStates.currentLoadDestinationIndex = 0;
    currentStates.currentUnloadDestinationIndex = 0;
  },

3.3. Code callback lấy hàng

• Sau khi robot dừng lại chính xác ở vị trí ghép với băng tải lấy hàng, robot sẽ thực hiện action PAUSE và gọi tới call-back lấy hàng. Callback này sẽ thưc hiện điều khiển bay tải lấy hàng và gửi dữ liệu để robot thực hiện các action tiếp theo

router.post("/handle-unload", async (req, res) => {
  // Gọi điều khiển bằng chuyền lấy hàng
  conveyorUnload();

  let nextVariableValue;
  if (
    currentStates.currentUnloadDestinationIndex + 1 <
    packageUnloadDestinations.length
  ) {
    // Nếu vẫn còn line trong danh sách line lấy hàng thì lấy ra dữ liệu của line lấy hàng tiếp theo
    nextVariableValue =
      packageUnloadDestinations[
        currentStates.currentUnloadDestinationIndex + 1
      ];

    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "currentUnloadMarkerId",
      nextVariableValue.currentUnloadMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "approachMarkerId",
      nextVariableValue.approachMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "lineMarkerId",
      nextVariableValue.lineMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "slotNumber",
      nextVariableValue.slotNumber,
    );

    currentStates.currentUnloadDestinationIndex =
      currentStates.currentUnloadDestinationIndex + 1;
  } else {
    // Nếu không còn line lấy hàng thì cập nhật biến `currentUnloadMarkerId` thành rỗng để kết thúc vòng lặp xử lý lấy hàng.
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "currentUnloadMarkerId",
      "",
    );
    currentStates.currentLoadDestinationIndex = 0;

    // Kết thúc lấy hàng (Unload) và chuyển sang xử lý các line nhận hàng (Load), gửi dữ liệu về line nhận hàng đầu tiên để robot thực hiện nhiệm vụ
    nextVariableValue = packageLoadDestinations[0];
    console.log("Conveyor Load: next variable: ", nextVariableValue);

    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "currentLoadMarkerId",
      nextVariableValue.currentLoadMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "approachMarkerId",
      nextVariableValue.approachMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "lineMarkerId",
      nextVariableValue.lineMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "slotNumber",
      nextVariableValue.slotNumber,
    );
  }

  res.status(200).send(nextVariableValue);
  // Gọi tới lệnh unpause để robot thực hiện nhiệm vụ
  rosService.unpause();
});

3.4. Code callback nhận hàng

• Sau khi robot dừng lại chính xác ở vị trí ghép với băng tải nhận hàng, robot sẽ thực hiện action PAUSE và gọi tới call-back nhận hàng. Callback này sẽ thưc hiện điều khiển bay tải nhận hàng và gửi dữ liệu để robot thực hiện các action tiếp theo

router.post("/handle-load", async (req, res) => {
  // Gọi điều khiển bằng chuyền nhận hàng
  conveyorUnload();

  // update variable
  let nextVariableValue;
  if (
    currentStates.currentLoadDestinationIndex + 1 <
    packageLoadDestinations.length
  ) {
    // Nếu vẫn còn line trong danh sách line nhận hàng thì lấy ra dữ liệu của line nhận hàng tiếp theo
    nextVariableValue =
      packageLoadDestinations[currentStates.currentLoadDestinationIndex + 1];
    console.log("Conveyor Load: next variable: ", nextVariableValue);
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "currentLoadMarkerId",
      nextVariableValue.currentLoadMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "approachMarkerId",
      nextVariableValue.approachMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "lineMarkerId",
      nextVariableValue.lineMarkerId,
    );
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "slotNumber",
      nextVariableValue.slotNumber,
    );

    currentStates.currentLoadDestinationIndex =
      currentStates.currentLoadDestinationIndex + 1;
  } else {
    // Kết thúc nhận hàng, cập nhật biến `currentLoadMarkerId` thành rỗng để kết thúc vòng lặp xử lý nhận hàng. TVC-NAV sẽ tiếp tục thực hiện các action kế tiếp sau khi unpause
    await webappAPI.setRuntimeVariable(
      executedMission.id,
      "currentLoadMarkerId",
      "",
    );
    currentStates.currentLoadDestinationIndex = 0;
  }

  // Gọi tới lệnh unpause để robot thực hiện nhiệm vụ
  res.status(200).send(nextVariableValue);
  rosService.unpause();
});