Tầng thấp nhất: Đóng vai trò giao tiếp với phần cứng, ánh xạ các tập lệnh,
ghi nhận và truyền thông điệp từ chương trình xuống bộ phận điều khiển robot. Tầng gồm các file launch: một file đơn giản giúp khởi động môi trường giao tiếp tới robot, một file giúp trao đổi tham số tới node và có cấu trúc cơ bản như sau:
Các thẻ arg: Định nghĩa các đối số cung cấp thông tin về kích thước dụng cụ quét (qua đó giúp robot hình dung được kích thước một cell) và tọa độ điểm xuất phát (không quá quan trọng khi thực hiện bao phủ nhưng thuận tiện cho quá trình ghi nhật ký để kiểm thử và gỡ lỗi)
Các thẻ remap: Ánh xạ các biến khai báo trong chương trình thành các cấu hình cho robot để truyền và nhận thông điệp điều khiển nguồn, tốc độ tịnh tiến và tốc độ góc hiện thời cũng như truy vấn trạng thái từ tọa độ và laser.
Các thẻ param: Khai báo những chỉ số mẫu cho quá trình điểu khiển robot và được thiết lập cố định sẵn như là tốc độ tịnh tiến và tốc độ góc khởi tạo và tối đa.
Gửi dữ liệu cảm biến và tham số đầu vào Hỗ trợ các lệnh đơn giản Cung cấp các thao tác điều khiển Ánh xạ lệnh thành những thông điệp và truyền lời gọi Phân tách thao tác điều khiển thành nhiều lệnh nhỏ Yêu cầu thực hiện thao tác điều khiển Kết nối với phần cứng Đọc thông điệp và truyền lệnh tới robot Điều khiển robot thực hiện các thao tác phức tạp Triển khai giải thuật tìm đường bao phủ Tầng vật lý Tầng giao tiếp Tầng điều khiển Tầng giải thuật
51
Tầng tiếp theo: Nhận thông điệp từ file launch gửi lên và truyền các lệnh điều khiển xuống. Tầng này thực hiện những nhiệm vụ:
Đọc các thông số báo cáo tham số đầu vào như kích thước dụng cụ quét, tên giải thuật, tọa độ điểm xuất phát... để thiết lập cấu hình khởi tạo.
Tiếp nhận tình trạng hiện thời của robot: Vị trí, tốc độ tịnh tiến, tốc độ góc, sự xuất hiện của vật cản ở 2 bên trái phải và trước mặt.
Truyền các lệnh điều khiển cơ bản: Thiết lập tốc độ tịnh tiến, tốc độ góc và miền lấy mẫu cho cảm biến.
Tầng giữa: Trừu tượng hoá mọi xử lý ở tầng thấp hơn, cung cấp các hàm hỗ
trợ robot thực hiện những thao tác phức tạp như đi đến một điểm cho trước, xoay một góc bất kỳ, tìm kiếm vật cản trong miền lấy mẫu... Tầng này đóng vai trò trung gian giữa những lệnh phức tạp ở tầng dưới với giải thuật được sử dụng ở tầng trên.
Tầng trên cùng: Thử nghiệm giải thuật, cụ thể ở đây là giải thuật tìm đường
đi theo bề rộng sử dụng giải thuật mặt sóng. Giải thuật sử dụng1 hàm để đánh trọng số các ô lưới trong bản đồ, 1 hàm dùng để thực thi luật tie-break
trong trường hợp có nhiều ô cùng trọng số, 1 hàm dùng để lựa chọn đường đi bằng cách chọn các ô hàng xóm có trọng số lớn nhất. Ngoài việc thực thi các xử lý theo đúng mô tả giải thuật, những hàm trên là đoạn mã duy nhất được truyền xuống các tầng dưới để robot hiểu và thể hiện ra bên ngoài rằng giải thuật đã thực sự chạy như thế nào.
52
Mã giả thuật toán 3: ccd_star (pha khởi tạo): Thuật toán 3: CCD_Star initialization()
Đầu vào: Khu vực làm việc W
Các bƣớc tiến hành:
1. Khởi tạo biến các giá trị ban đầu cần thiết cho cell với cell là các giá trị và kiểm tra ô đó đã được D* thăm chưa hoặc đã được D*' thăm chưa....
2. Khởi tạo ban đầu cho starting_cell và ô quay lui chính của nó
3. Xây dựng list_cells gồm 10x10 temp_cell ảo xung quanh bốn hướng với ô starting_cell
Mã giả thuật toán 4: tìm đƣờng đi có thể theo D* và D*'(pha tìm đƣờng giả định)
Mã giả thuật toán 4: d_star (X,d,t)
Đầu vào: X là cell robot đang đứng, D tính D* hoặc D*',t là trạng trái truy nhập lần thứ mấy
Các bƣớc tiến hành:
1. Thiết lập list_path và list_path_d_star_extra từ ô mà nó đang đứng
2. if D*' &&( FIRST_CALL || THIRD_CALL) then
3. Xóa list_path_d_star_extra.
4. Thiết lập lại trên các ô xung quanh giống vị trí ban đầu như chưa thăm
53
6. Tính toán các giá trị các ô xung quanh khi chưa được tính của ô cho D* và D*'
7. Tìm trong temp_cell ảo thiết lập các bốn ô tại bốn hướng (trên, dưới, trái, phải) xung quanh vị trí robot đang đứng.
8. Đánh dấu các ô mà robot đang đứng là đã thăm
9. if D* then
10. Xóa list_neighbors
11. Tìm kiếm cell bên lân cận vị trí nó đang đứng
12. if No_visit then thêm vào list_neighbors khi
13. if không có các ô lân cận then
14. Đánh dấu ô đang đứng cell_for_check_third_call để thưc hiện D*'
15. Gọi lênh tìm các ô lân cận với D*'
16. else
17. Tìm kiếm ô quay lui backpoint trong list_neighbors nếu giá trị bé nhất.
18. Cập nhật lại ô cha cho ô hiện tại
19. Thêm cell backpoint vào list_path
20. Tiếp tục gọi hàm d_star( X, D, t) tìm kiếm backpoint, D* với
SECOND_CALL
54
22. Thiết lập ô đang đứng là đã thực hiện D*'
23. Thực hiên tìm kiếm các cell lân cận sao nếu nó được thăm hay chưa theo thứ tứ các hướng trên, dưới, trái, phải.
24. if neighbor thoả mãn nó chưa đc thăm then
25. Thêm neighbor up vào list_path_d_star_extra
26. Thêm tất cả list_path_d_star_extra vào list_path
27. xóa list_path_d_star_extra
28. Tiếp tục gọi hàm d_star( X, D, t) tìm kiếm neighbor, D* với
SECOND_CALL
29. else if neighbor chưa được thăm khi thực hiên D*' then thêm vào list neighbor để xét
30. if không có ô lân cạn nào then
31. Thoát
32. else
33. Tìm kiếm ô quay lui backpoint trong list_neighbors nếu giá trị lớn nhất.
34. Thiết lập ô quay lui với D*' cho ô đang xét là backpoint
35. Tiếp tục gọi hàm d_star( X, D, t) tìm kiếm backpoint, D* với
FOURTH_CALL
36. else //các trường hợp của D*' còn l
37. Thực hiên tìm kiếm các cell lân cận sao nếu nó được thăm hay chưa theo thứ tứ các hướng trên, dưới, trái, phải.
55
38. if neighbor thoả mãn nó chưa đc thăm then
39. thêm neighbor up vào list_path_d_star_extra
40. thêm tất cả list_path_d_star_extra vào list_path
41. xóa list_path_d_star_extra
42. Tiếp tục gọi hàm d_star( X, D, t) tìm kiếm neighbor, D* với
SECOND_CALL
43. else if neighbor chưa được thăm khi thực hiên D*' then thêm vào list neighbor để xét
44. if không có ô lân cận nào then
45. Gọi hàm d_star( X, D, t) tìm kiếm cell_for_check_third_call,
D*' với THIRD_CALL
46. else
47. Tìm kiếm ô quay lui backpoint trong list_neighbors nếu giá trị bé nhất.
48. Cập nhật lại ô cha cho ô hiện tại
49. Thêm cell backpoint vào list_path
50. Tiếp tục gọi hàm d_star( X, D, t) tìm kiếm backpoint, D*' với
56
Mã giả thuật toán 5 (Pha thực thi) Thuật toán 5: CCD_star Scan
Đầu vào: cell hiện tại robot đang đứng
1. Tại cell hiện tại gọi D* cho lần thực hiện đầu tiên để có list_path 2. Duyệt list path
3. Xác định hướng của ô tiếp theo so với ô hiện tại 4. Kiểm tra cell hiện tại đã được thăm hay chưa 5. If visited_r = false then
6. If nhìn thấy vật cản dưới, trái, phải so với hướng đã xác định then 7. Giá trị cho next_cell D* và D*'
8. If phía trước nhìn thấy vật cản then
9. Giá trị cho next_cell D* và D*'
10. Dừng robot lại và thực hiện replanning, ccd_scan tại vị trí hiện tại.
11. else
12. visited_r true 13. overlapped_r true
14. Di chuyển robot theo hướng phía trước bằng 1 tool_size 15. cập nhật giá trị cell hiện tại sau khi di chuyển.
57