122 lines
2.6 KiB
Plaintext
122 lines
2.6 KiB
Plaintext
|
|
# Write to matrix
|
|
robot_marker = "X"
|
|
|
|
# copy a full matrix row
|
|
function mat_copyrow(out,ro,in,ri){
|
|
out[ro] = {}
|
|
var icr = 0
|
|
while(icr < size(in[ri])) {
|
|
out[ro][icr]=in[ri][icr]
|
|
icr = icr + 1
|
|
}
|
|
}
|
|
|
|
function getvec(t,row){
|
|
return math.vec2.new(t[row][1],t[row][2])
|
|
}
|
|
|
|
function init_test_map(len){
|
|
map = {}
|
|
var indexR = 0
|
|
while(indexR < len) {
|
|
map[indexR] = {}
|
|
var indexC = 0
|
|
while(indexC < len) {
|
|
map[indexR][indexC] = 1.0
|
|
indexC = indexC + 1
|
|
}
|
|
indexR = indexR + 1
|
|
}
|
|
# DEBUG\TEST: puts an obstacle right in the middle
|
|
map[5][5] = 0.0
|
|
map[6][5] = 0.0
|
|
map[4][5] = 0.0
|
|
|
|
log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
|
|
}
|
|
|
|
function init_map(len){
|
|
map = {}
|
|
var indexR = 0
|
|
while(indexR < len) {
|
|
map[indexR] = {}
|
|
var indexC = 0
|
|
while(indexC < len) {
|
|
map[indexR][indexC] = 1.0
|
|
indexC = indexC + 1
|
|
}
|
|
indexR = indexR + 1
|
|
}
|
|
log("Occupancy grid initialized (",len,"x",len,").")
|
|
}
|
|
|
|
function add_obstacle(pos, off, inc_trust) {
|
|
var xi = math.round(pos.x)
|
|
var yi = math.round(pos.y)
|
|
|
|
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
|
|
#log("Add obstacle in cell: ", xi, " ", yi)
|
|
old=map[xi][yi]
|
|
if(old-inc_trust > 0.0)
|
|
map[xi][yi] = old-inc_trust
|
|
else
|
|
map[xi][yi] = 0.0
|
|
}
|
|
}
|
|
|
|
function remove_obstacle(pos, off, dec_trust) {
|
|
var xi = math.round(pos.x)
|
|
var yi = math.round(pos.y)
|
|
|
|
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){
|
|
#log("Remove obstacle in cell: ", xi, " ", yi)
|
|
old=map[xi][yi]
|
|
if(old + dec_trust < 1.0) #x,y
|
|
map[xi][yi] = old+dec_trust
|
|
else
|
|
map[xi][yi] = 1.0
|
|
}
|
|
}
|
|
|
|
function table_print(t) {
|
|
foreach(t, function(key, value) {
|
|
log(key, " -> ", value)
|
|
})
|
|
}
|
|
|
|
function table_copy(t) {
|
|
var t2 = {}
|
|
foreach(t, function(key, value) {
|
|
t2[key] = value
|
|
})
|
|
return t2
|
|
}
|
|
|
|
function print_pos(t) {
|
|
var ir=1
|
|
while(ir <= size(t)) {
|
|
log(ir, ": ", t[ir][1], " ", t[ir][2])
|
|
ir = ir + 1
|
|
}
|
|
}
|
|
|
|
function print_map(t) {
|
|
var ir=size(t)
|
|
log("Printing a ", size(t), " by ", size(t[1]), " map")
|
|
while(ir > 0) {
|
|
logst=string.concat("\t", string.tostring(ir), "\t:")
|
|
ic = size(t[ir])
|
|
while(ic > 0) {
|
|
if(ir==cur_cell.x and ic==cur_cell.y)
|
|
logst = string.concat(logst, " XXXXXXXX")
|
|
else
|
|
logst = string.concat(logst, " ", string.tostring(t[ir][ic]))
|
|
ic = ic - 1
|
|
}
|
|
log(logst)
|
|
ir = ir - 1
|
|
}
|
|
export_map(map)
|
|
}
|