概要
ルンバを 2 [m] くらい直進させることにし、途中に障害物があれば停止させます。
停止しているときに、障害物が取り除かれたら前進を再開します。
方針
状態遷移でルンバの行動を管理します。
具体的には、前方に障害物があって停止している状態(Stopping)と障害物がなくて停止している状態(Moving)と、移動が完了した状態(End) を定義します。
実装
Moving と Stopping, そして End それぞれの処理に分けて説明します。ソースコード全体は stop_to_obstacle.lua となります。
Moving の処理
これらの処理を Lua スクリプトで実装すると、以下のようになります。
start_state = {} -- state: Start
start_state["Enter"] =
function(object)
end
start_state["Execute"] =
function(object)
state:change_state(moving_state, "moving_state")
end
start_state["Exit"] =
function(object)
-- 計測を開始する
urg:start_measurement(Lidar.Distance, Urg_driver.Infinity_scan_times, 0)
end
-- end state: Start
ここで Enter は、その状態に入るときに実行される処理で、Execute は状態を保持している間に実行される処理, Exit はその状態から遷移するときに実行される処理です。
つまり Start 状態では start_state["Enter"] 関数が呼ばれ、次に start_state["Execute"] 関数、そして Execute の処理に状態の遷移を指示する change_state() が呼ばれているため、次に start_state["Exit"] 関数が呼び出されます。この、状態遷移を管理する処理は State_machine.lua に実装されています。
Moving の処理
- 直線追従を開始する。
- 前方に障害物があれば Stopping に遷移する。
- 2[m] 移動したら End に遷移する。
これらの処理を Lua スクリプトで実装すると、以下のようになります。
moving_state = {} -- state: Moving
moving_state["Enter"] =
function(object)
-- 移動を開始する
robot:follow_line(goal_line_)
end
moving_state["Execute"] =
function(object)
-- 前方に障害物があるかを計測する
local is_exist_obstacle = is_exist_front_obstacle()
-- 前方に障害物があったら stopping に遷移させる
if is_exist_obstacle then
state:change_state(stopping_state, "stopping_state")
end
-- 目標位置を越えたら end に遷移させる
if robot:distance_to_perpendicular(goal_line_) >= 0 then
state:change_state(end_state, "end_state")
end
end
moving_state["Exit"] =
function(object)
end
-- end state: Moving
Stopping の処理
- 停止する。
- 障害物がなくなれば Moving に遷移する。
これらの処理を Lua スクリプトで実装すると、以下のようになります。
stopping_state = {} -- state: Stopping
stopping_state["Enter"] =
function(object)
-- 停止させる
robot:stop()
end
stopping_state["Execute"] =
function(object)
-- 前方に障害物があるかを計測する
local is_exist_obstacle = is_exist_front_obstacle(urg)
-- 前方に障害物がなければ moving に遷移させる
if not is_exist_obstacle then
state:change_state(moving_state, "moving_state")
end
end
stopping_state["Exit"] =
function(object)
end
-- end state: Stopping
End の処理
これらの処理を Lua スクリプトで実装すると、以下のようになります。
end_state = {} -- state: End
end_state["Enter"] =
function(object)
-- 停止させる
robot:stop()
-- 計測を停止する
urg:stop_measurement()
urg:close()
end
end_state["Execute"] =
function(object)
-- 実際に停止したらプログラムを終了する
if robot:is_stable() then
os.exit()
end
delay(0.1)
end
end_state["Exit"] =
function(object)
end
-- ロボットの状態遷移を処理する
state:set_current_state(start_state)
while true do
state:update()
end
-- end state: End
実行
roomba_urg の引数に Lua スクリプトを渡して実行します。
$ cd hobby_robot_sdk/example/roomba_urg
$ ./roomba_urg stop_to_obstacle.lua