Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor fix from sitl

This commit is contained in:
dave 2018-09-07 00:56:17 -04:00
parent d79b0f8530
commit 5fc3276ac2
52 changed files with 8126 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
CMakeLists.txt Normal file
View File

@ -0,0 +1,77 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosbuzz)
if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}")
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
mavros_msgs
sensor_msgs
nav_msgs
message_generation
)
##############################
##############################
add_message_files(
FILES
neigh_pos.msg
)
generate_messages(
DEPENDENCIES
sensor_msgs
)
###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
# LIBRARIES xbee_ros_node
CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs message_runtime
# DEPENDS system_lib
)
###########
## Build ##
###########
include_directories(
include ${rosbuzz_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# set the path to the library folder
link_directories(/usr/local/lib)
# Executables
add_executable(rosbuzz_node src/rosbuzz.cpp
src/roscontroller.cpp
src/buzz_utility.cpp
src/buzzuav_closures.cpp
src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
# Executables and libraries for installation to do
install(TARGETS rosbuzz_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Install project namespaced headers
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE)
find_package(catkin REQUIRED COMPONENTS roslaunch)
roslaunch_add_file_check(launch)

0
buzz_scripts/Update.log Normal file
View File

90
buzz_scripts/include/act/barrier.bzz Normal file
View File

@ -0,0 +1,90 @@
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
bc = 0;
#
# Sets a barrier
#
function barrier_create() {
# reset
timeW = 0
# create barrier vstig
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
# BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef, bc) {
statef = function() {
barrier_wait(threshold, transf, resumef, bc);
}
BVMSTATE = "BARRIERWAIT"
barrier_create()
}
#
# Make yourself ready
#
function barrier_ready(bc) {
#log("BARRIER READY -------")
barrier.put(id, bc)
barrier.put("d", 0)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, bc)
barrier.get(id)
var allgood = 0
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
allgood = barrier_allgood(barrier,bc)
}
if(allgood) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier = nil
timeW = 0
BVMSTATE = resumef
} else if(timeW % 100 == 0 and bc > 0)
neighbors.broadcast("cmd", bc)
timeW = timeW+1
}
barriergood = 1
# Barrer check all entries
function barrier_allgood(barrier, bc) {
barriergood = 1
barrier.foreach(
function(key, value, robot){
#log("VS entry : ", key, " ", value, " ", robot)
if(value != bc and key != "d"){
barriergood = 0
}
}
)
return barriergood
}

69
buzz_scripts/include/act/old_barrier.bzz Normal file
View File

@ -0,0 +1,69 @@
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_TIMEOUT = 1200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
#
# Sets a barrier
#
function barrier_create() {
# reset
timeW = 0
# create barrier vstig
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef,bc) {
statef = function() {
barrier_wait(threshold, transf, resumef,bc);
}
BVMSTATE = "BARRIERWAIT"
barrier_create()
}
#
# Make yourself ready
#
function barrier_ready(bc) {
#log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put("d", 0)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef,bc) {
barrier.put(id, 1)
barrier.get(id)
log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
barrier.put("d", 1)
timeW = 0
BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
barrier=nil
timeW = 0
BVMSTATE = resumef
}
timeW = timeW+1
}

311
buzz_scripts/include/act/states.bzz Normal file
View File

@ -0,0 +1,311 @@
########################################
#
# FLIGHT-RELATED FUNCTIONS
#
########################################
include "utils/vec2.bzz"
include "act/barrier.bzz"
#include "act/old_barrier.bzz"
include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 0.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 1.0 # m.
GOTOANG_TOL = 0.1 # rad.
path_it = 0
graphid = 3
pic_time = 0
g_it = 0
function turnedoff() {
BVMSTATE = "TURNEDOFF"
}
function idle() {
BVMSTATE = "IDLE"
}
# Custom function for the user.
function cusfun(){
BVMSTATE="CUSFUN"
log("cusfun: yay!!!")
LOCAL_TARGET = math.vec2.new(5 ,0 )
local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
if(math.vec2.length(local_set_point) > GOTODIST_TOL){
local_set_point = LimitSpeed(local_set_point, 1.0)
goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
}
else{
log("TARGET REACHED")
}
}
function launch() {
BVMSTATE = "LAUNCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22)
} else {
log("Altitude: ", pose.position.altitude)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
} else {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22)
}
}
gohomeT=100
function goinghome() {
BVMSTATE = "GOHOME"
if(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
} else
BVMSTATE = AUTO_LAUNCH_STATE
}
function stop() {
BVMSTATE = "STOP"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21)
uav_land()
if(flight.status != 2 and flight.status != 3) {
BVMSTATE = "TURNEDOFF"
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21)
}
} else {
BVMSTATE = "TURNEDOFF"
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21)
}
}
function take_picture() {
BVMSTATE="PICTURE"
setgimbal(0.0, 0.0, -90.0, 20.0)
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
takepicture()
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
BVMSTATE="IDLE"
pic_time=0
}
pic_time=pic_time+1
}
function goto_gps(transf) {
log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude)
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.")
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf()
else {
m_navigation = LimitSpeed(m_navigation, 1.0)
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
}
}
function follow() {
if(size(targets)>0) {
BVMSTATE = "FOLLOW"
attractor=math.vec2.newp(0,0)
foreach(targets, function(id, tab) {
force=(0.05)*(tab.range)^4
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
})
goto_abs(attractor.x, attractor.y, 0.0, 0.0)
} else {
log("No target in local table!")
BVMSTATE = "IDLE"
}
}
# converge to centroid
function aggregate() {
BVMSTATE="AGGREGATE"
centroid = neighbors.reduce(function(rid, data, centroid) {
centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
return centroid
}, {.x=0, .y=0})
if(neighbors.count() > 0)
centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0)
}
# circle all together around centroid
function pursuit() {
BVMSTATE="PURSUIT"
rd = 20.0
pc = 3.2
vd = 15.0
r_vec = neighbors.reduce(function(rid, data, r_vec) {
r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth))
return r_vec
}, {.x=0, .y=0})
if(neighbors.count() > 0)
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
r = math.vec2.length(r_vec)
var sqr = (r-rd)*(r-rd)+pc*pc*r*r
gamma = vd / math.sqrt(sqr)
dr = -gamma * (r-rd)
dT = gamma * pc
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
vfg = LimitSpeed(vfg, 2.0)
goto_abs(vfg.x, vfg.y, 0.0, 0.0)
}
# Lennard-Jones interaction magnitude
TARGET = 8.0
EPSILON = 30.0
function lj_magnitude(dist, target, epsilon) {
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return lj
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function formation() {
BVMSTATE="FORMATION"
# Calculate accumulator
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}
# listens to commands from the remote control (web, commandline, etc)
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "TURNEDOFF"
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
#barrier_ready(21)
BVMSTATE = "STOP"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "IDLE"
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
barrier_ready(20)
neighbors.broadcast("cmd", 20)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
BVMSTATE = "PATHPLAN"
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
disarm()
neighbors.broadcast("cmd", 401)
} else if (flight.rc_cmd==666){
flight.rc_cmd=0
stattab_send()
} else if (flight.rc_cmd==777){
flight.rc_cmd=0
reinit_time_sync()
neighbors.broadcast("cmd", 777)
}else if (flight.rc_cmd==900){
flight.rc_cmd=0
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
}
}
# listens to other fleet members broadcasting commands
function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(BVMSTATE!="BARRIERWAIT") {
if(value==22) {
BVMSTATE = "LAUNCH"
}else if(value==20) {
AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME"
} else if(value==21) {
AUTO_LAUNCH_STATE = "TURNEDOFF"
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
disarm()
} else if(value==777 and BVMSTATE=="TURNEDOFF"){
reinit_time_sync()
#neighbors.broadcast("cmd", 777)
}else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if(value==901){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
#barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if(value==902){ # Agreggate
destroyGraph()
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
#barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if(value==903){ # Formation
destroyGraph()
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
#barrier_ready(903)
neighbors.broadcast("cmd", 903)
} else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
}
}
})
}

145
buzz_scripts/include/plan/mapmatrix.bzz Normal file
View File

@ -0,0 +1,145 @@
# Write to matrix
robot_marker = "X"
# copy a full matrix row
function mat_copyrow(out,ro,in,ri){
out[ro] = {}
var icr = 1
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 = 1
while(indexR <= len) {
map[indexR] = {}
var indexC = 1
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 (",size(map),"x",size(map[1]),") with obstacles.")
}
function init_map(len){
map = {}
var indexR = 1
while(indexR <= len) {
map[indexR] = {}
var indexC = 1
while(indexC <= len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
indexR = indexR + 1
}
log("Occupancy grid initialized (",size(map),"x",size(map[1]),").")
}
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)
var 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)
var 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 get_occupied_cells(cur_cell){
var i = 1
var occupied_cells = {}
occupied_cells[0] = cur_cell
occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y)
occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y)
occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1)
occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1)
return occupied_cells
}
function is_in(dictionary, x, y){
var i = 0
while(i < size(dictionary)){
if(dictionary[i].x == x and dictionary[i].y == y){
return 1
}
i = i + 1
}
return 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)
}

525
buzz_scripts/include/plan/rrtstar.bzz Normal file
View File

@ -0,0 +1,525 @@
#####
# RRT* Path Planing
#
# map table-based matrix
#####
include "plan/mapmatrix.bzz"
RRT_TIMEOUT = 500
RRT_RUNSTEP = 3
PROX_SENSOR_THRESHOLD_M = 8.0
GSCALE = {.x=1, .y=1}
map = nil
cur_cell = {}
nei_cell = {}
mapgoal = {}
#
# Work in progress....
function navigate() {
BVMSTATE="NAVIGATE"
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
}
print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
log("Sorry this is too far.")
return
}
# create the map
if(map==nil) {
dyn_init_map(cur_goal)
if(V_TYPE == 0) {
homegps.lat = pose.position.latitude
homegps.long = pose.position.longitude
}
}
if(Path==nil) {
# add proximity sensor and neighbor obstacles to the map
populateGrid(m_pos)
# start the planner
path_it = 1
pathPlanner(cur_goal, m_pos)
} else if(path_it <= size(Path)) {
var cur_path_pt = convert_pt(getvec(Path, path_it))
print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
populateGrid(m_pos)
if(check_path(Path, path_it, m_pos, 0)) {
# stop
goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0)
Path = nil
if(V_TYPE == 0)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
# re-start the planner
path_it = 1
pathPlanner(cur_goal, m_pos)
} else {
cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0)
}
} else
path_it = path_it + 1
} else {
Path = nil
BVMSTATE="IDLE"
}
}
# create a map with its length to fit the goal (rel. pos.) and the current position as the center
# TODO: test with khepera/argos
function dyn_init_map(m_navigation) {
# create a map big enough for the goal
init_map(math.round(2*math.vec2.length(m_navigation))+10)
# center the robot on the grid
cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
}
# start the RRT* to reach the goal (rel. pos.)
# TODO: test with khepera/argos
function pathPlanner(m_goal, m_pos, kh4) {
# place the robot on the grid
cur_cell = getcell(m_pos)
# create the goal in the map grid
mapgoal = getcell(math.vec2.add(m_goal, m_pos))
#print_map(map)
export_map(map)
# Initialize RRTStar var
log("--------Initialize RRTStar--------------")
HEIGHT = size(map)
WIDTH = size(map[1])
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0}
#table_print(goalBoundary)
numberOfPoints = 1
arrayOfPoints = {}
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
Q = {}
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0}
goalReached = 0
timeout = 0
# search for a path
old_statef = statef
rrtstar()
}
# get the grid cell position (x,y) equivalent to the input position
# input should be relative to home position (planing start point)
function getcell(m_curpos) {
var cell = {}
# relative to center (start position)
cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y)
cell = math.vec2.add(cell, offset)
cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
if(cell.x > size(map))
cell.x = size(map)
if(cell.y > size(map[1]))
cell.y = size(map[1])
if(cell.x < 1)
cell.x = 1
if(cell.y < 1)
cell.y = 1
return cell
}
function populateGrid(m_pos) {
# getproxobs(m_pos)
getneiobs (m_pos)
export_map(map)
}
# TODO: populate the map with neighors as blobs instead ?
function getneiobs (m_curpos) {
cur_cell = getcell(m_curpos)
# add all neighbors as obstacles in the grid
neighbors.foreach(function(rid, data) {
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
})
}
# old function tested with the khepera for reference
# DS 20/11
# function getneiobs (m_curpos) {
# var foundobstacle = 0
# cur_cell = getcell(m_curpos)
# var old_nei = table_copy(nei_cell)
# neighbors.foreach(function(rid, data) {
# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
# if(old_nei[rid]!=nil) {
# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
# add_obstacle(Ncell, 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
# foundobstacle = 1
# }
# } else {
# add_obstacle(Ncell, 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
# foundobstacle = 1
# }
# })
# return foundobstacle
# }
# populate a line in front of the sensor using plateform independant proximity sensing
# TODO: adapt to M100 stereo
function getproxobs (m_curpos) {
var updated_obstacle = 0
cur_cell = getcell(m_curpos)
reduce(proximity,
function(key, value, acc) {
if (value.angle != -1) { # down sensor of M100
if(value.value < PROX_SENSOR_THRESHOLD_M) {
obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw)))
per = math.vec2.sub(obs,cur_cell)
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
# obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs)))
# obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
# obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
add_obstacle(obs, 0, 0.25)
add_obstacle(obsr, 0, 0.25)
add_obstacle(obsl, 0, 0.25)
add_obstacle(obsr2, 0, 0.25)
add_obstacle(obsl2, 0, 0.25)
# add_obstacle(obs2, 0, 0.25)
# add_obstacle(obsr2, 0, 0.25)
# add_obstacle(obsl2, 0, 0.25)
updated_obstacle = 1
}
} else {
var line_length = PROX_SENSOR_THRESHOLD_M;
while(line_length > 0) {
obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw)))
per = math.vec2.sub(obs,cur_cell)
obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs)
obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs)
remove_obstacle(obs, 0, 0.05)
remove_obstacle(obsr, 0, 0.05)
remove_obstacle(obsl, 0, 0.05)
line_length = line_length - 1;
}
}
}
return acc
}, math.vec2.new(0, 0))
return updated_obstacle
}
# check if any obstacle blocks the way
# TODO: remove the kh4 bool for retro-compatilibty
function check_path(m_path, goal_l, m_curpos, kh4) {
var pathisblocked = 0
var nb = goal_l
cur_cell = getcell(m_curpos)
var Cvec = cur_cell
while(nb <= size(m_path) and nb <= goal_l+1) {
var Nvec = getvec(m_path, nb)
if(kh4 == 0) {
Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
Nvec = getcell(Nvec)
}
if(doesItIntersect(Cvec, Nvec)) {
log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
pathisblocked = 1
}
Cvec = Nvec
nb = nb + 1
}
return pathisblocked
}
##
# main search loop as a state
##
function rrtstar() {
# update state machine variables
BVMSTATE = "PATHPLAN"
step_timeout = 0
while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) {
# Point generation only as grid cell centers
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
#log("Point generated: ", pt.x, " ", pt.y)
var pointList = findPointsInRadius(pt,Q,RADIUS);
# Find connection that provides the least cost to come
nbCount = 0;
minCounted = 999;
minCounter = 0;
if(size(pointList) != 0) {
#log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3])
ipt=1
while(ipt <= size(pointList)) {
pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(pointNumber,1));
#log("intersects1: ", intersects)
# If there is no intersection we need consider its connection
nbCount = nbCount + 1;
if(intersects != 1) {
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5]
if(distance < minCounted) {
minCounted = distance;
minCounter = nbCount;
}
}
ipt = ipt + 1
}
if(minCounter > 0) {
numberOfPoints = numberOfPoints + 1;
arrayOfPoints[numberOfPoints] = {}
arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y
Q[numberOfPoints] = {}
Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointList[minCounter][4]
Q[numberOfPoints][4] = numberOfPoints
Q[numberOfPoints][5] = minCounted
#log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)
# Now check to see if any of the other points can be redirected
nbCount = 0;
ipt = 1
while(ipt <= size(pointList)) {
pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt)
#log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4])
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(pointNumber,1));
#log("intersects2: ", intersects)
# If there is no intersection we need consider its connection
nbCount = nbCount + 1;
if(intersects != 1) {
# If the alternative path is shorter than change it
tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
#log("Q: ", size(Q), "x", size(Q[1]))
if(tmpdistance < Q[pointNumber[1][4]][5]) {
Q[pointNumber[1][4]][3] = numberOfPoints
Q[pointNumber[1][4]][5] = tmpdistance
}
}
ipt = ipt + 1
}
# Check to see if this new point is within the goal
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
goalReached = 1;
}
} else {
# Associate with the closest point
pointNum = findClosestPoint(pt,arrayOfPoints);
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum));
#log("intersects3 (", pointNum, "): ", intersects)
# If there is no intersection we need to add to the tree
if(intersects != 1) {
numberOfPoints = numberOfPoints + 1;
arrayOfPoints[numberOfPoints] = {}
arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y
Q[numberOfPoints] = {}
Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointNum
Q[numberOfPoints][4] = numberOfPoints
Q[numberOfPoints][5] = Q[pointNum][5]+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))
#log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)
# Check to see if this new point is within the goal
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
goalReached = 1;
}
}
if(numberOfPoints % 100 == 0) {
log(numberOfPoints, " points processed. Still looking for goal.");
}
timeout = timeout + 1
step_timeout = step_timeout + 1
}
if(goalReached){
log("Goal found(",numberOfPoints,")!")
Path = convert_path(getPath(Q,numberOfPoints))
# done. resume the state machine
BVMSTATE = "NAVIGATE"
} else if(timeout >= RRT_TIMEOUT) {
log("FAILED TO FIND A PATH!!!")
Path = nil
# done. resume the state machine
BVMSTATE = "NAVIGATE"
}
}
# Go through each points and find the distances between them and the
# target point
function findClosestPoint(point,aPt) {
var distance = 999
var pointNb = -1
var ifcp=1
while(ifcp <= size(aPt)) {
var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
if(range < distance) {
distance = range;
pointNb = ifcp;
}
ifcp = ifcp + 1
}
return pointNb
}
# Find the closest point in the tree
function findPointsInRadius(point,q,r) {
var ptList = {}
var counted = 0;
var iir = 1
while(iir <= size(q)) {
var tmpvv = getvec(q,iir)
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
if(distance < r) {
counted = counted+1;
mat_copyrow(ptList,counted,q,iir)
}
iir = iir + 1
}
return ptList
}
# check if the line between 2 point intersect an obstacle
function doesItIntersect(point,vector) {
#log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
var dif = math.vec2.sub(point,vector)
var distance = math.vec2.length(dif)
if(distance == 0.0){
# Find what block we're in right now
var xi = math.round(point.x) #+1
var yi = math.round(point.y) #+1
if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) {
if(map[xi][yi] > 0.5)
return 1
else
return 0
} else
return 0
}
var vec = math.vec2.scale(dif,1.0/distance)
var pathstep = size(map) - 2
idii = 1.0
while(idii <= pathstep) {
var range = distance*idii/pathstep
var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
# Find what block we're in right now
var xi = math.round(pos_chk.x) #+1
var yi = math.round(pos_chk.y) #+1
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")
# TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross
if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){
#if(xi!=cur_cell.x and yi!=cur_cell.y) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
if(map[xi][yi] < 0.5) { # because obstacle use trust values
#log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
return 1;
}
} else {
#log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y)
return 1;
}
}
idii = idii + 1.0
}
#log("No intersect!")
return 0
}
# create a table with only the path's points in order
function getPath(Q,nb){
var pathL={}
var npt=0
# get the path from the point list
while(nb > 0) {
npt=npt+1
pathL[npt] = {}
pathL[npt][1]=Q[nb][1]
pathL[npt][2]=Q[nb][2]
if( nb > 1) {
if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening?
log("ERROR - Recursive path !!!")
return nil
}
}
nb = Q[nb][3];
}
# Re-Order the list.
var pathR={}
var totpt = npt + 1
while(npt > 0){
pathR[totpt-npt] = {}
var tmpgoal = getvec(pathL,npt)
pathR[totpt-npt][1]=tmpgoal.x
pathR[totpt-npt][2]=tmpgoal.y
npt = npt - 1
}
#log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
return pathR
}

798
buzz_scripts/include/taskallocate/graphformGPS.bzz Normal file
View File

@ -0,0 +1,798 @@
#
# Include files
#
include "taskallocate/graphs/shapes_P.bzz"
include "taskallocate/graphs/shapes_O.bzz"
include "taskallocate/graphs/shapes_L.bzz"
include "taskallocate/graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
#
# Global variables
#
#
# Save message from all neighours
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
#
#Save the message to send
#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#navigation vector
m_navigation={.x=0,.y=0}
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
m_messageID={}
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
#neighbor distance to lock the current pattern
lock_neighbor_id={}
lock_neighbor_dis={}
#Label request id
m_unRequestId=0
#Global bias, used to map local coordinate to global coordinate
m_bias=0
#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
#Counter to wait for something to happen
m_unWaitCount=0
#Number of steps to wait before looking for a free label
m_unLabelSearchWaitTime=0
#Number of steps to wait for an answer to be received
m_unResponseTimeThreshold=0
#Number of steps to wait until giving up joining
m_unJoiningLostPeriod=0
#Tolerance distance to a target location
m_fTargetDistanceTolerance=0
# virtual stigmergy for the LOCK barrier.
m_lockstig = 1
# Lennard-Jones parameters, may need change
EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#
#map from int to state
#
function i2s(value){
if(value==1){
return "GRAPH_FREE"
}
else if(value==2){
return "GRAPH_ASKING"
}
else if(value==3){
return "GRAPH_JOINING"
}
else if(value==4){
return "GRAPH_JOINED"
}
else if(value==5){
return "GRAPH_LOCK"
}
}
#
#map from state to int
#
function s2i(value){
if(value=="GRAPH_FREE"){
return 1
}
else if(value=="GRAPH_ASKING"){
return 2
}
else if(value=="GRAPH_JOINING"){
return 3
}
else if(value=="GRAPH_JOINED"){
return 4
}
else if(value=="GRAPH_LOCK"){
return 5
}
}
#
#map form int to response
#
function i2r(value){
if(value==1){
return "REQ_NONE"
}
else if(value==2){
return "REQ_GRANTED"
}
}
#
#map from response to int
#
function r2i(value){
if(value=="REQ_NONE"){
return 1
}
else if(value=="REQ_GRANTED"){
return 2
}
}
#
#return the index of value
#
function find(table,value){
var ind=nil
var i=0
while(i<size(table)){
if(table[i]==value)
ind=i
i=i+1
}
return ind
}
#
#pack message into 1 number
#
function packmessage(send_table){
var send_value
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
return send_value
}
#
#pack guide message into 1 number
#
function pack_guide_msg(send_table){
var send_value
var r_id=send_table.Label#id of target robot
var pon#positive or negative ,0 postive, 1 negative
if(send_table.Bearing>=0){
pon=0
}
else{
pon=1
}
var b=math.abs(send_table.Bearing)
send_value=r_id*1000+pon*100+b
return send_value
}
#
#unpack message
#
function unpackmessage(recv_value){
var wan=(recv_value-recv_value%100000)/100000
recv_value=recv_value-wan*100000
var qian=(recv_value-recv_value%10000)/10000
recv_value=recv_value-qian*10000
var bai=(recv_value-recv_value%1000)/1000
recv_value=recv_value-bai*1000
var shi=(recv_value-recv_value%10)/10
recv_value=recv_value-shi*10
var ge=recv_value
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
return_table.State=wan
return_table.Label=qian
return_table.ReqLabel=bai
return_table.ReqID=shi
return_table.Response=ge
return return_table
}
#
#unpack guide message
#
function unpack_guide_msg(recv_value){
#log(id,"I pass value=",recv_value)
var qian=(recv_value-recv_value%1000)/1000
recv_value=recv_value-qian*1000
var bai=(recv_value-recv_value%100)/100
recv_value=recv_value-bai*100
var b=recv_value
var return_table={.Label=0.0,.Bearing=0.0}
return_table.Label=qian
if(bai==1){
b=b*-1.0
}
return_table.Bearing=b
return return_table
}
#
#get the target distance to neighbr nei_id
#
function target4label(nei_id){
var return_val="miss"
var i=0
while(i<size(lock_neighbor_id)){
if(lock_neighbor_id[i]==nei_id){
return_val=lock_neighbor_dis[i]
}
i=i+1
}
return return_val
}
#
#calculate LJ vector for neibhor stored in i
#
function LJ_vec(i){
var dis=m_MessageRange[i]
var ljbearing=m_MessageBearing[i]
var nei_id=m_messageID[i]
var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0}
if(target!="miss"){
cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing)
}
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y)
return cDir
}
#
#calculate the motion vector
#
function motion_vector(){
var i=0
var m_vector={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
#calculate and add the motion vector
m_vector=math.vec2.add(m_vector,LJ_vec(i))
#log(id,"x=",m_vector.x,"y=",m_vector.y)
i=i+1
}
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
return m_vector
}
#
# starts the neighbors listener
#
function start_listen(){
neighbors.listen("m",
function(vid,value,rid){
#store the received message
var temp_id=rid
var recv_val=unpackmessage(value)
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
m_MessageLabel[m_neighbourCount]=recv_val.Label
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid
#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
m_neighbourCount=m_neighbourCount+1
})
}
#
#Function used to get the station info of the sender of the message
#
function Get_DisAndAzi(id){
neighbors.foreach(
function(rid, data) {
if(rid==id){
m_receivedMessage.Range=data.distance*100.0
m_receivedMessage.Bearing=data.azimuth
}
})
}
#
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="GRAPH_JOINED"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="GRAPH_JOINING"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
i=i+1
}
#Forget old information
i=0
while(i<size(m_vecNodes)){
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
if(m_vecNodes[i].StateAge==0)
m_vecNodes[i].State="UNASSIGNED"
}
i=i+1
}
}
#
#Transistion to state free
#
function TransitionToFree(){
BVMSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(BVMSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
BVMSTATE="GRAPH_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
}
#
#Transistion to state joined
#
function TransitionToJoined(){
BVMSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create()
barrier_ready(900+50)
m_navigation.x=0.0
m_navigation.y=0.0
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
BVMSTATE="GRAPH_LOCK"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]
i=i+1
}
m_navigation.x=0.0
m_navigation.y=0.0
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
#stop listening
neighbors.ignore("m")
}
#
# Do free
#
function DoFree() {
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLabels={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="GRAPH_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper Label
var unFoundLabel=0
# var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLabel==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
unFoundLabel=m_vecNodes[i].Label
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
}
i=i+1
}
if(unFoundLabel>0){
TransitionToAsking(unFoundLabel)
return
}
#set message
m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph()
}
#
#Do asking
#
function DoAsking(){
UpdateGraph()
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCount and psResponse==-1){
#the respond robot in joined state
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="GRAPH_JOINED"){
#log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree()
return
}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){
#TransitionToFree()
#return
#}
}
else{
log("respond id=",m_MessageReqID[psResponse])
if(m_MessageReqID[psResponse]!=m_unRequestId){
m_vecNodes[m_nLabel].State="ASSIGNING"
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
TransitionToFree()
}
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
BroadcastGraph()
}
#
#Do joining
#
function DoJoining(){
UpdateGraph()
if(m_gotjoinedparent!=1)
set_rc_goto()
else
goto_gps(TransitionToJoined)
#pack the communication package
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
BroadcastGraph()
}
function set_rc_goto() {
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="GRAPH_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
var S2Target=math.vec2.add(S2Pred,P2Target)
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude)
storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1
}
}
#
#Do joined
#
function DoJoined(){
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLabel
var JoiningLabel
var seenPred=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="GRAPH_ASKING"){
ReqLabel=m_MessageReqLabel[i]
#log("ReqLabel var:",ReqLabel)
#log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="GRAPH_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
}
}
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0
}
#if it is the pred
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
}
i=i+1
}
#get request
if(size(mapRequests)!=0){
i=1
var ReqIndex=0
while(i<size(mapRequests)){
#compare the distance
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
ReqIndex=i
i=i+1
}
if(repeat_assign==0){
#get the best index, whose ReqLabel and Reqid are
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
assign_label=ReqLabel
assign_id=ReqID
repeat_assign=1
}
m_selfMessage.ReqLabel=assign_label
m_selfMessage.ReqID=assign_id
m_selfMessage.Response=r2i("REQ_GRANTED")
#m_vecNodes[ReqLabel].State="ASSIGNING"
log("Label=",assign_label)
log("ID=",assign_id)
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
}
#lost pred, wait for some time and transit to free
if(seenPred==0){
m_unWaitCount=m_unWaitCount-1
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941)
BroadcastGraph()
}
#
#Do Lock
#
timeout_graph = 40
function DoLock() {
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0 #change value so that robot 0 will move
m_navigation.y=0.0
}
if(m_nLabel!=0){
m_navigation=motion_vector()
}
# #move
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
BroadcastGraph()
# if(loop) {
# if(timeout_graph==0) {
# if(graphid < 3)
# graphid = graphid + 1
# else
# graphid = 0
# timeout_graph = 40
# BVMSTATE="TASK_ALLOCATE"
# }
# timeout_graph = timeout_graph - 1
# }
}
#
# Executed at init
#
function initGraph() {
#
# Global parameters for graph formation
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
}
#
# Executed every step (main loop)
#
function UpdateGraph() {
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
}
function BroadcastGraph() {
#navigation
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLabel={}#store received neighbour message
m_MessageReqLabel={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCount=0
}
#
# Executed when reset
#
function resetGraph(){
BVMSTATE="GRAPH_FREE"
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_navigation={.x=0,.y=0}
m_nLabel=-1
m_messageID={}
lock_neighbor_id={}
lock_neighbor_dis={}
m_unRequestId=0
m_bias=0
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
m_unWaitCount=0
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
if(graphid==0){
log("Loading P graph")
Read_GraphP()
} else if(graphid==1) {
log("Loading O graph")
Read_GraphO()
} else if(graphid==2) {
log("Loading L graph")
Read_GraphL()
} else if(graphid==3) {
log("Loading Y graph")
Read_GraphY()
}
#start listening
start_listen()
#
#set initial state, only one robot choose [A], while the rest choose [B]
#
#[A]The robot used to triger the formation process is defined as joined,
if(id==ROOT_ID){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroyGraph() {
#clear neighbour message
m_navigation.x=0.0
m_navigation.y=0.0
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")
}

6
buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph Normal file
View File

@ -0,0 +1,6 @@
0 -1 -1 -1 3000.0
1 0 200.0 0.0 5000.0
2 0 200.0 1.57 7000.0
3 1 200.0 0.0 9000.0
4 2 200.0 1.57 11000.0
5 4 200.0 1.57 11000.0

6
buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph Normal file
View File

@ -0,0 +1,6 @@
0 -1 -1 -1 3000.0
1 0 200.0 0.0 5000.0
2 0 200.0 1.57 7000.0
3 1 200.0 1.57 9000.0
4 1 141.0 3.93 11000.0
5 2 141.0 0.785 11000.0

6
buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph Normal file
View File

@ -0,0 +1,6 @@
0 -1 -1 -1 3000.0
1 0 200.0 0.0 5000.0
2 0 200.0 1.57 7000.0
3 0 200.0 4.71 9000.0
4 1 141.0 0.79 11000.0
5 2 200.0 0.0 11000.0

6
buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph Normal file
View File

@ -0,0 +1,6 @@
0 -1 -1 -1 3000.0
1 0 200.0 4.71 5000.0
2 0 141.0 2.36 7000.0
3 0 141.0 0.79 9000.0
4 2 141.0 2.36 11000.0
5 3 141.0 0.79 11000.0

BIN
buzz_scripts/include/taskallocate/graphs/images/frame_01186.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 67 KiB

BIN
buzz_scripts/include/taskallocate/graphs/images/frame_01207.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 77 KiB

BIN
buzz_scripts/include/taskallocate/graphs/images/frame_01394.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 75 KiB

BIN
buzz_scripts/include/taskallocate/graphs/images/frame_01424.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 96 KiB

59
buzz_scripts/include/taskallocate/graphs/shapes_L.bzz Normal file
View File

@ -0,0 +1,59 @@
#Table of the nodes in the graph
m_vecNodes={}
function Read_GraphL(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Label = 1,
.Pred = 0,
.distance = 800,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Label = 2,
.Pred = 0,
.distance = 800,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Label = 3,
.Pred = 1,
.distance = 800,
.bearing = 0.0,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Label = 4,
.Pred = 2,
.distance = 800,
.bearing = 1.57,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[5] = {
.Label = 5,
.Pred = 4,
.distance = 800,
.bearing = 1.57,
.height = 14000,
.State="UNASSIGNED",
.StateAge=0
}
}

59
buzz_scripts/include/taskallocate/graphs/shapes_O.bzz Normal file
View File

@ -0,0 +1,59 @@
#Table of the nodes in the graph
m_vecNodes={}
function Read_GraphO(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Label = 1,
.Pred = 0,
.distance = 2000,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Label = 2,
.Pred = 0,
.distance = 2000,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Label = 3,
.Pred = 1,
.distance = 2000,
.bearing = 1.57,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Label = 4,
.Pred = 1,
.distance = 1414,
.bearing = 3.93,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[5] = {
.Label = 5,
.Pred = 2,
.distance = 1414,
.bearing = 0.785,
.height = 14000,
.State="UNASSIGNED",
.StateAge=0
}
}

59
buzz_scripts/include/taskallocate/graphs/shapes_P.bzz Normal file
View File

@ -0,0 +1,59 @@
#Table of the nodes in the graph
m_vecNodes={}
function Read_GraphP(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Label = 1,
.Pred = 0,
.distance = 1000,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Label = 2,
.Pred = 0,
.distance = 1000,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Label = 3,
.Pred = 0,
.distance = 1000,
.bearing = 4.71,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Label = 4,
.Pred = 1,
.distance = 707,
.bearing = 0.79,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[5] = {
.Label = 5,
.Pred = 2,
.distance = 1000,
.bearing = 0.0,
.height = 14000,
.State="UNASSIGNED",
.StateAge=0
}
}

59
buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz Normal file
View File

@ -0,0 +1,59 @@
#Table of the nodes in the graph
m_vecNodes={}
function Read_GraphY(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Label = 1,
.Pred = 0,
.distance = 1000,
.bearing = 4.71,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Label = 2,
.Pred = 0,
.distance = 707,
.bearing = 2.36,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Label = 3,
.Pred = 0,
.distance = 707,
.bearing = 0.79,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Label = 4,
.Pred = 2,
.distance = 707,
.bearing = 2.36,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[5] = {
.Label = 5,
.Pred = 3,
.distance = 707,
.bearing = 0.79,
.height = 14000,
.State="UNASSIGNED",
.StateAge=0
}
}

60
buzz_scripts/include/taskallocate/graphs/shapes_square.bzz Normal file
View File

@ -0,0 +1,60 @@
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
function Read_Graph(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point
.Pred = -1, # Label of its predecessor
.distance = -1, # distance to the predecessor
.bearing = -1, # bearing form the predecessor to this dot
.height = 3000, # height of this dot
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[1] = {
.Label = 1,
.Pred = 0,
.distance = 1000,
.bearing = 0.0,
.height = 5000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[2] = {
.Label = 2,
.Pred = 0,
.distance = 1000,
.bearing = 1.57,
.height = 7000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[3] = {
.Label = 3,
.Pred = 0,
.distance = 1000,
.bearing = 3.14,
.height = 9000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[4] = {
.Label = 4,
.Pred = 0,
.distance = 707,
.bearing = 3.93,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
m_vecNodes[5] = {
.Label = 5,
.Pred = 0,
.distance = 1000,
.bearing = 4.71,
.height = 11000,
.State="UNASSIGNED",
.StateAge=0
}
}

89
buzz_scripts/include/taskallocate/waypointlist.csv Normal file
View File

@ -0,0 +1,89 @@
0,-73.1531935978886,45.5084960903092,15,15
1,-73.1530989420915,45.5085624255498,15,15
2,-73.1530042862771,45.5086287608025,15,15
3,-73.1529096304626,45.5086950960552,15,15
4,-73.1529458247399,45.5087204611798,15,15
5,-73.1530404805543,45.5086541259271,15,15
6,-73.1531351363515,45.5085877906865,15,15
7,-73.1532297921486,45.508521455446,15,15
8,-73.1533244479457,45.5084551202054,15,15
9,-73.1533606422273,45.508480485333,15,15
10,-73.1532659864302,45.5085468205736,15,15
11,-73.153171330633,45.5086131558142,15,15
12,-73.1530766748359,45.5086794910548,15,15
13,-73.1529820190215,45.5087458263075,15,15
14,-73.1530182132901,45.5087711914261,15,15
15,-73.1531128691045,45.5087048561733,15,15
16,-73.1532075249016,45.5086385209327,15,15
17,-73.1533021806988,45.5085721856922,15,15
18,-73.1533968364959,45.5085058504516,15,15
19,-73.1534330307645,45.5085312155701,15,15
20,-73.1533383749674,45.5085975508107,15,15
21,-73.1532437191702,45.5086638860513,15,15
22,-73.1531490633731,45.5087302212919,15,15
23,-73.1530544075587,45.5087965565446,15,15
24,-73.1530906018273,45.5088219216632,15,15
25,-73.1531852576417,45.5087555864105,15,15
26,-73.1532799134389,45.5086892511699,15,15
27,-73.153374569236,45.5086229159293,15,15
28,-73.1534692250331,45.5085565806887,15,15
29,-73.1535054193017,45.5085819458072,15,15
30,-73.1534107635046,45.5086482810478,15,15
31,-73.1533161077075,45.5087146162884,15,15
32,-73.1532214519103,45.508780951529,15,15
33,-73.1531267960959,45.5088472867817,15,15
34,-73.1531629903645,45.5088726519003,15,15
35,-73.1532576461789,45.5088063166476,15,15
36,-73.1533523019761,45.508739981407,15,15
37,-73.1534469577732,45.5086736461664,15,15
38,-73.1535416135703,45.5086073109258,15,15
39,-73.1535778078389,45.5086326760444,15,15
40,-73.1534831520418,45.5086990112849,15,15
41,-73.1533884962447,45.5087653465255,15,15
42,-73.1532938404476,45.5088316817661,15,15
43,-73.1531991846331,45.5088980170188,15,15
44,-73.1532353789017,45.5089233821374,15,15
45,-73.1533300347162,45.5088570468847,15,15
46,-73.1534246905133,45.5087907116441,15,15
47,-73.1535193463104,45.5087243764035,15,15
48,-73.1536140021075,45.5086580411629,15,15
49,-73.1536501963762,45.5086834062815,15,15
50,-73.153555540579,45.508749741522,15,15
51,-73.1534608847819,45.5088160767626,15,15
52,-73.1533662289848,45.5088824120032,15,15
53,-73.1532715731703,45.508948747256,15,15
54,-73.1533077674389,45.5089741123745,15,15
55,-73.1534024232534,45.5089077771218,15,15
56,-73.1534970790505,45.5088414418812,15,15
57,-73.1535917348476,45.5087751066406,15,15
58,-73.1536863906448,45.5087087714,15,15
59,-73.1537225849134,45.5087341365186,15,15
60,-73.1536279291163,45.5088004717592,15,15
61,-73.1535332733191,45.5088668069997,15,15
62,-73.153438617522,45.5089331422403,15,15
63,-73.1533439617076,45.5089994774931,15,15
64,-73.1533801559762,45.5090248426116,15,15
65,-73.1534748117906,45.5089585073589,15,15
66,-73.1535694675877,45.5088921721183,15,15
67,-73.1536641233849,45.5088258368777,15,15
68,-73.153758779182,45.5087595016371,15,15
69,-73.1537949734506,45.5087848667557,15,15
70,-73.1537003176535,45.5088512019963,15,15
71,-73.1536056618563,45.5089175372369,15,15
72,-73.1535110060592,45.5089838724775,15,15
73,-73.1534163502448,45.5090502077302,15,15
74,-73.1534525445134,45.5090755728487,15,15
75,-73.1535472003278,45.509009237596,15,15
76,-73.1536418561249,45.5089429023554,15,15
77,-73.1537365119221,45.5088765671148,15,15
78,-73.1538311677192,45.5088102318742,15,15
79,-73.1538673619878,45.5088355969928,15,15
80,-73.1537727061907,45.5089019322334,15,15
81,-73.1536780503936,45.508968267474,15,15
82,-73.1535833945964,45.5090346027146,15,15
83,-73.153488738782,45.5091009379673,15,15
84,-73.1535249330852,45.5091263031101,15,15
85,-73.1536195888996,45.5090599678574,15,15
86,-73.1537142446968,45.5089936326168,15,15
87,-73.1538089004939,45.5089272973762,15,15
88,-73.153903556291,45.5088609621356,15,15
1 0 -73.1531935978886 45.5084960903092 15 15
2 1 -73.1530989420915 45.5085624255498 15 15
3 2 -73.1530042862771 45.5086287608025 15 15
4 3 -73.1529096304626 45.5086950960552 15 15
5 4 -73.1529458247399 45.5087204611798 15 15
6 5 -73.1530404805543 45.5086541259271 15 15
7 6 -73.1531351363515 45.5085877906865 15 15
8 7 -73.1532297921486 45.508521455446 15 15
9 8 -73.1533244479457 45.5084551202054 15 15
10 9 -73.1533606422273 45.508480485333 15 15
11 10 -73.1532659864302 45.5085468205736 15 15
12 11 -73.153171330633 45.5086131558142 15 15
13 12 -73.1530766748359 45.5086794910548 15 15
14 13 -73.1529820190215 45.5087458263075 15 15
15 14 -73.1530182132901 45.5087711914261 15 15
16 15 -73.1531128691045 45.5087048561733 15 15
17 16 -73.1532075249016 45.5086385209327 15 15
18 17 -73.1533021806988 45.5085721856922 15 15
19 18 -73.1533968364959 45.5085058504516 15 15
20 19 -73.1534330307645 45.5085312155701 15 15
21 20 -73.1533383749674 45.5085975508107 15 15
22 21 -73.1532437191702 45.5086638860513 15 15
23 22 -73.1531490633731 45.5087302212919 15 15
24 23 -73.1530544075587 45.5087965565446 15 15
25 24 -73.1530906018273 45.5088219216632 15 15
26 25 -73.1531852576417 45.5087555864105 15 15
27 26 -73.1532799134389 45.5086892511699 15 15
28 27 -73.153374569236 45.5086229159293 15 15
29 28 -73.1534692250331 45.5085565806887 15 15
30 29 -73.1535054193017 45.5085819458072 15 15
31 30 -73.1534107635046 45.5086482810478 15 15
32 31 -73.1533161077075 45.5087146162884 15 15
33 32 -73.1532214519103 45.508780951529 15 15
34 33 -73.1531267960959 45.5088472867817 15 15
35 34 -73.1531629903645 45.5088726519003 15 15
36 35 -73.1532576461789 45.5088063166476 15 15
37 36 -73.1533523019761 45.508739981407 15 15
38 37 -73.1534469577732 45.5086736461664 15 15
39 38 -73.1535416135703 45.5086073109258 15 15
40 39 -73.1535778078389 45.5086326760444 15 15
41 40 -73.1534831520418 45.5086990112849 15 15
42 41 -73.1533884962447 45.5087653465255 15 15
43 42 -73.1532938404476 45.5088316817661 15 15
44 43 -73.1531991846331 45.5088980170188 15 15
45 44 -73.1532353789017 45.5089233821374 15 15
46 45 -73.1533300347162 45.5088570468847 15 15
47 46 -73.1534246905133 45.5087907116441 15 15
48 47 -73.1535193463104 45.5087243764035 15 15
49 48 -73.1536140021075 45.5086580411629 15 15
50 49 -73.1536501963762 45.5086834062815 15 15
51 50 -73.153555540579 45.508749741522 15 15
52 51 -73.1534608847819 45.5088160767626 15 15
53 52 -73.1533662289848 45.5088824120032 15 15
54 53 -73.1532715731703 45.508948747256 15 15
55 54 -73.1533077674389 45.5089741123745 15 15
56 55 -73.1534024232534 45.5089077771218 15 15
57 56 -73.1534970790505 45.5088414418812 15 15
58 57 -73.1535917348476 45.5087751066406 15 15
59 58 -73.1536863906448 45.5087087714 15 15
60 59 -73.1537225849134 45.5087341365186 15 15
61 60 -73.1536279291163 45.5088004717592 15 15
62 61 -73.1535332733191 45.5088668069997 15 15
63 62 -73.153438617522 45.5089331422403 15 15
64 63 -73.1533439617076 45.5089994774931 15 15
65 64 -73.1533801559762 45.5090248426116 15 15
66 65 -73.1534748117906 45.5089585073589 15 15
67 66 -73.1535694675877 45.5088921721183 15 15
68 67 -73.1536641233849 45.5088258368777 15 15
69 68 -73.153758779182 45.5087595016371 15 15
70 69 -73.1537949734506 45.5087848667557 15 15
71 70 -73.1537003176535 45.5088512019963 15 15
72 71 -73.1536056618563 45.5089175372369 15 15
73 72 -73.1535110060592 45.5089838724775 15 15
74 73 -73.1534163502448 45.5090502077302 15 15
75 74 -73.1534525445134 45.5090755728487 15 15
76 75 -73.1535472003278 45.509009237596 15 15
77 76 -73.1536418561249 45.5089429023554 15 15
78 77 -73.1537365119221 45.5088765671148 15 15
79 78 -73.1538311677192 45.5088102318742 15 15
80 79 -73.1538673619878 45.5088355969928 15 15
81 80 -73.1537727061907 45.5089019322334 15 15
82 81 -73.1536780503936 45.508968267474 15 15
83 82 -73.1535833945964 45.5090346027146 15 15
84 83 -73.153488738782 45.5091009379673 15 15
85 84 -73.1535249330852 45.5091263031101 15 15
86 85 -73.1536195888996 45.5090599678574 15 15
87 86 -73.1537142446968 45.5089936326168 15 15
88 87 -73.1538089004939 45.5089272973762 15 15
89 88 -73.153903556291 45.5088609621356 15 15

28
buzz_scripts/include/timesync.bzz Normal file
View File

@ -0,0 +1,28 @@
TIME_TO_SYNC = 100
sync_timer = 0
timesync_old_state = 0
timesync_itr = 0
timesync_state = 0
# Function to sync. algo
function check_time_sync(){
# sync_timer = sync_timer + 1
# if(sync_timer < TIME_TO_SYNC){
# log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC)
# timesync_state = 1
# }
# else{
# timesync_state = 0
# }
# if(timesync_old_state == 0 and timesync_state == 1){
# timesync_itr = timesync_itr + 1
# timesync_old_state = 0
# }
# timesync_old_state = timesync_state
}
# Function to set sync timer to zero and reinitiate sync. algo
function reinit_time_sync(){
sync_timer = 0
}

10
buzz_scripts/include/update.bzz Normal file
View File

@ -0,0 +1,10 @@
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
updates_active = 1
function updated_no_bct(){
neighbors.broadcast(updated, update_no)
}

69
buzz_scripts/include/utils/conversions.bzz Normal file
View File

@ -0,0 +1,69 @@
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
function LimitSpeed(vel_vec, factor){
if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
return vel_vec
}
# TODO: add other conversions....
function convert_path(P) {
var pathR={}
if(V_TYPE == 0) {
var n = 1
while(n <= size(P)){
pathR[n] = {}
var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell))
pathR[n][1]=tmpgoal.latitude
pathR[n][2]=tmpgoal.longitude
n = n + 1
}
}
return pathR
}
# TODO: add other conversions....
function convert_pt(in) {
if(V_TYPE == 0)
return vec_from_gps(in.x, in.y, 0)
}
function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - pose.position.longitude
d_lat = lat - pose.position.latitude
if(home_ref == 1) {
d_lon = lon - homegps.long
d_lat = lat - homegps.lat
}
ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.new(ned_x,ned_y)
}
function gps_from_vec(vec) {
Lgoal = {.latitude=0, .longitude=0}
Vrange = math.vec2.length(vec)
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude)
latR = pose.position.latitude*math.pi/180.0;
lonR = pose.position.longitude*math.pi/180.0;
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
Lgoal.latitude = target_lat*180.0/math.pi;
Lgoal.longitude = target_lon*180.0/math.pi;
#d_lat = (vec.x / 6371000.0)*180.0/math.pi;
#goal.latitude = d_lat + pose.position.latitude;
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
#goal.longitude = d_lon + pose.position.longitude;
return Lgoal
}

92
buzz_scripts/include/utils/string.bzz Normal file
View File

@ -0,0 +1,92 @@
#
# Returns the string character at the given position.
# PARAM s: The string
# PARAM n: The position of the wanted character
# RETURN The character at the wanted position, or nil
#
string.charat = function(s, n) {
return string.sub(s, n, n+1)
}
#
# Returns the index of the first occurrence of the given string m
# within another string s. If none is found, this function returns
# nil.
# PARAM s: The string
# PARAM m: The string to match
# RETURN: The position of the first match, or nil
#
string.indexoffirst = function(s, m) {
var las = string.length(s)
var lm = string.length(m)
var i = 0
while(i < las-lm+1) {
if(string.sub(s, i, i+lm) == m) return i
i = i + 1
}
return nil
}
#
# Returns the index of the last occurrence of the given string m
# within another string s. If none is found, this function returns
# nil.
# PARAM s: The string
# PARAM m: The string to match
# RETURN: The position of the last match, or nil
#
string.indexoflast = function(s, m) {
var las = string.length(s)
var lm = string.length(m)
var i = las - lm + 1
while(i >= 0) {
if(string.sub(s, i, i+lm) == m) return i
i = i - 1
}
return nil
}
# Splits a string s using the delimiters in d. The string list is
# returned in a table indexed by value (starting at 0).
# PARAM s: The string
# PARAM d: A string containing the delimiters
# RETURN: A table containing the tokens
string.split = function(s, d) {
var i1 = 0 # index to move along s (token start)
var i2 = 0 # index to move along s (token end)
var c = 0 # token count
var t = {} # token list
var las = string.length(s)
var ld = string.length(d)
# Go through string s
while(i2 < las) {
# Try every delimiter
var j = 0 # index to move along d
var f = nil # whether the delimiter was found or not
while(j < ld and (not f)) {
if(string.charat(s, i2) == string.charat(d, j)) {
# Delimiter found
f = 1
# Is it worth adding a new token?
if(i2 > i1) {
t[c] = string.sub(s, i1, i2)
c = c + 1
}
# Start new token
i1 = i2 + 1
}
else {
# Next delimiter
j = j + 1
}
}
# Next string character
i2 = i2 + 1
}
# Is it worth adding a new token?
if(i2 > i1) {
t[c] = string.sub(s, i1, i2)
}
# Return token list
return t;
}

107
buzz_scripts/include/utils/vec2.bzz Normal file
View File

@ -0,0 +1,107 @@
#
# Create a new namespace for vector2 functions
#
math.vec2 = {}
#
# Creates a new vector2.
# PARAM x: The x coordinate.
# PARAM y: The y coordinate.
# RETURN: A new vector2.
#
math.vec2.new = function(x, y) {
return { .x = x, .y = y }
}
#
# Creates a new vector2 from polar coordinates.
# PARAM l: The length of the vector2.
# PARAM a: The angle of the vector2.
# RETURN: A new vector2.
#
math.vec2.newp = function(l, a) {
return {
.x = l * math.cos(a),
.y = l * math.sin(a)
}
}
#
# Calculates the length of the given vector2.
# PARAM v: The vector2.
# RETURN: The length of the vector.
#
math.vec2.length = function(v) {
return math.sqrt(v.x * v.x + v.y * v.y)
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
math.vec2.angle = function(v) {
return math.atan(v.y, v.x)
}
#
# Returns the normalized form of a vector2.
# PARAM v: The vector2.
# RETURN: The normalized form.
#
math.vec2.norm = function(v) {
var l = math.length(v)
return {
.x = v.x / l,
.y = v.y / l
}
}
#
# Calculates v1 + v2.
# PARAM v1: A vector2.
# PARAM v2: A vector2.
# RETURN: v1 + v2
#
math.vec2.add = function(v1, v2) {
return {
.x = v1.x + v2.x,
.y = v1.y + v2.y
}
}
#
# Calculates v1 - v2.
# PARAM v1: A vector2.
# PARAM v2: A vector2.
# RETURN: v1 + v2
#
math.vec2.sub = function(v1, v2) {
return {
.x = v1.x - v2.x,
.y = v1.y - v2.y
}
}
#
# Scales a vector by a numeric constant.
# PARAM v: A vector2.
# PARAM s: A number (float or int).
# RETURN: s * v
#
math.vec2.scale = function(v, s) {
return {
.x = v.x * s,
.y = v.y * s
}
}
#
# Calculates v1 . v2 (the dot product)
# PARAM v1: A vector2.
# PARAM v2: A vector2.
# RETURN: v1 . v2
#
math.vec2.dot = function(v1, v2) {
return v1.x * v2.x + v1.y * v2.y
}

131
buzz_scripts/include/vstigenv.bzz Normal file
View File

@ -0,0 +1,131 @@
########################################
#
# FLEET V.STIGMERGY-RELATED FUNCTIONS
#
########################################
#
# Constants
#
STATUS_VSTIG = 20
GROUND_VSTIG = 21
HIGHEST_ROBOTID = 14
WAIT4STEP = 10
#
# Init var
#
var v_status = {}
var v_ground = {}
b_updating = 0
vstig_counter = WAIT4STEP
function init_swarm() {
s = swarm.create(1)
s.join()
}
function init_stig() {
v_status = stigmergy.create(STATUS_VSTIG)
v_ground = stigmergy.create(GROUND_VSTIG)
}
function uav_updatestig() {
# TODO: Push values on update only.
if(vstig_counter<=0) {
b_updating=1
#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
log("Pushing ", ls, "on vstig with id:", id)
v_status.put(id, ls)
vstig_counter=WAIT4STEP
} else {
b_updating=0
vstig_counter=vstig_counter-1
}
}
function unpackstatus(recv_value){
gps=(recv_value-recv_value%1000000)/1000000
recv_value=recv_value-gps*1000000
batt=(recv_value-recv_value%1000)/1000
recv_value=recv_value-batt*1000
xbee=(recv_value-recv_value%10)/10
recv_value=recv_value-xbee*10
fc=recv_value
log("- GPS ", gps)
log("- Battery ", batt)
log("- Xbee ", xbee)
log("- Status ", fc)
}
function checkusers() {
# Read a value from the structure
if(size(users)>0)
log("Got a user!")
# log(users)
# users_print(users.dataG)
# if(size(users.dataG)>0)
# vt.put("p", users.dataG)
# Get the number of keys in the structure
# log("The vstig has ", vt.size(), " elements")
# users_save(vt.get("p"))
# table_print(users.dataL)
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function usertab_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
function stattab_print() {
if(v_status.size()>0) {
if(b_updating==0) {
u=0
while(u<HIGHEST_ROBOTID){
tab = v_status.get(u)
if(tab!=nil)
unpackstatus(tab)
u=u+1
}
}
}
}
function stattab_send() {
if(v_status.size()>0) {
if(b_updating==0) {
u=0
while(u<HIGHEST_ROBOTID){
tab = v_status.get(u)
if(tab!=nil){
recv_value=tab
gps=(recv_value-recv_value%1000000)/1000000
recv_value=recv_value-gps*1000000
batt=(recv_value-recv_value%1000)/1000
recv_value=recv_value-batt*1000
xbee=(recv_value-recv_value%10)/10
recv_value=recv_value-xbee*10
fc=recv_value
add_neighborStatus(u,gps,batt,xbee,fc)
}
u=u+1
}
}
}
}

117
buzz_scripts/main.bzz Normal file
View File

@ -0,0 +1,117 @@
include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
include "timesync.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
#AUTO_LAUNCH_STATE = "CUSFUN"
#Lowest robot id in the network
LOWEST_ROBOT_ID = 0
TARGET = 9.0
EPSILON = 30.0
#####
# Vehicule type:
# 0 -> outdoor flying vehicle
# 1 -> indoor flying vehicle
# 2 -> outdoor wheeled vehicle
# 3 -> indoor wheeled vehicle
V_TYPE = 0
goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
# Executed once at init time.
function init() {
init_stig()
init_swarm()
# initGraph()
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
loop = 1
# start the swarm command listener
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
}
# Executed at each time step.
function step() {
# check time sync algorithm state
check_time_sync()
# listen to Remote Controller
rc_cmd_listen()
# update the vstig (status/net/batt/...)
# uav_updatestig()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="CUSFUN")
statef=cusfun
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="AGGREGATE")
statef=aggregate
else if(BVMSTATE=="FORMATION")
statef=formation
else if(BVMSTATE=="PURSUIT")
statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
statef=resetGraph
else if(BVMSTATE=="GRAPH_FREE")
statef=DoFree
else if(BVMSTATE=="GRAPH_ASKING")
statef=DoAsking
else if(BVMSTATE=="GRAPH_JOINING")
statef=DoJoining
else if(BVMSTATE=="GRAPH_JOINED")
statef=DoJoined
else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
statef=TransitionToLock
else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
statef=DoLock
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
statef=navigate
else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure
statef=follow
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
statef=take_picture
statef()
log("Current state: ", BVMSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

80
buzz_scripts/mainRRT.bzz Normal file
View File

@ -0,0 +1,80 @@
include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "plan/rrtstar.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "IDLE"
#####
# Vehicule type:
# 0 -> outdoor flying vehicle
# 1 -> indoor flying vehicle
# 2 -> outdoor wheeled vehicle
# 3 -> indoor wheeled vehicle
V_TYPE = 0
goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
# Executed once at init time.
function init() {
init_stig()
init_swarm()
TARGET_ALTITUDE = 25.0 # m
# start the swarm command listener
# nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup.
# BVMSTATE = "TURNEDOFF"
BVMSTATE = "LAUNCHED"
}
# Executed at each time step.
function step() {
# listen to Remote Controller
rc_cmd_listen()
# update the vstig (status/net/batt/...)
# uav_updatestig()
#
# graph state machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure
statef=makegraph # or bidding
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
statef=navigate
else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure
statef=follow
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
statef=take_picture
statef()
log("Current state: ", BVMSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

63
buzz_scripts/minimal.bzz Normal file
View File

@ -0,0 +1,63 @@
include "update.bzz"
include "act/states.bzz"
include "vstigenv.bzz"
# State launched after takeoff
AUTO_LAUNCH_STATE = "ACTION"
#####
# Vehicule type:
# 0 -> outdoor flying vehicle
# 1 -> indoor flying vehicle
# 2 -> outdoor wheeled vehicle
# 3 -> indoor wheeled vehicle
V_TYPE = 0
# Executed once at init time.
function init() {
init_swarm()
TARGET_ALTITUDE = 25.0 # m
# start the swarm command listener
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
}
function action() {
BVMSTATE = "ACTION"
# do some actions....
}
# Executed at each time step.
function step() {
# listen to Remote Controller
rc_cmd_listen()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="ACTION")
statef=action
statef()
log("Current state: ", BVMSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

38
buzz_scripts/stand_by.bzz Normal file
View File

@ -0,0 +1,38 @@
include "act/states.bzz"
include "vstigenv.bzz"
updated="update_ack"
update_no=0
BVMSTATE = "UPDATESTANDBY"
# Executed once at init time.
function init(){
barrier = stigmergy.create(101)
barrier.put(id,1)
barrier_val=0
barrier.onconflict(function (k,l,r) {
if(r. data < l. data or (r. data == l. data )) return l
else return r
})
init_swarm()
# start the swarm command listener
nei_cmd_listen()
}
function stand_by(){
barrier.get(id)
barrier_val = barrier.size()
neighbors.listen(updated,
function(vid, value, rid) {
if(value==update_no) barrier.put(rid,1)
}
)
}
# Executed at each time step.
function step() {
stand_by()
}

72
buzz_scripts/testLJ.bzz Executable file
View File

@ -0,0 +1,72 @@
include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "vstigenv.bzz"
V_TYPE = 0
#State launched after takeoff
AUTO_LAUNCH_STATE = "FORMATION"
TARGET = 8.0
EPSILON = 3.0
GOTO_MAXVEL = 2.5 # m/steps
# Executed once at init time.
function init() {
init_stig()
init_swarm()
# start the swarm command listener
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
TAKEOFF_COUNTER = 20
}
# Executed at each time step.
function step() {
rc_cmd_listen()
# update the vstig (status/net/batt/...)
# uav_updatestig()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="FORMATION")
statef=formation
statef()
log("Current state: ", BVMSTATE)
# Auto-takeoff (delayed for simulator boot)
if(id == 0) {
if(TAKEOFF_COUNTER>0)
TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1
else if(TAKEOFF_COUNTER == 0) {
BVMSTATE="LAUNCH"
TAKEOFF_COUNTER = -1
}
}
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

57
buzz_scripts/testaloneWP.bzz Normal file
View File

@ -0,0 +1,57 @@
include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "ACTION"
function action() {
BVMSTATE = "ACTION"
uav_storegoal(-1.0,-1.0,-1.0)
goto_gps(picture)
}
# Executed once at init time.
function init() {
init_stig()
init_swarm()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
}
# Executed at each time step.
function step() {
rc_cmd_listen()
# update the vstig (status/net/batt/...)
# uav_updatestig()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="ACTION")
statef=action
statef()
log("Current state: ", BVMSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

171
include/buzz_update.h Normal file
View File

@ -0,0 +1,171 @@
#ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/time.h>
#include <sys/inotify.h>
#include <buzz/buzztype.h>
#include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h>
#include <buzz_utility.h>
#include <fstream>
#define delete_p(p) \
do \
{ \
free(p); \
p = NULL; \
} while (0)
namespace buzz_update
{
static const uint16_t CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251;
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/
/* Updater states */
/********************/
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update
} code_states_e;
/*********************/
/*Message types */
/********************/
typedef enum {
SENT_CODE = 0, // Broadcast code
RESEND_CODE, // ReBroadcast request
} code_message_e;
/*************************/
/*Updater message queue */
/*************************/
struct updater_msgqueue_s
{
uint8_t* queue;
uint8_t* size;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s
{
uint8_t* bcode;
uint8_t* bcode_size;
};
typedef struct updater_code_s* updater_code_t;
/**************************/
/*Updater data*/
/**************************/
struct buzz_updater_elem_s
{
/* robot id */
// uint16_t robotid;
/*current Bytecode content */
uint8_t* bcode;
/*old Bytecode name */
const char* old_bcode;
/*current bcode size*/
size_t* bcode_size;
/*Update patch*/
uint8_t* patch;
/* Update patch size*/
size_t* patch_size;
/*current Bytecode content */
uint8_t* standby_bcode;
/*current bcode size*/
size_t* standby_bcode_size;
/*updater out msg queue */
updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/
updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/
void update_routine();
/************************************************/
/*Initalizes the updater */
/************************************************/
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*********************************************************/
/*Processes messages inside the queue of the updater*/
/*********************************************************/
void code_message_inqueue_process();
/*****************************************************/
/*Obtains messages from out msgs queue of the updater*/
/*******************************************************/
uint8_t* getupdater_out_msg();
/******************************************************/
/*Obtains out msg queue size*/
/*****************************************************/
uint8_t* getupdate_out_msg_size();
/**************************************************/
/*Destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
// buzz_updater_elem_t get_updater();
/***************************************************/
/*Sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file, bool dbg);
/****************************************************/
/*Tests the code from a buffer*/
/***************************************************/
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
/****************************************************/
/*Destroys the updater*/
/***************************************************/
void destroy_updater();
/****************************************************/
/*Checks for updater message*/
/***************************************************/
int is_msg_present();
/****************************************************/
/*Compiles a bzz script to bo and bdbg*/
/***************************************************/
int compile_bzz(std::string bzz_file);
/****************************************************/
/*Set number of robots in the updater*/
/***************************************************/
void updates_set_robots(int robots);
}
#endif

108
include/buzz_utility.h Normal file
View File

@ -0,0 +1,108 @@
#pragma once
#include <stdio.h>
#include "buzz_utility.h"
#include "buzzuav_closures.h"
#include "buzz_update.h"
#include <buzz/buzzdebug.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include <stdint.h>
#include <map>
using namespace std;
namespace buzz_utility
{
struct pos_struct
{
double x, y, z;
pos_struct(double x, double y, double z) : x(x), y(y), z(z){};
pos_struct()
{
}
};
typedef struct pos_struct Pos_struct;
struct rb_struct
{
double r, b, latitude, longitude, altitude;
rb_struct(double la, double lo, double al, double r, double b)
: latitude(la), longitude(lo), altitude(al), r(r), b(b){};
rb_struct()
{
}
};
typedef struct rb_struct RB_struct;
struct neiStatus
{
uint gps_strenght = 0;
uint batt_lvl = 0;
uint xbee = 0;
uint flight_status = 0;
};
typedef struct neiStatus neighbors_status;
struct neitime
{
uint64_t nei_hardware_time;
uint64_t nei_logical_time;
uint64_t node_hardware_time;
uint64_t node_logical_time;
double nei_rate;
double relative_rate;
int age;
neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr)
: nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt),
nei_rate(nr), relative_rate(mr) {};
neitime()
{
}
};
typedef struct neitime neighbor_time;
uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type, int msg_size);
int make_table(buzzobj_t* t);
int buzzusers_reset();
int create_stig_tables();
void in_msg_append(uint64_t* payload);
uint64_t* obt_out_msg();
void update_sensors();
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id);
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
void buzz_script_step();
void buzz_script_destroy();
int buzz_script_done();
int update_step_test();
int get_robotid();
int get_swarmsize();
buzzvm_t get_vm();
void set_robot_var(int ROBOTS);
int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector();
std::string get_bvmstate();
int get_timesync_state();
int get_timesync_itr();
}

183
include/buzzuav_closures.h Normal file
View File

@ -0,0 +1,183 @@
#pragma once
#include <buzz/buzzvm.h>
#include <stdio.h>
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/Mavlink.h"
#include "ros/ros.h"
#include "buzz_utility.h"
#include "rosbuzz/mavrosCC.h"
#define EARTH_RADIUS (double)6371000.0
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI)))
namespace buzzuav_closures
{
/*
* prextern int() function in Buzz
* This function is used to print data from buzz
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
*/
int buzzros_print(buzzvm_t vm);
void setWPlist(std::string path);
/*
* closure to move following a vector
*/
int buzzuav_moveto(buzzvm_t vm);
/*
* closure to store a new GPS goal
*/
int buzzuav_storegoal(buzzvm_t vm);
/*
* closure to control the gimbal
*/
int buzzuav_setgimbal(buzzvm_t vm);
/*
* parse a csv list of waypoints
*/
void parse_gpslist();
/*
* closure to export a 2D map
*/
int buzz_exportmap(buzzvm_t vm);
/*
* closure to take a picture
*/
int buzzuav_takepicture(buzzvm_t vm);
/*
* Returns the current command from local variable
*/
int getcmd();
/*
* Sets goto position from rc client
*/
void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*
*Sets gimbal orientation from rc client
*/
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
/*
* sets rc requested command
*/
void rc_call(int rc_cmd);
/*
* sets the battery state
*/
void set_battery(float voltage, float current, float remaining);
/*
* sets the xbee network status
*/
void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_packet_loss(float value);
void set_filtered_packet_loss(float value);
// void set_api_rssi(float value);
/*
* sets current position
*/
void set_currentNEDpos(double x, double y);
void set_currentpos(double latitude, double longitude, float altitude, float yaw);
/*
* returns the current go to position
*/
double* getgoto();
/*
* returns the current grid
*/
std::map<int, std::map<int,int>> getgrid();
/*
* returns the gimbal commands
*/
float* getgimbal();
/*
*updates flight status
*/
void flight_status_update(uint8_t state);
/*
*Update neighbors table
*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation);
/*
* update neighbors from in msgs
*/
void update_neighbors(buzzvm_t vm);
/*
*Clear neighbours struct
*/
void clear_neighbours_pos();
/*
* closure to add a neighbor status
*/
int buzzuav_addNeiStatus(buzzvm_t vm);
/*
* returns the current array of neighbors status
*/
mavros_msgs::Mavlink get_status();
/*
*Flight status
*/
void set_obstacle_dist(float dist[]);
/*
* Commands the UAV to takeoff
*/
int buzzuav_takeoff(buzzvm_t vm);
/*
* Arm command from Buzz
*/
int buzzuav_arm(buzzvm_t vm);
/*
* Disarm from buzz
*/
int buzzuav_disarm(buzzvm_t vm);
/* Commands the UAV to land
*/
int buzzuav_land(buzzvm_t vm);
/*
* Command the UAV to go to home location
*/
int buzzuav_gohome(buzzvm_t vm);
/*
* Updates battery information in Buzz
*/
int buzzuav_update_battery(buzzvm_t vm);
/*
* Updates xbee_status information in Buzz
*/
int buzzuav_update_xbee_status(buzzvm_t vm);
/*
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
/*
* add new target in the BVM
*/
int buzzuav_addtargetRB(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status
* use flight.rc_cmd for current rc cmd
*/
int buzzuav_update_flight_status(buzzvm_t vm);
/*
* Updates IR information in Buzz
* Proximity and ground sensors to do !!!!
*/
int buzzuav_update_prox(buzzvm_t vm);
/*
* returns the current FC command
*/
int bzz_cmd();
int dummy_closure(buzzvm_t vm);
}

24
include/rosbuzz/mavrosCC.h Normal file
View File

@ -0,0 +1,24 @@
#pragma once
#if MAVROSKINETIC
const short MISSION_START = mavros_msgs::CommandCode::MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START;
const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM;
#endif
const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF;
const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND;
const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT;
const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT;
const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE;
const short CMD_REQUEST_UPDATE = 666;
const short CMD_SYNC_CLOCK = 777;

309
include/roscontroller.h Normal file
View File

@ -0,0 +1,309 @@
#pragma once
#include <ros/ros.h>
#include <tf/tf.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/UInt8.h>
#include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/CommandLong.h"
#include "mavros_msgs/CommandBool.h"
#include "mavros_msgs/ExtendedState.h"
#include "mavros_msgs/SetMode.h"
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "sensor_msgs/BatteryState.h"
#include "mavros_msgs/Mavlink.h"
#include "mavros_msgs/PositionTarget.h"
#include "sensor_msgs/NavSatStatus.h"
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/Waypoint.h"
#include "mavros_msgs/PositionTarget.h"
#include "mavros_msgs/StreamRate.h"
#include "mavros_msgs/ParamGet.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/Float64.h"
#include "std_msgs/String.h"
#include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h>
#include <sstream>
#include <buzz/buzzasm.h>
#include "buzz_utility.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <signal.h>
#include <ostream>
#include <map>
#include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h"
/*
* ROSBuzz message types
*/
typedef enum {
ROS_BUZZ_MSG_NIL = 0, // dummy msg
UPDATER_MESSAGE, // Update msg
BUZZ_MESSAGE_NO_TIME, // Broadcast message wihout time info
BUZZ_MESSAGE_TIME, // Broadcast message with time info
} rosbuzz_msgtype;
// Time sync algo. constants
#define COM_DELAY 100000000 // in nano seconds i.e 100 ms
#define TIME_SYNC_JUMP_THR 500000000
#define MOVING_AVERAGE_ALPHA 0.1
#define MAX_NUMBER_OF_ROBOTS 10
#define TIMEOUT 60
#define BUZZRATE 10
using namespace std;
namespace rosbuzz_node
{
class roscontroller
{
public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
private:
struct num_robot_count
{
uint8_t history[10];
uint8_t index = 0;
uint8_t current = 0;
num_robot_count()
{
}
};
typedef struct num_robot_count Num_robot_count;
Num_robot_count count_robots;
struct POSE
{
double longitude = 0.0;
double latitude = 0.0;
float altitude = 0.0;
// NED coordinates
float x = 0.0;
float y = 0.0;
float z = 0.0;
float yaw = 0.0;
};
typedef struct POSE ros_pose;
ros_pose target, home, cur_pos;
struct MsgData
{
int msgid;
uint16_t nid;
uint16_t size;
double sent_time;
uint64_t received_time;
MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt):
msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
MsgData(){};
};
typedef struct MsgData msg_data;
uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
int timer_step = 0;
int robot_id = 0;
ros::Time logical_clock;
ros::Time previous_step_time;
std::vector<msg_data> inmsgdata;
uint64_t out_msg_time;
double logical_time_rate;
bool time_sync_jumped;
std::string robot_name = "";
int rc_cmd;
float fcu_timeout;
int armstate;
int barrier;
int update;
int message_number = 0;
uint8_t no_of_robots = 0;
bool rcclient;
bool xbeeplugged = false;
bool multi_msg;
uint8_t no_cnt = 0;
uint8_t old_val = 0;
bool debug = false;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
std::string modeclient;
std::string rcservice_name;
std::string bcfname, dbgfname;
std::string out_payload;
std::string in_payload;
std::string stand_by;
std::string xbeesrv_name;
std::string capture_srv_name;
std::string setpoint_name;
std::string stream_client_name;
std::string setpoint_nonraw;
// ROS service, publishers and subscribers
ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv;
ros::ServiceClient capture_srv;
ros::ServiceClient stream_client;
ros::Publisher payload_pub;
ros::Publisher MPpayload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher bvmstate_pub;
ros::Publisher grid_pub;
ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber users_sub;
ros::Subscriber battery_sub;
ros::Subscriber payload_sub;
ros::Subscriber flight_estatus_sub;
ros::Subscriber flight_status_sub;
ros::Subscriber obstacle_sub;
ros::Subscriber Robot_id_sub;
ros::Subscriber relative_altitude_sub;
ros::Subscriber local_pos_sub;
std::map<std::string, std::string> m_smTopic_infos;
int setpoint_counter;
std::ofstream log;
// Commands for flight controller
mavros_msgs::CommandLong cmd_srv;
mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient arm_client;
mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
// Initialize publisher and subscriber, done in the constructor
void Initialize_pub_sub(ros::NodeHandle& n_c);
std::string current_mode;
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*compiles buzz script from the specified .bzz file*/
std::string Compile_bzz(std::string bzzfile_name);
/*Flight controller service call*/
void flight_controller_service_call();
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*UAVState publisher*/
void state_publisher();
/*Grid publisher*/
void grid_publisher();
/*BVM message payload publisher*/
void send_MPpayload();
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude, double longitude, double altitude);
/*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x);
void gps_rb(POSE nei_pos, double out[]);
void gps_ned_cur(float& ned_x, float& ned_y, POSE t);
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat);
/*battery status callback */
void battery(const sensor_msgs::BatteryState::ConstPtr& msg);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*flight status callback*/
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
/*current position callback*/
void global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*current relative altitude callback*/
void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg);
/*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/
void obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/
void Arm();
/*set mode like guided for solo*/
void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c);
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
void fc_command_setup();
void SetLocalPosition(float x, float y, float z, float yaw);
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
// functions related to Xbee modules information
void GetRobotId();
bool GetDequeFull(bool& result);
bool GetRssi(float& result);
bool TriggerAPIRssi(const uint8_t short_id);
bool GetAPIRssi(const uint8_t short_id, float& result);
bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status();
void time_sync_step();
void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr);
uint64_t get_logical_time();
void set_logical_time_correction(uint64_t cor);
};
}

12
launch/launch_config/solo.yaml Normal file
View File

@ -0,0 +1,12 @@
topics:
gps : mavros/global_position/global
battery : mavros/battery
status : mavros/state
fcclient: mavros/cmd/command
setpoint: mavros/setpoint_position/local
armclient: mavros/cmd/arming
modeclient: mavros/set_mode
localpos: mavros/local_position/pose
stream: mavros/set_stream_rate
altitude: mavros/global_position/rel_alt

13
launch/launch_config/topics.yaml Normal file
View File

@ -0,0 +1,13 @@
topics:
gps : global_position/global
battery : battery
status : state
estatus : extended_state
fcclient: cmd/command
setpoint: setpoint_position/local
armclient: cmd/arming
modeclient: set_mode
localpos: local_position/pose
stream: set_stream_rate
altitude: global_position/rel_alt
obstacles: obstacles

25
launch/rosbuzz.launch Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<!-- Generic launch file for ROSBuzz -->
<!-- This file is included in all ROS workspace launch files -->
<!-- Modify with great care! -->
<launch>
<arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/>
<arg name="launch_config" default="topics"/>
<arg name="debug" default="false" />
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" />
<param name="debug" value="$(arg debug)" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

25
launch/rosbuzzd.launch Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<!-- Generic launch file for ROSBuzz -->
<!-- This file is included in all ROS workspace launch files -->
<!-- Modify with great care! -->
<launch>
<arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/>
<arg name="launch_config" default="topics"/>
<arg name="debug" default="true" />
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" />
<param name="debug" value="$(arg debug)" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
<param name="out_payload" value="outMavlink"/>
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/>
<param name="xbee_status_srv" value="xbee_status"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
</node>
</launch>

44
misc/cmdlinectr.sh Normal file
View File

@ -0,0 +1,44 @@
#! /bin/bash
function takeoff {
rosservice call $1/buzzcmd 0 22 0 0 0 0 0 0 0 0
}
function land {
rosservice call $1/buzzcmd 0 21 0 0 0 0 0 0 0 0
}
function arm {
rosservice call $1/buzzcmd 0 400 0 1 0 0 0 0 0 0
}
function disarm {
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
}
function timesync {
rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0
}
function testWP {
rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
}
function record {
rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
}
function clean {
sudo rm /var/log/upstart/robot*
sudo rm /var/log/upstart/dji*
sudo rm /var/log/upstart/x3s*
}
function startrobot {
sudo service dji start
}
function stoprobot {
sudo service dji stop
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXHeavenbuzzy.launch
# rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXxbeebuzzy.launch
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch
}
function uavstate {
let "a = $1 + 900"
rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0
}

2
msg/neigh_pos.msg Normal file
View File

@ -0,0 +1,2 @@
Header header
sensor_msgs/NavSatFix[] pos_neigh

61
package.xml Normal file
View File

@ -0,0 +1,61 @@
<?xml version="1.0"?>
<package>
<name>rosbuzz</name>
<version>0.1.0</version>
<description>The rosbuzz package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="vivek-shankar.varadharajan@polymtl.ca">vivek</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>MIT</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rosbuzz</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<author email="vivek-shankar.varadharajan@polymtl.ca">vivek</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>
<build_depend>std_msgs</build_depend>
<run_depend>std_msgs</run_depend>
<build_depend>mavros_msgs</build_depend>
<run_depend>mavros_msgs</run_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>sensor_msgs</run_depend>
<build_depend>nav_msgs</build_depend>
<run_depend>nav_msgs</run_depend>
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

111
readme.md Normal file
View File

@ -0,0 +1,111 @@
ROS Implemenation of Buzz
=========================
What is Buzz?
=============
Buzz is a novel programming language for heterogeneous robots swarms.
Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion.
Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations.
Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm.
Self-organization results from the fact that the Buzz run-time platform is purely distributed.
The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS.
More information is available at http://the.swarming.buzz/wiki/doku.php?id=start.
Description:
============
Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS.
Downloading ROS Package
=======================
$ git clone https://github.com/MISTLab/ROSBuzz.git rosbuzz
Requirements
============
* Buzz :
You can download the development sources through git:
$ git clone https://github.com/MISTLab/Buzz.git buzz
* ROS binary distribution (Indigo or higher) with catkin (could be used with older versions of ROS with catkin but not tested)
You need the following package:
* mavros_msgs :
You can install using apt-get:
$ sudo apt-get install ros-<distro>-mavros ros-<distro>-mavros-extras
Compilation
===========
To compile the ros package, execute the following:
$ cd catkin_ws
$ catkin_make
$ source devel/setup.bash
Run
===
To run the ROSBuzz package using the launch file, execute the following:
$ roslaunch rosbuzz rosbuzz.launch
Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch).
* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
Publisher
=========
* Messages from Buzz (BVM):
The package publishes mavros_msgs/Mavlink message "outMavlink".
* Command to the flight controller:
The package publishes geometry_msgs/PoseStamped message "setpoint_position/local".
Subscribers
-----------
* Current position of the Robot:
The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose".
* Messages to Buzz (BVM):
The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink".
* Status:
The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state".
Service
-------
* Remote Controller:
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line.
References
------
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
Visual Studio Code
--------------------
To activate highlights of the code in Visual Studio Code or Roboware add the following to settings.json:
```
"files.associations": {
"*.bzz":"python"
}
```

724
src/buzz_update.cpp Normal file
View File

@ -0,0 +1,724 @@
/** @file buzz_utility.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzz_update.h"
namespace buzz_update{
/*Temp for data collection*/
// static int neigh=-1;
static struct timeval t1, t2;
static int timer_steps = 0;
// static clock_t end;
/*Temp end */
static int fd, wd = 0;
static int old_update = 0;
static buzz_updater_elem_t updater;
static int no_of_robot;
static const char* dbgf_name;
static const char* bcfname;
static const char* old_bcfname = "old_bcode.bo";
static const char* bzz_file;
static int Robot_id = 0;
static int neigh = -1;
static int updater_msg_ready;
static uint16_t update_try_counter = 0;
static const uint16_t MAX_UPDATE_TRY = 5;
static size_t old_byte_code_size = 0;
static bool debug = false;
/*Initialize updater*/
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
{
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){
Robot_id = robot_id;
dbgf_name = dbgfname;
bcfname = bo_filename;
ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)");
if(debug) ROS_INFO("Initializing file monitor...");
fd = inotify_init1(IN_NONBLOCK);
if (fd < 0)
{
perror("inotify_init error");
}
/*If simulation set the file monitor only for robot 0*/
if (SIMULATION == 1 && robot_id == 0)
{
/* watch /.bzz file for any activity and report it back to update */
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
}
else if (SIMULATION == 0)
{
/* watch /.bzz file for any activity and report it back to update */
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
}
/*load the .bo under execution into the updater*/
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bo_filename, "rb");
if (!fp)
{
perror(bo_filename);
}
fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp);
rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size);
if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
{
perror(bo_filename);
// fclose(fp);
// return 0;
}
fclose(fp);
/*Load stand_by .bo file into the updater*/
uint8_t* STD_BO_BUF = 0;
fp = fopen(stand_by_script, "rb");
if (!fp)
{
perror(stand_by_script);
}
fseek(fp, 0, SEEK_END);
size_t stdby_bcode_size = ftell(fp);
rewind(fp);
STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size)
{
perror(stand_by_script);
// fclose(fp);
// return 0;
}
fclose(fp);
/*Create the updater*/
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
/*Intialize the updater with the required data*/
updater->bcode = BO_BUF;
/*Store a copy of the Bcode for rollback*/
updater->outmsg_queue = NULL;
updater->inmsg_queue = NULL;
updater->patch = NULL;
updater->patch_size = (size_t*)malloc(sizeof(size_t));
updater->bcode_size = (size_t*)malloc(sizeof(size_t));
updater->update_no = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(updater->update_no) = 0;
*(size_t*)(updater->bcode_size) = bcode_size;
updater->standby_bcode = STD_BO_BUF;
updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
*(size_t*)(updater->standby_bcode_size) = stdby_bcode_size;
updater->mode = (int*)malloc(sizeof(int));
*(int*)(updater->mode) = CODE_RUNNING;
// no_of_robot=barrier;
updater_msg_ready = 0;
/*Write the bcode to a file for rollback*/
fp = fopen("old_bcode.bo", "wb");
fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
fclose(fp);
return 1;
}
else{
ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)");
return 0;
}
}
/*Check for .bzz file chages*/
int check_update()
{
char buf[1024];
int check = 0;
int i = 0;
int len = read(fd, buf, 1024);
while (i < len)
{
struct inotify_event* event = (struct inotify_event*)&buf[i];
/*Report file changes and self deletes*/
if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
{
/*Respawn watch if the file is self deleted */
inotify_rm_watch(fd, wd);
close(fd);
fd = inotify_init1(IN_NONBLOCK);
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
/*To mask multiple writes from editors*/
if (!old_update)
{
check = 1;
old_update = 1;
}
}
/*Update index to start of next event*/
i += sizeof(struct inotify_event) + event->len;
}
if (!check)
old_update = 0;
return check;
}
int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch)
{
if (SIMULATION == 1)
{
return 1;
}
else
{
/*Patch the old bo with new patch*/
std::stringstream patch_writefile;
patch_writefile << path << name1 << "tmp_patch.bo";
/*Write the patch to a file and call bsdiff to patch*/
FILE* fp = fopen(patch_writefile.str().c_str(), "wb");
fwrite(patch, update_patch_size, 1, fp);
fclose(fp);
std::stringstream patch_exsisting;
patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
<< "tmp_patch.bo";
if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
if (system(patch_exsisting.str().c_str()))
return 0;
else
return 1;
}
}
updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
{
if (SIMULATION == 1)
{
/*Read the exsisting file to simulate the patching*/
std::stringstream read_patched;
read_patched << path << name1 << ".bo";
if(debug){
ROS_WARN("Simulation patching ...");
ROS_WARN("Read file for patching %s", read_patched.str().c_str());
}
uint8_t* patched_bo_buf = 0;
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
{
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_bo_buf = (uint8_t*)malloc(patched_size);
if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
}
fclose(fp);
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_bo_buf;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
return update_code;
}
else
{
/*Read the new patched file*/
std::stringstream read_patched;
read_patched << path << name1 << "-patched.bo";
if(debug) {
ROS_WARN("Robot patching ...");
ROS_WARN("Read file for patching %s", read_patched.str().c_str());
}
uint8_t* patched_bo_buf = 0;
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
{
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_bo_buf = (uint8_t*)malloc(patched_size);
if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
}
fclose(fp);
/* delete old bo file & rename new bo file */
remove((path + name1 + ".bo").c_str());
rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str());
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_bo_buf;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
return update_code;
}
}
void code_message_outqueue_append()
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
/* if size less than 250 Pad with zeors to assure transmission*/
uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size;
size += padding_size;
updater->outmsg_queue->queue = (uint8_t*)malloc(size);
memset(updater->outmsg_queue->queue, 0, size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(updater->outmsg_queue->size) = size;
size = 0;
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE;
size += sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no);
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size);
size += sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size));
// size += (uint16_t) * (size_t*)(updater->patch_size);
updater_msg_ready = 1;
}
void outqueue_append_code_request(uint16_t update_no)
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size = 0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE;
size += sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue + size) = update_no;
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter;
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
updater_msg_ready = 1;
if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter);
}
void code_message_inqueue_append(uint8_t* msg, uint16_t size)
{
updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// ROS_INFO("[DEBUG] Updater append code of size %d", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size);
*(uint16_t*)(updater->inmsg_queue->size) = size;
}
/*Used for data logging*/
/*void set_packet_id(int packet_id)
{
packet_id_ = packet_id;
}*/
void code_message_inqueue_process()
{
int size = 0;
updater_code_t out_code = NULL;
if(debug) {
ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)),
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
ROS_WARN("[Debug] Updater received patch of size %u",
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t))));
}
if (*(int*)(updater->mode) == CODE_RUNNING)
{
// fprintf(stdout,"[debug]Inside inmsg code running");
if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
{
// fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t);
uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t);
/*Generate patch*/
std::string bzzfile_name(bzz_file);
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0, name1.find_last_of("."));
if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size)))
{
out_code = obtain_patched_bo(path, name1);
// fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
// fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
// FILE *fp;
// fp=fopen("update.bo", "wb");
// fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
// fclose(fp);
if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
{
if(debug) ROS_WARN("Received patch PASSED tests!");
*(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/
delete_p(updater->patch);
/*copy the patch into the updater*/
updater->patch = (uint8_t*)malloc(update_patch_size);
memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size);
*(size_t*)(updater->patch_size) = update_patch_size;
// code_message_outqueue_append();
neigh = 1;
}
/*clear the temp code buff*/
delete_p(out_code->bcode);
delete_p(out_code->bcode_size);
delete_p(out_code);
}
else
{
ROS_ERROR("Patching the .bo file failed");
update_try_counter++;
outqueue_append_code_request(update_no);
}
}
}
}
size = 0;
if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{
if(debug) ROS_WARN("Patch rebroadcast request received.");
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
{
size += sizeof(uint16_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
{
update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter);
code_message_outqueue_append();
}
if (update_try_counter > MAX_UPDATE_TRY)
ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!");
}
}
// fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
void create_update_patch()
{
std::stringstream genpatch;
std::string bzzfile_name(bzz_file);
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0, name1.find_last_of("."));
std::string name2 = name1 + "-update";
/*Launch bsdiff and create a patch*/
genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
system(genpatch.str().c_str());
/*Delete old files & rename new files*/
remove((path + name1 + ".bo").c_str());
remove((path + name1 + ".bdb").c_str());
rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
/*Read the diff file */
std::stringstream patchfileName;
patchfileName << path << "diff.bo";
uint8_t* patch_buff = 0;
FILE* fp = fopen(patchfileName.str().c_str(), "rb");
if (!fp)
{
perror(patchfileName.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patch_size = ftell(fp);
rewind(fp);
patch_buff = (uint8_t*)malloc(patch_size);
if (fread(patch_buff, 1, patch_size, fp) < patch_size)
{
perror(patchfileName.str().c_str());
}
fclose(fp);
delete_p(updater->patch);
updater->patch = patch_buff;
*(size_t*)(updater->patch_size) = patch_size;
/* Delete the diff file */
remove(patchfileName.str().c_str());
}
void update_routine()
{
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
// fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if (*(int*)updater->mode == CODE_RUNNING)
{
buzzvm_function_call(VM, "updated_no_bct", 0);
// Check for update
if (check_update())
{
ROS_INFO("Update found \tUpdating script ...");
if (compile_bzz(bzz_file))
{
ROS_ERROR("Error in compiling script, resuming old script.");
}
else
{
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << path << name << "-update.bo";
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb");
if (!fp)
{
perror(bzzfile_in_compile.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp);
rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size);
if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
{
perror(bcfname);
}
fclose(fp);
if (test_set_code(BO_BUF, dbgf_name, bcode_size))
{
uint16_t update_no = *(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) = update_no + 1;
create_update_patch();
VM = buzz_utility::get_vm();
if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
neigh = -1;
if(debug) ROS_INFO("Broadcasting patch ...");
code_message_outqueue_append();
}
delete_p(BO_BUF);
}
}
}
else
{
timer_steps++;
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1));
buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value);
if (tObj->i.value == no_of_robot)
{
ROS_WARN("Patch deployment successful, rolling forward");
*(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL);
// collect_data();
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
// buzzvm_function_call(m_tBuzzVM, "updated", 0);
update_try_counter = 0;
timer_steps = 0;
}
else if (timer_steps > TIMEOUT_FOR_ROLLBACK)
{
ROS_ERROR("TIME OUT Reached, rolling back");
/*Time out hit deceided to roll back*/
*(int*)(updater->mode) = CODE_RUNNING;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
update_try_counter = 0;
timer_steps = 0;
}
}
}
uint8_t* getupdater_out_msg()
{
return (uint8_t*)updater->outmsg_queue->queue;
}
uint8_t* getupdate_out_msg_size()
{
// fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return (uint8_t*)updater->outmsg_queue->size;
}
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
{
if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
{
if(debug) ROS_WARN("Initializtion test passed");
if (buzz_utility::update_step_test())
{
/*data logging*/
old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/
if(debug) ROS_WARN("Step test passed");
*(int*)(updater->mode) = CODE_STANDBY;
// fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode);
updater->bcode = (uint8_t*)malloc(bcode_size);
memcpy(updater->bcode, BO_BUF, bcode_size);
*(size_t*)updater->bcode_size = bcode_size;
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
gettimeofday(&t1, NULL);
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM);
return 1;
}
/*Unable to step something wrong*/
else
{
if (*(int*)(updater->mode) == CODE_RUNNING)
{
ROS_ERROR("Step test failed, resuming old script");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
}
else
{
/*You will never reach here*/
ROS_ERROR("Step test failed, returning to standby");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM);
}
return 0;
}
}
else
{
if (*(int*)(updater->mode) == CODE_RUNNING)
{
ROS_ERROR("Initialization test failed, resuming old script");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size));
}
else
{
/*You will never reach here*/
ROS_ERROR("Initialization test failed, returning to standby");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM);
}
return 0;
}
}
void destroy_out_msg_queue()
{
// fprintf(stdout,"out queue freed\n");
delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue);
updater_msg_ready = 0;
}
int is_msg_present()
{
return updater_msg_ready;
}
/*buzz_updater_elem_t get_updater()
{
return updater;
}*/
void destroy_updater()
{
delete_p(updater->bcode);
delete_p(updater->bcode_size);
delete_p(updater->standby_bcode);
delete_p(updater->standby_bcode_size);
delete_p(updater->mode);
delete_p(updater->update_no);
if (updater->outmsg_queue)
{
delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue);
}
if (updater->inmsg_queue)
{
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
inotify_rm_watch(fd, wd);
close(fd);
}
void set_bzz_file(const char* in_bzz_file, bool dbg)
{
debug=dbg;
bzz_file = in_bzz_file;
}
void updates_set_robots(int robots)
{
no_of_robot = robots;
}
/*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script input
/-------------------------------------------------------*/
int compile_bzz(std::string bzz_file)
{
/*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
bzzfile_in_compile << " -b " << path << name << "-update.bo";
bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
bzzfile_in_compile << bzzfile_name;
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str());
}
/*void collect_data(std::ofstream& logger)
{
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
// RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
//
Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << ","
<< old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
<< (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
}*/
}

626
src/buzz_utility.cpp Normal file
View File

@ -0,0 +1,626 @@
/** @file buzz_utility.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzz_utility.h"
namespace buzz_utility
{
/****************************************/
/****************************************/
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map<int, Pos_struct> users_map;
/****************************************/
uint16_t* u64_cvt_u16(uint64_t u64)
/*
/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user
------------------------------------------------------------------------*/
{
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000)) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000)) >> 16;
// DEBUG
// cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
int get_robotid()
/*
/ return this robot ID
------------------------------------------------------------------------*/
{
return Robot_id;
}
void in_msg_append(uint64_t* payload)
/*
/ Appends obtained messages to buzz in message Queue
---------------------------------------------------------------------
/ Message format of payload (Each slot is uint64_t)
/ _______________________________________________________________________________________________________________
/| | |
/|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|
/|__________________________________________________________________________|____________________________________|
---------------------------------------------------------------------------------------------------------------------*/
{
// Go through messages and append them to the vector
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
// Size is at first 2 bytes
uint16_t size = data[0] * sizeof(uint64_t);
delete[] data;
uint8_t* pl = (uint8_t*)malloc(size);
// Copy packet into temporary buffer
memcpy(pl, payload, size);
IN_MSG.push_back(pl);
}
void in_message_process()
/*
/ Process msgs in
---------------------------------------------------------------------------------------------------------------------*/
{
while (!IN_MSG.empty())
{
// Go through messages and append them to the FIFO
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
size_t tot = 0;
// Size is at first 2 bytes
uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
tot += sizeof(uint16_t);
// Decode neighbor Id
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
// Go through the messages until there's nothing else to read
uint16_t unMsgSize = 0;
// Obtain Buzz messages push it into queue
do
{
// Get payload size
unMsgSize = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
// Append message to the Buzz input message queue
if (unMsgSize > 0 && unMsgSize <= size - tot)
{
buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize));
tot += unMsgSize;
}
} while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
free(first_INmsg);
IN_MSG.erase(IN_MSG.begin());
}
// Process messages VM call*
buzzvm_process_inmsgs(VM);
}
uint64_t* obt_out_msg()
/*
/ Obtains messages from buzz out message Queue
-------------------------------------------------*/
{
// Process out messages
buzzvm_process_outmsgs(VM);
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
// Taking into consideration the sizes included at the end
ssize_t tot = sizeof(uint16_t);
// Send robot id
*(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot;
tot += sizeof(uint16_t);
// Send messages from FIFO
do
{
// Are there more messages?
if (buzzoutmsg_queue_isempty(VM))
break;
// Get first message
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
// Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes
// DEBUG
// ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE)
{
buzzmsg_payload_destroy(&m);
break;
}
// Add message length to data buffer
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t);
// Add payload to data buffer
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m);
// Get rid of message
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while (1);
uint16_t total_size = (ceil((float)tot / (float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t)total_size;
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
free(buff_send);
// DEBUG
// for(int i=0;i<total_size;i++){
// cout<<" payload from out msg "<<*(payload_64+i)<<endl;
//}
// Send message
return payload_64;
}
static const char* buzz_error_info()
/*
/ Buzz script not able to load
---------------------------------*/
{
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg;
if (dbg != NULL)
{
asprintf(&msg, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", BO_FNAME, dbg->fname,
dbg->line, dbg->col, VM->errormsg);
}
else
{
asprintf(&msg, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", BO_FNAME, VM->pc, VM->errormsg);
}
return msg;
}
static int buzz_register_hooks()
/*
/ Buzz hooks that can be used inside .bzz file
------------------------------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap));
buzzvm_gstore(VM);
return VM->state;
}
static int testing_buzz_register_hooks()
/*
/ Register dummy Buzz hooks for test during update
---------------------------------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
return VM->state;
}
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
/*
/ Sets the .bzz and .bdbg file
---------------------------------*/
{
ROS_INFO(" Robot ID: %i", robot_id);
Robot_id = robot_id;
// Read bytecode from file and fill the bo buffer
FILE* fd = fopen(bo_filename, "rb");
if (!fd)
{
perror(bo_filename);
return 0;
}
fseek(fd, 0, SEEK_END);
size_t bcode_size = ftell(fd);
rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size);
if (fread(BO_BUF, 1, bcode_size, fd) < bcode_size)
{
perror(bo_filename);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fclose(fd);
return 0;
}
fclose(fd);
// Save bytecode file name
BO_FNAME = strdup(bo_filename);
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
/*
/ Sets a new update
-----------------------*/
{
// Reset the Buzz VM
if (VM)
buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
// Get rid of debug info
if (DBG_INFO)
buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
// Read debug information
if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename))
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
// Set byte code
if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id);
return 0;
}
// Register hook functions
if (buzz_register_hooks() != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
return 0;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
// Execute the global part of the script
if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE)
{
ROS_ERROR("Error executing global part, VM state : %i", VM->state);
return 0;
}
// Call the Init() function
if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK
return 1;
}
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
/*
/ Performs a initialization test
-----------------------------------*/
{
// Reset the Buzz VM
if (VM)
buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id);
// Get rid of debug info
if (DBG_INFO)
buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
// Read debug information
if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename))
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename);
return 0;
}
// Set byte code
if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id);
return 0;
}
// Register hook functions
if (testing_buzz_register_hooks() != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
return 0;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
// Execute the global part of the script
if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE)
{
ROS_ERROR("Error executing global part, VM state : %i", VM->state);
return 0;
}
// Call the Init() function
if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK
return 1;
}
struct buzzswarm_elem_s
/*
/ Swarm struct
----------------*/
{
buzzdarray_t swarms;
uint16_t age;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void update_sensors()
/*
/ Update from all external inputs
-------------------------------*/
{
// Update sensors
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
}
void buzz_script_step()
/*
/ Step through the buzz script
-------------------------------*/
{
// Process available messages
in_message_process();
// Update sensors
update_sensors();
// Call Buzz step() function
if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
buzzvm_dump(VM);
}
}
void buzz_script_destroy()
/*
/ Destroy the bvm and other related ressources
-------------------------------------*/
{
if (VM)
{
if (VM->state != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
buzzvm_dump(VM);
}
buzzvm_function_call(VM, "destroy", 0);
buzzvm_destroy(&VM);
free(BO_FNAME);
buzzdebug_destroy(&DBG_INFO);
}
ROS_INFO("Script execution stopped.");
}
int buzz_script_done()
/*
/ Check if the BVM execution terminated
---------------------------------------*/
{
return VM->state != BUZZVM_STATE_READY;
}
int update_step_test()
/*
/ Step test for the update mechanism
------------------------------------*/
{
// Process available messages
in_message_process();
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
int a = buzzvm_function_call(VM, "step", 0);
if (a != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info());
fprintf(stdout, "step test VM state %i\n", a);
}
return a == BUZZVM_STATE_READY;
}
void set_robot_var(int ROBOTS)
/*
/ set swarm size in the BVM
-----------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, ROBOTS);
buzzvm_gstore(VM);
}
int get_inmsg_size()
/*
/ get the incoming msgs size
------------------------------*/
{
return IN_MSG.size();
}
std::vector<uint8_t*> get_inmsg_vector(){
return IN_MSG;
}
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
string get_bvmstate()
/*
/ return current BVM state
---------------------------------------*/
{
std::string uav_state = "Unknown";
if(VM && VM->strings){
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_STRING)
uav_state = string(obj->s.value.str);
else
uav_state = "Not Available";
buzzvm_pop(VM);
}
return uav_state;
}
int get_swarmsize() {
return (int)buzzdict_size(VM->swarmmembers) + 1;
}
int get_timesync_state()
/*
/ return time sync state from bzz script
--------------------------------------*/
{
int timesync_state = 0;
if(VM){
buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value;
buzzvm_pop(VM);
}
return timesync_state;
}
int get_timesync_itr()
/*
/ return time sync iteration from bzz script
--------------------------------------*/
{
int timesync_itr = 0;
if(VM){
buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value;
buzzvm_pop(VM);
}
return timesync_itr;
}
}

859
src/buzzuav_closures.cpp Normal file
View File

@ -0,0 +1,859 @@
/** @file buzzuav_closures.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzzuav_closures.h"
#include "math.h"
namespace buzzuav_closures
{
// TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr;
static double goto_pos[4];
static double rc_goto_pos[3];
static float rc_gimbal[4];
static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 };
static double cur_pos[4];
static double cur_NEDpos[2];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd = 0;
static int rc_id = -1;
static int buzz_cmd = 0;
static float height = 0;
static bool deque_full = false;
static int rssi = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
std::map<int, buzz_utility::RB_struct> targets_map;
std::map<int, buzz_utility::RB_struct> wplist_map;
std::map<int, buzz_utility::Pos_struct> neighbors_map;
std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
std::map<int, std::map<int,int>> grid;
/****************************************/
/****************************************/
int buzzros_print(buzzvm_t vm)
/*
/ Buzz closure to print out
----------------------------------------------------------- */
{
std::ostringstream buffer(std::ostringstream::ate);
buffer << "[" << buzz_utility::get_robotid() << "] ";
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{
buzzvm_lload(vm, index);
buzzobj_t o = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
switch (o->o.type)
{
case BUZZTYPE_NIL:
buffer << " BUZZ - [nil]";
break;
case BUZZTYPE_INT:
buffer << " " << o->i.value;
break;
case BUZZTYPE_FLOAT:
buffer << " " << o->f.value;
break;
case BUZZTYPE_TABLE:
buffer << " [table with " << buzzdict_size(o->t.value) << " elems]";
break;
case BUZZTYPE_CLOSURE:
if (o->c.value.isnative)
{
buffer << " [n-closure @" << o->c.value.ref << "]";
}
else
{
buffer << " [c-closure @" << o->c.value.ref << "]";
}
break;
case BUZZTYPE_STRING:
buffer << " " << o->s.value.str;
break;
case BUZZTYPE_USERDATA:
buffer << " [userdata @" << o->u.value << "]";
break;
default:
break;
}
}
ROS_INFO("%s", buffer.str().c_str());
return buzzvm_ret0(vm);
}
void setWPlist(string path)
/*
/ set the absolute path for a csv list of waypoints
----------------------------------------------------------- */
{
WPlistname = path + "include/taskallocate/waypointlist.csv";
}
float constrainAngle(float x)
/*
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x + M_PI, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x - M_PI;
}
void rb_from_gps(double nei[], double out[], double cur[])
/*
/ Compute Range and Bearing from 2 GPS set of coordinates
/----------------------------------------*/
{
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
out[1] = constrainAngle(atan2(ned_y, ned_x));
out[2] = 0.0;
}
void parse_gpslist()
/*
/ parse a csv of GPS targets
/----------------------------------------*/
{
// Open file:
ROS_INFO("WP list file: %s", WPlistname.c_str());
std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
// Opening may fail, always check.
if (!fin)
{
ROS_ERROR("GPS list parser, could not open file.");
return;
}
// Prepare a C-string buffer to be used when reading lines.
const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need.
char buffer[MAX_LINE_LENGTH] = {};
const char* DELIMS = "\t ,"; // Tab, space or comma.
// Read the file and load the data:
buzz_utility::RB_struct RB_arr;
// Read one line at a time.
while (fin.getline(buffer, MAX_LINE_LENGTH))
{
// Extract the tokens:
int tid = atoi(strtok(buffer, DELIMS));
double lon = atof(strtok(NULL, DELIMS));
double lat = atof(strtok(NULL, DELIMS));
int alt = atoi(strtok(NULL, DELIMS));
// int tilt = atoi(strtok(NULL, DELIMS));
// DEBUG
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude = lat;
RB_arr.longitude = lon;
RB_arr.altitude = alt;
// Insert elements.
map<int, buzz_utility::RB_struct>::iterator it = wplist_map.find(tid);
if (it != wplist_map.end())
wplist_map.erase(it);
wplist_map.insert(make_pair(tid, RB_arr));
}
ROS_INFO("----->Saved %i waypoints.", wplist_map.size());
// Close the file:
fin.close();
}
int buzz_exportmap(buzzvm_t vm)
/*
/ Buzz closure to export a 2D map
/----------------------------------------*/
{
grid.clear();
buzzvm_lnum_assert(vm, 1);
// Get the parameter
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) {
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
std::map<int, int> row;
for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
buzzvm_dup(vm);
buzzvm_pushi(vm, j);
buzzvm_tget(vm);
row.insert(std::pair<int,int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
buzzvm_pop(vm);
}
grid.insert(std::pair<int,std::map<int, int>>(i,row));
buzzvm_pop(vm);
}
// DEBUG
// ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value));
return buzzvm_ret0(vm);
}
int buzzuav_moveto(buzzvm_t vm)
/*
/ Buzz closure to move following a 3D vector + Yaw
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // dx
buzzvm_lload(vm, 2); // dy
buzzvm_lload(vm, 3); // dheight
buzzvm_lload(vm, 4); // dyaw
buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dyaw = buzzvm_stack_at(vm, 1)->f.value;
float dh = buzzvm_stack_at(vm, 2)->f.value;
float dy = buzzvm_stack_at(vm, 3)->f.value;
float dx = buzzvm_stack_at(vm, 4)->f.value;
goto_pos[0] = dx;
goto_pos[1] = dy;
goto_pos[2] = height + dh;
goto_pos[3] = dyaw;
// DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros?
return buzzvm_ret0(vm);
}
int buzzuav_addtargetRB(buzzvm_t vm)
/*
/ Buzz closure to add a target (goal) GPS
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // longitude
buzzvm_lload(vm, 2); // latitude
buzzvm_lload(vm, 3); // id
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
double tmp[3];
tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
tmp[2] = 0.0;
int uid = buzzvm_stack_at(vm, 3)->i.value;
double rb[3];
rb_from_gps(tmp, rb, cur_pos);
if (fabs(rb[0]) < 100.0)
{
buzz_utility::RB_struct RB_arr;
RB_arr.latitude = tmp[0];
RB_arr.longitude = tmp[1];
RB_arr.altitude = tmp[2];
RB_arr.r = rb[0];
RB_arr.b = rb[1];
map<int, buzz_utility::RB_struct>::iterator it = targets_map.find(uid);
if (it != targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(uid, RB_arr));
// DEBUG
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
}
else
ROS_WARN(" ---------- Target too far %f", rb[0]);
return 0;
}
int buzzuav_addNeiStatus(buzzvm_t vm)
/*
/ closure to add neighbors status to the BVM
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee
buzzvm_lload(vm, 3); // batt
buzzvm_lload(vm, 4); // gps
buzzvm_lload(vm, 5); // id
buzzvm_type_assert(vm, 5, BUZZTYPE_INT);
buzzvm_type_assert(vm, 4, BUZZTYPE_INT);
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzz_utility::neighbors_status newRS;
uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value;
newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value;
newRS.xbee = buzzvm_stack_at(vm, 2)->i.value;
newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
if (it != neighbors_status_map.end())
neighbors_status_map.erase(it);
neighbors_status_map.insert(make_pair(id, newRS));
return vm->state;
}
mavros_msgs::Mavlink get_status()
/*
/ return neighbors status from BVM
/----------------------------------------*/
{
mavros_msgs::Mavlink payload_out;
map<int, buzz_utility::neighbors_status>::iterator it;
for (it = neighbors_status_map.begin(); it != neighbors_status_map.end(); ++it)
{
payload_out.payload64.push_back(it->first);
payload_out.payload64.push_back(it->second.gps_strenght);
payload_out.payload64.push_back(it->second.batt_lvl);
payload_out.payload64.push_back(it->second.xbee);
payload_out.payload64.push_back(it->second.flight_status);
}
// Add Robot id and message number to the published message
payload_out.sysid = (uint8_t)neighbors_status_map.size();
return payload_out;
}
int buzzuav_takepicture(buzzvm_t vm)
/*
/ Buzz closure to take a picture here.
/----------------------------------------*/
{
buzz_cmd = IMAGE_START_CAPTURE;
return buzzvm_ret0(vm);
}
int buzzuav_setgimbal(buzzvm_t vm)
/*
/ Buzz closure to change locally the gimbal orientation
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time
buzzvm_lload(vm, 2); // pitch
buzzvm_lload(vm, 3); // roll
buzzvm_lload(vm, 4); // yaw
buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value;
rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value;
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
buzz_cmd = DO_MOUNT_CONTROL;
return buzzvm_ret0(vm);
}
int buzzuav_storegoal(buzzvm_t vm)
/*
/ Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude
buzzvm_lload(vm, 2); // longitude
buzzvm_lload(vm, 3); // latitude
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
double goal[3];
goal[0] = buzzvm_stack_at(vm, 3)->f.value;
goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1)
{
if (wplist_map.size() <= 0)
parse_gpslist();
goal[0] = wplist_map.begin()->second.latitude;
goal[1] = wplist_map.begin()->second.longitude;
goal[2] = wplist_map.begin()->second.altitude;
wplist_map.erase(wplist_map.begin()->first);
}
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0) {
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
}
return buzzvm_ret0(vm);
}
int buzzuav_arm(buzzvm_t vm)
/*
/ Buzz closure to arm
/---------------------------------------*/
{
cur_cmd = COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
int buzzuav_disarm(buzzvm_t vm)
/*
/ Buzz closure to disarm
/---------------------------------------*/
{
cur_cmd = COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
int buzzuav_takeoff(buzzvm_t vm)
/*
/ Buzz closure to takeoff
/---------------------------------------*/
{
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value;
height = goto_pos[2];
cur_cmd = NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n");
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
int buzzuav_land(buzzvm_t vm)
/*
/ Buzz closure to land
/-------------------------------------------------------------*/
{
cur_cmd = NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
int buzzuav_gohome(buzzvm_t vm)
/*
/ Buzz closure to return Home
/-------------------------------------------------------------*/
{
cur_cmd = NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
double* getgoto()
/*
/ return the GPS goal
/-------------------------------------------------------------*/
{
return goto_pos;
}
std::map<int, std::map<int,int>> getgrid()
/*
/ return the grid
/-------------------------------------------------------------*/
{
return grid;
}
float* getgimbal()
/*
/ return current gimbal commands
---------------------------------------*/
{
return rc_gimbal;
}
int getcmd()
/*
/ return current mavros command to the FC
---------------------------------------*/
{
return cur_cmd;
}
int bzz_cmd()
/*
/ return and clean the custom command from Buzz to the FC
----------------------------------------------------------*/
{
int cmd = buzz_cmd;
buzz_cmd = 0;
return cmd;
}
void rc_set_goto(int id, double latitude, double longitude, double altitude)
/*
/ update interface RC GPS goal input
-----------------------------------*/
{
rc_id = id;
rc_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude;
rc_goto_pos[2] = altitude;
}
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
/*
/ update interface RC gimbal control input
-----------------------------------*/
{
rc_id = id;
rc_gimbal[0] = yaw;
rc_gimbal[1] = roll;
rc_gimbal[2] = pitch;
rc_gimbal[3] = t;
}
void rc_call(int rc_cmd_in)
/*
/ update interface RC command input
-----------------------------------*/
{
rc_cmd = rc_cmd_in;
}
void set_obstacle_dist(float dist[])
/*
/ update interface proximity value array
-----------------------------------*/
{
for (int i = 0; i < 5; i++)
obst[i] = dist[i];
}
void set_battery(float voltage, float current, float remaining)
/*
/ update interface battery value array
-----------------------------------*/
{
batt[0] = voltage;
batt[1] = current;
batt[2] = remaining;
}
int buzzuav_update_battery(buzzvm_t vm)
/*
/ update BVM battery table
-----------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1));
buzzvm_pushf(vm, batt[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1));
buzzvm_pushf(vm, batt[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
buzzvm_pushi(vm, (int)batt[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
/*
/ Set of function to update interface variable of xbee network status
----------------------------------------------------------------------*/
void set_deque_full(bool state)
{
deque_full = state;
}
void set_rssi(float value)
{
rssi = round(value);
}
void set_raw_packet_loss(float value)
{
raw_packet_loss = value;
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = round(100 * value);
}
/*void set_api_rssi(float value)
{
api_rssi = value;
}*/
int buzzuav_update_xbee_status(buzzvm_t vm)
/*
/ update BVM xbee_status table
-----------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1));
buzzvm_pushi(vm, static_cast<uint8_t>(deque_full));
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
buzzvm_pushi(vm, rssi);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
buzzvm_pushf(vm, raw_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
buzzvm_pushi(vm, filtered_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));
buzzvm_pushf(vm, api_rssi);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
void set_currentNEDpos(double x, double y)
/*
/ update interface position array
-----------------------------------*/
{
cur_NEDpos[0] = x;
cur_NEDpos[1] = y;
}
void set_currentpos(double latitude, double longitude, float altitude, float yaw)
/*
/ update interface position array
-----------------------------------*/
{
cur_pos[0] = latitude;
cur_pos[1] = longitude;
cur_pos[2] = altitude;
cur_pos[3] = yaw;
}
// adds neighbours position
void neighbour_pos_callback(int id, float range, float bearing, float elevation)
{
buzz_utility::Pos_struct pos_arr;
pos_arr.x = range;
pos_arr.y = bearing;
pos_arr.z = elevation;
map<int, buzz_utility::Pos_struct>::iterator it = neighbors_map.find(id);
if (it != neighbors_map.end())
neighbors_map.erase(it);
neighbors_map.insert(make_pair(id, pos_arr));
}
// update at each step the VM table
void update_neighbors(buzzvm_t vm)
{
// Reset neighbor information
buzzneighbors_reset(vm);
// Get robot id and update neighbor information
map<int, buzz_utility::Pos_struct>::iterator it;
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
{
buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z);
}
}
// Clear neighbours pos
void clear_neighbours_pos(){
neighbors_map.clear();
}
int buzzuav_update_currentpos(buzzvm_t vm)
/*
/ Update the BVM position table
/------------------------------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "pose", 1));
buzzvm_pusht(vm);
buzzobj_t tPoseTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm);
// Create table for i-th read
buzzvm_pusht(vm);
buzzobj_t tPosition = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
// Fill in the read
buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 0));
buzzvm_pushf(vm, cur_pos[0]);
buzzvm_tput(vm);
buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 0));
buzzvm_pushf(vm, cur_pos[1]);
buzzvm_tput(vm);
buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0));
buzzvm_pushf(vm, cur_pos[2]);
buzzvm_tput(vm);
buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 0));
buzzvm_pushf(vm, cur_NEDpos[0]);
buzzvm_tput(vm);
buzzvm_push(vm, tPosition);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 0));
buzzvm_pushf(vm, cur_NEDpos[1]);
buzzvm_tput(vm);
// Store read table in the proximity table
buzzvm_push(vm, tPoseTable);
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0));
buzzvm_push(vm, tPosition);
buzzvm_tput(vm);
// Create table for i-th read
buzzvm_pusht(vm);
buzzobj_t tOrientation = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
// Fill in the read
buzzvm_push(vm, tOrientation);
buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 0));
buzzvm_pushf(vm, cur_pos[3]);
buzzvm_tput(vm);
// Store read table in the proximity table
buzzvm_push(vm, tPoseTable);
buzzvm_pushs(vm, buzzvm_string_register(vm, "orientation", 0));
buzzvm_push(vm, tOrientation);
buzzvm_tput(vm);
return vm->state;
}
void flight_status_update(uint8_t state)
/*
/ Update the interface status variable
/------------------------------------------------------*/
{
status = state;
}
int buzzuav_update_flight_status(buzzvm_t vm)
/*
/ Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot
/------------------------------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm);
buzzvm_dup(vm);
rc_cmd = 0;
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_gstore(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
buzzvm_pushi(vm, rc_id);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, rc_goto_pos[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, rc_goto_pos[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
int buzzuav_update_prox(buzzvm_t vm)
/*
/ Create an obstacle Buzz table from proximity sensors
/ Acessing proximity in buzz script
/ proximity[0].angle and proximity[0].value - front
/ "" "" "" - right and back
/ proximity[3].angle and proximity[3].value - left
/ proximity[4].angle = -1 and proximity[4].value -bottom
-------------------------------------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm);
// Fill into the proximity table
buzzobj_t tProxRead;
float angle = 0;
for (size_t i = 0; i < 4; ++i)
{
// Create table for i-th read
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
// Fill in the read
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i + 1]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
// Store read table in the proximity table
buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
angle += 1.5708;
}
// Create table for bottom read
angle = -1;
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
// Fill in the read
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
// Store read table in the proximity table
buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
return vm->state;
}
int dummy_closure(buzzvm_t vm)
/*
/ Dummy closure for use during update testing
----------------------------------------------------*/
{
return buzzvm_ret0(vm);
}
}

24
src/rosbuzz.cpp Normal file
View File

@ -0,0 +1,24 @@
/** @file rosbuzz.cpp
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
#include "roscontroller.h"
int main(int argc, char** argv)
/*
/ This program implements Buzz in a ROS node using mavros_msgs.
-----------------------------------------------------------------*/
{
// Initialize rosBuzz node
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbuzz_node::roscontroller RosController(nh, nh_priv);
RosController.RosControllerRun();
return 0;
}

1517
src/roscontroller.cpp Normal file
View File

File diff suppressed because it is too large Load Diff