changes to the updater
This commit is contained in:
vivek-shankar
parent
b871c2e25a
commit
664e7e4dc0
|
|
@ -129,6 +129,8 @@ int get_update_status();
|
|||
|
||||
void set_read_update_status();
|
||||
|
||||
int compile_bzz();
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@ if(r. data < l. data or (r. data == l. data )) return l
|
|||
else return r
|
||||
})
|
||||
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
}
|
||||
|
||||
function stand_by(){
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
|
||||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
|
|
|
|||
|
|
@ -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 ls = string.length(s)
|
||||
var lm = string.length(m)
|
||||
var i = 0
|
||||
while(i < ls-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 ls = string.length(s)
|
||||
var lm = string.length(m)
|
||||
var i = ls - 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 ls = string.length(s)
|
||||
var ld = string.length(d)
|
||||
# Go through string s
|
||||
while(i2 < ls) {
|
||||
# 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;
|
||||
}
|
||||
|
|
@ -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.atan2(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
|
||||
}
|
||||
|
|
@ -1,4 +1,5 @@
|
|||
# We need this for 2D vectors
|
||||
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
include "vec2.bzz"
|
||||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
|
|
@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
|
|||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
TARGET = 12.0 #0.000001001
|
||||
EPSILON = 3.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
|
|
@ -26,7 +26,7 @@ function lj_magnitude(dist, target, epsilon) {
|
|||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
}cd sr
|
||||
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
|
|
@ -48,9 +48,12 @@ function hexagon() {
|
|||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.06,0.0)
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# uav_moveto(0.0,0.06)
|
||||
# }
|
||||
}
|
||||
|
||||
|
|
@ -142,11 +145,32 @@ function land() {
|
|||
}
|
||||
}
|
||||
|
||||
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)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_print(t) {
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(u, tab) {
|
||||
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
|
||||
vt = stigmergy.create(5)
|
||||
t = {}
|
||||
vt.put("p",t)
|
||||
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
|
@ -197,6 +221,14 @@ neighbors.listen("cmd",
|
|||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Check local users and push to v.stig
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
|
||||
# Save locally the users and print RG
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
include "vec2.bzz"
|
||||
####################################################################################################
|
||||
# Updater related
|
||||
# This should be here for the updater to work, changing position of code will crash the updater
|
||||
|
|
@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
|
|||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
TARGET = 12.0 #0.000001001
|
||||
EPSILON = 3.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
|
|
@ -48,9 +48,12 @@ function hexagon() {
|
|||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.06,0.0)
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# uav_moveto(0.0,0.06)
|
||||
# }
|
||||
}
|
||||
|
||||
|
|
@ -142,11 +145,32 @@ function land() {
|
|||
}
|
||||
}
|
||||
|
||||
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)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_print(t) {
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(u, tab) {
|
||||
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
|
||||
vt = stigmergy.create(5)
|
||||
t = {}
|
||||
vt.put("p",t)
|
||||
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
|
@ -197,6 +221,14 @@ neighbors.listen("cmd",
|
|||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Check local users and push to v.stig
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
|
||||
# Save locally the users and print RG
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -1,208 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
# 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
|
||||
function updated_neigh(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
||||
TARGET_ALTITUDE = 3.0
|
||||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
}
|
||||
|
||||
# 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 hexagon() {
|
||||
statef=hexagon
|
||||
CURSTATE = "HEXAGON"
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
# Move according to vector
|
||||
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
|
||||
uav_moveto(accum.x,accum.y)
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# BARRIER-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_VSTIG = 1
|
||||
# ROBOTS = 3 # number of robots in the swarm
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_set(threshold, transf) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf);
|
||||
}
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready() {
|
||||
barrier.put(id, 1)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
WAIT_TIMEOUT = 200
|
||||
timeW=0
|
||||
function barrier_wait(threshold, transf) {
|
||||
barrier.get(id)
|
||||
CURSTATE = "BARRIERWAIT"
|
||||
if(barrier.size() >= threshold) {
|
||||
barrier = nil
|
||||
transf()
|
||||
} else if(timeW>=WAIT_TIMEOUT) {
|
||||
barrier = nil
|
||||
statef=land
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# flight status
|
||||
|
||||
function idle() {
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
CURSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
log("TakeOff: ", flight.status)
|
||||
log("Relative position: ", position.altitude)
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,hexagon)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
function land() {
|
||||
CURSTATE = "LAND"
|
||||
statef=land
|
||||
log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
else {
|
||||
timeW=0
|
||||
barrier = nil
|
||||
statef=idle
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef = takeoff
|
||||
CURSTATE = "TAKEOFF"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef = land
|
||||
CURSTATE = "LAND"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
statef = idle
|
||||
uav_goto()
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
}
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22 and CURSTATE=="IDLE") {
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
} else if(value==400 and CURSTATE=="IDLE") {
|
||||
uav_arm()
|
||||
} else if(value==401 and CURSTATE=="IDLE"){
|
||||
uav_disarm()
|
||||
}
|
||||
}
|
||||
|
||||
)
|
||||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -1,208 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
# 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
|
||||
function updated_neigh(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
||||
TARGET_ALTITUDE = 3.0
|
||||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
}
|
||||
|
||||
# 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 hexagon() {
|
||||
statef=hexagon
|
||||
CURSTATE = "HEXAGON"
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
# Move according to vector
|
||||
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
|
||||
uav_moveto(accum.x,accum.y)
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# BARRIER-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_VSTIG = 1
|
||||
# ROBOTS = 3 # number of robots in the swarm
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_set(threshold, transf) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf);
|
||||
}
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready() {
|
||||
barrier.put(id, 1)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
WAIT_TIMEOUT = 200
|
||||
timeW=0
|
||||
function barrier_wait(threshold, transf) {
|
||||
barrier.get(id)
|
||||
CURSTATE = "BARRIERWAIT"
|
||||
if(barrier.size() >= threshold) {
|
||||
barrier = nil
|
||||
transf()
|
||||
} else if(timeW>=WAIT_TIMEOUT) {
|
||||
barrier = nil
|
||||
statef=land
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# flight status
|
||||
|
||||
function idle() {
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
CURSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
log("TakeOff: ", flight.status)
|
||||
log("Relative position: ", position.altitude)
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,hexagon)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
function land() {
|
||||
CURSTATE = "LAND"
|
||||
statef=land
|
||||
log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
else {
|
||||
timeW=0
|
||||
barrier = nil
|
||||
statef=idle
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef = takeoff
|
||||
CURSTATE = "TAKEOFF"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef = land
|
||||
CURSTATE = "LAND"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
statef = idle
|
||||
uav_goto()
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
}
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22 and CURSTATE=="IDLE") {
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
} else if(value==400 and CURSTATE=="IDLE") {
|
||||
uav_arm()
|
||||
} else if(value==401 and CURSTATE=="IDLE"){
|
||||
uav_disarm()
|
||||
}
|
||||
}
|
||||
|
||||
)
|
||||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -1,208 +0,0 @@
|
|||
# We need this for 2D vectors
|
||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||
####################################################################################################
|
||||
# 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
|
||||
function updated_neigh(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
||||
TARGET_ALTITUDE = 3.0
|
||||
CURSTATE = "TURNEDOFF"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 10.0 #0.000001001
|
||||
EPSILON = 18.0 #0.001
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
}
|
||||
|
||||
# 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 hexagon() {
|
||||
statef=hexagon
|
||||
CURSTATE = "HEXAGON"
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
# Move according to vector
|
||||
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
|
||||
uav_moveto(accum.x,accum.y)
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else {
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# BARRIER-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_VSTIG = 1
|
||||
# ROBOTS = 3 # number of robots in the swarm
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_set(threshold, transf) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf);
|
||||
}
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready() {
|
||||
barrier.put(id, 1)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
WAIT_TIMEOUT = 200
|
||||
timeW=0
|
||||
function barrier_wait(threshold, transf) {
|
||||
barrier.get(id)
|
||||
CURSTATE = "BARRIERWAIT"
|
||||
if(barrier.size() >= threshold) {
|
||||
barrier = nil
|
||||
transf()
|
||||
} else if(timeW>=WAIT_TIMEOUT) {
|
||||
barrier = nil
|
||||
statef=land
|
||||
timeW=0
|
||||
}
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
# flight status
|
||||
|
||||
function idle() {
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
|
||||
}
|
||||
|
||||
function takeoff() {
|
||||
CURSTATE = "TAKEOFF"
|
||||
statef=takeoff
|
||||
log("TakeOff: ", flight.status)
|
||||
log("Relative position: ", position.altitude)
|
||||
|
||||
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS,hexagon)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
}
|
||||
function land() {
|
||||
CURSTATE = "LAND"
|
||||
statef=land
|
||||
log("Land: ", flight.status)
|
||||
if(flight.status == 2 or flight.status == 3){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
else {
|
||||
timeW=0
|
||||
barrier = nil
|
||||
statef=idle
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
s = swarm.create(1)
|
||||
# s.select(1)
|
||||
s.join()
|
||||
statef=idle
|
||||
CURSTATE = "IDLE"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
statef = takeoff
|
||||
CURSTATE = "TAKEOFF"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
log("To land")
|
||||
flight.rc_cmd=0
|
||||
statef = land
|
||||
CURSTATE = "LAND"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
statef = idle
|
||||
uav_goto()
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
}
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
if(value==22 and CURSTATE=="IDLE") {
|
||||
statef=takeoff
|
||||
} else if(value==21) {
|
||||
statef=land
|
||||
} else if(value==400 and CURSTATE=="IDLE") {
|
||||
uav_arm()
|
||||
} else if(value==401 and CURSTATE=="IDLE"){
|
||||
uav_disarm()
|
||||
}
|
||||
}
|
||||
|
||||
)
|
||||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -29,7 +29,7 @@ static int updated=0;
|
|||
|
||||
/*Initialize updater*/
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
||||
fprintf(stdout,"intiialized file monitor.\n");
|
||||
ROS_INFO("intiialized file monitor.\n");
|
||||
fd=inotify_init1(IN_NONBLOCK);
|
||||
if ( fd < 0 ) {
|
||||
perror( "inotify_init error" );
|
||||
|
|
@ -48,7 +48,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
|||
BO_BUF = (uint8_t*)malloc(bcode_size);
|
||||
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
|
||||
perror(bo_filename);
|
||||
fclose(fp);
|
||||
//fclose(fp);
|
||||
//return 0;
|
||||
}
|
||||
fclose(fp);
|
||||
|
|
@ -65,7 +65,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
|||
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);
|
||||
//fclose(fp);
|
||||
//return 0;
|
||||
}
|
||||
fclose(fp);
|
||||
|
|
@ -147,7 +147,7 @@ void code_message_outqueue_append(){
|
|||
|
||||
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
|
||||
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
|
||||
fprintf(stdout,"in ms append code size %d\n", (int) size);
|
||||
//ROS_INFO("[DEBUG] Updater append code of size %d\n", (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);
|
||||
|
|
@ -156,9 +156,9 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
|
|||
|
||||
void code_message_inqueue_process(){
|
||||
int size=0;
|
||||
fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) );
|
||||
fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
|
||||
fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
|
||||
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) );
|
||||
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
|
||||
ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
|
||||
|
||||
if( *(int*) (updater->mode) == CODE_RUNNING){
|
||||
//fprintf(stdout,"[debug]Inside inmsg code running");
|
||||
|
|
@ -191,7 +191,6 @@ void update_routine(const char* bcfname,
|
|||
const char* dbgfname){
|
||||
dbgf_name=(char*)dbgfname;
|
||||
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);
|
||||
|
|
@ -199,37 +198,13 @@ void update_routine(const char* bcfname,
|
|||
if(*(int*)updater->mode==CODE_RUNNING){
|
||||
buzzvm_function_call(VM, "updated_neigh", 0);
|
||||
if(check_update()){
|
||||
std::string bzzfile_name(bzz_file);
|
||||
stringstream bzzfile_in_compile;
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
|
||||
bzzfile_in_compile<<path<<"/";
|
||||
path = bzzfile_in_compile.str();
|
||||
bzzfile_in_compile.str("");
|
||||
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name = name.substr(0,name.find_last_of("."));
|
||||
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
|
||||
FILE *fp;
|
||||
int comp=0;
|
||||
char buf[128];
|
||||
fprintf(stdout,"Update found \nUpdating script ...\n");
|
||||
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
|
||||
fprintf(stdout,"Error opening pipe!\n");
|
||||
}
|
||||
while (fgets(buf, 128, fp) != NULL) {
|
||||
fprintf(stdout,"OUTPUT: %s \n", buf);
|
||||
comp=1;
|
||||
|
||||
|
||||
ROS_INFO("Update found \nUpdating script ...\n");
|
||||
|
||||
if(compile_bzz()){
|
||||
ROS_WARN("Errors in comipilg script so staying on old script\n");
|
||||
}
|
||||
bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
|
||||
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
|
||||
fprintf(stdout,"Error opening pipe!\n");
|
||||
}
|
||||
while (fgets(buf, 128, fp) != NULL) {
|
||||
fprintf(stdout,"OUTPUT: %s \n", buf);
|
||||
}
|
||||
if(pclose(fp) || comp) {
|
||||
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
|
||||
}
|
||||
else {
|
||||
uint8_t* BO_BUF = 0;
|
||||
FILE* fp = fopen(bcfname, "rb"); // to change file edit
|
||||
|
|
@ -250,12 +225,12 @@ void update_routine(const char* bcfname,
|
|||
*(uint16_t*)(updater->update_no) =update_no +1;
|
||||
code_message_outqueue_append();
|
||||
VM = buzz_utility::get_vm();
|
||||
fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no));
|
||||
ROS_INFO("Current Update no %d\n", *(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;
|
||||
fprintf(stdout,"Sending code... \n");
|
||||
ROS_INFO("Sending code... \n");
|
||||
code_message_outqueue_append();
|
||||
}
|
||||
delete_p(BO_BUF);
|
||||
|
|
@ -268,7 +243,7 @@ void update_routine(const char* bcfname,
|
|||
else{
|
||||
//gettimeofday(&t1, NULL);
|
||||
if(neigh==0 && (!is_msg_present())){
|
||||
fprintf(stdout,"Sending code... \n");
|
||||
ROS_INFO("Sending code... \n");
|
||||
code_message_outqueue_append();
|
||||
|
||||
}
|
||||
|
|
@ -277,7 +252,7 @@ void update_routine(const char* bcfname,
|
|||
buzzvm_gload(VM);
|
||||
buzzobj_t tObj = buzzvm_stack_at(VM, 1);
|
||||
buzzvm_pop(VM);
|
||||
fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value);
|
||||
ROS_INFO("Barrier ..................... %i \n",tObj->i.value);
|
||||
if(tObj->i.value==no_of_robot) {
|
||||
*(int*)(updater->mode) = CODE_RUNNING;
|
||||
gettimeofday(&t2, NULL);
|
||||
|
|
@ -307,12 +282,12 @@ 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)){
|
||||
fprintf(stdout,"Initializtion of script test passed\n");
|
||||
ROS_WARN("Initializtion of script test passed\n");
|
||||
if(buzz_utility::update_step_test()){
|
||||
/*data logging*/
|
||||
//start =1;
|
||||
/*data logging*/
|
||||
fprintf(stdout,"Step test passed\n");
|
||||
ROS_WARN("Step test passed\n");
|
||||
*(int*) (updater->mode) = CODE_STANDBY;
|
||||
//fprintf(stdout,"updater value = %i\n",updater->mode);
|
||||
delete_p(updater->bcode);
|
||||
|
|
@ -330,12 +305,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
|
|||
/*Unable to step something wrong*/
|
||||
else{
|
||||
if(*(int*) (updater->mode) == CODE_RUNNING){
|
||||
fprintf(stdout,"step test failed, stick to old script\n");
|
||||
ROS_ERROR("step test failed, stick to old script\n");
|
||||
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size));
|
||||
}
|
||||
else{
|
||||
/*You will never reach here*/
|
||||
fprintf(stdout,"step test failed, Return to stand by\n");
|
||||
ROS_ERROR("step test failed, Return to stand by\n");
|
||||
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();
|
||||
|
|
@ -350,12 +325,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
|
|||
}
|
||||
else {
|
||||
if(*(int*) (updater->mode) == CODE_RUNNING){
|
||||
fprintf(stdout,"Initialization test failed, stick to old script\n");
|
||||
ROS_ERROR("Initialization test failed, stick to old script\n");
|
||||
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size));
|
||||
}
|
||||
else{
|
||||
/*You will never reach here*/
|
||||
fprintf(stdout,"Initialization test failed, Return to stand by\n");
|
||||
ROS_ERROR("Initialization test failed, Return to stand by\n");
|
||||
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();
|
||||
|
|
@ -420,6 +395,23 @@ void updates_set_robots(int robots){
|
|||
no_of_robot=robots;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------
|
||||
/ Create Buzz bytecode from the bzz script inputed
|
||||
/-------------------------------------------------------*/
|
||||
int compile_bzz(){
|
||||
/*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 << ".bo";
|
||||
bzzfile_in_compile << " -d " << path << name << ".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(){
|
||||
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
|
||||
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
|
||||
|
|
|
|||
|
|
@ -502,13 +502,20 @@ static int create_stig_tables() {
|
|||
|
||||
/* Save bytecode file name */
|
||||
BO_FNAME = strdup(bo_filename);
|
||||
/* Execute the global part of the script */
|
||||
buzzvm_execute_script(VM);
|
||||
/* Call the Init() function */
|
||||
buzzvm_function_call(VM, "init", 0);
|
||||
|
||||
// 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 */
|
||||
|
||||
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
|
||||
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
|
||||
|
||||
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||
}
|
||||
|
|
@ -554,10 +561,17 @@ static int create_stig_tables() {
|
|||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Execute the global part of the script
|
||||
buzzvm_execute_script(VM);
|
||||
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
|
||||
buzzvm_function_call(VM, "init", 0);
|
||||
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;
|
||||
}
|
||||
|
|
@ -604,9 +618,15 @@ static int create_stig_tables() {
|
|||
return 0;
|
||||
}
|
||||
// Execute the global part of the script
|
||||
buzzvm_execute_script(VM);
|
||||
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
|
||||
buzzvm_function_call(VM, "init", 0);
|
||||
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;
|
||||
}
|
||||
|
|
@ -725,19 +745,25 @@ static int create_stig_tables() {
|
|||
}
|
||||
|
||||
int update_step_test() {
|
||||
|
||||
/*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);
|
||||
update_users();
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
//set_robot_var(buzzdict_size(VM->swarmmembers)+1);
|
||||
|
||||
int a = buzzvm_function_call(VM, "step", 0);
|
||||
if(a != BUZZVM_STATE_READY) {
|
||||
|
||||
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);
|
||||
fprintf(stdout, " execution terminated abnormally\n\n");
|
||||
}
|
||||
}
|
||||
|
||||
return a == BUZZVM_STATE_READY;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -101,10 +101,11 @@ namespace rosbzz_node{
|
|||
get_number_of_robots();
|
||||
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
|
||||
//buzz_utility::set_robot_var(no_of_robots);
|
||||
/*Set no of robots for updates*/
|
||||
/*Set no of robots for updates TODO only when not updating*/
|
||||
//if(multi_msg)
|
||||
updates_set_robots(no_of_robots);
|
||||
/*run once*/
|
||||
ros::spinOnce();
|
||||
/*run once*/
|
||||
ros::spinOnce();
|
||||
/*loop rate of ros*/
|
||||
ros::Rate loop_rate(10);
|
||||
loop_rate.sleep();
|
||||
|
|
|
|||
Loading…
Reference in New Issue