fix bzz compilation and moved scripts and include
This commit is contained in:
parent
9edceb77b1
commit
f8917cd39e
|
@ -1,3 +1,7 @@
|
||||||
src/test*
|
src/test*
|
||||||
.cproject
|
.cproject
|
||||||
.project
|
.project
|
||||||
|
.basm
|
||||||
|
.bo
|
||||||
|
.bdb
|
||||||
|
.bdbg
|
||||||
|
|
|
@ -46,7 +46,6 @@ void rc_call(int rc_cmd);
|
||||||
void set_battery(float voltage,float current,float remaining);
|
void set_battery(float voltage,float current,float remaining);
|
||||||
/* sets current position */
|
/* sets current position */
|
||||||
void set_currentpos(double latitude, double longitude, double altitude);
|
void set_currentpos(double latitude, double longitude, double altitude);
|
||||||
void set_userspos(double latitude, double longitude, double altitude);
|
|
||||||
/*retuns the current go to position */
|
/*retuns the current go to position */
|
||||||
double* getgoto();
|
double* getgoto();
|
||||||
/* updates flight status*/
|
/* updates flight status*/
|
||||||
|
@ -85,7 +84,6 @@ int buzzuav_update_battery(buzzvm_t vm);
|
||||||
* Updates current position in Buzz
|
* Updates current position in Buzz
|
||||||
*/
|
*/
|
||||||
int buzzuav_update_currentpos(buzzvm_t vm);
|
int buzzuav_update_currentpos(buzzvm_t vm);
|
||||||
buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
|
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
|
||||||
|
|
|
@ -137,7 +137,7 @@ private:
|
||||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||||
|
|
||||||
/*compiles buzz script from the specified .bzz file*/
|
/*compiles buzz script from the specified .bzz file*/
|
||||||
void Compile_bzz();
|
std::string Compile_bzz(std::string bzzfile_name);
|
||||||
|
|
||||||
/*Flight controller service call*/
|
/*Flight controller service call*/
|
||||||
void flight_controller_service_call();
|
void flight_controller_service_call();
|
||||||
|
|
|
@ -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
|
||||||
|
}
|
Binary file not shown.
Binary file not shown.
|
@ -8,7 +8,7 @@ include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||||
updated="update_ack"
|
updated="update_ack"
|
||||||
update_no=0
|
update_no=0
|
||||||
function updated_neigh(){
|
function updated_neigh(){
|
||||||
neighbors.broadcast(updated, update_no)
|
neighbors.broadcast(updated, update_no)
|
||||||
}
|
}
|
||||||
|
|
||||||
TARGET_ALTITUDE = 10.0
|
TARGET_ALTITUDE = 10.0
|
||||||
|
@ -147,11 +147,20 @@ function table_print(t) {
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
|
########################################
|
||||||
|
#
|
||||||
|
# MAIN FUNCTIONS
|
||||||
|
#
|
||||||
|
########################################
|
||||||
|
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
s = swarm.create(1)
|
s = swarm.create(1)
|
||||||
# s.select(1)
|
|
||||||
s.join()
|
s.join()
|
||||||
|
v = stigmergy.create(5)
|
||||||
|
t = {}
|
||||||
|
v.put("p",t)
|
||||||
|
v.put("u",1)
|
||||||
statef=idle
|
statef=idle
|
||||||
CURSTATE = "IDLE"
|
CURSTATE = "IDLE"
|
||||||
}
|
}
|
||||||
|
@ -202,15 +211,22 @@ neighbors.listen("cmd",
|
||||||
statef()
|
statef()
|
||||||
log("Current state: ", CURSTATE)
|
log("Current state: ", CURSTATE)
|
||||||
log("Swarm size: ",ROBOTS)
|
log("Swarm size: ",ROBOTS)
|
||||||
log("User position: ", users.range)
|
#log("User position: ", users.range)
|
||||||
|
|
||||||
# Read a value from the structure
|
# Read a value from the structure
|
||||||
t = vt.get("p")
|
#t = v.get("p")
|
||||||
log(t)
|
log(users)
|
||||||
table_print(t)
|
table_print(users)
|
||||||
|
if(size(users)>0){
|
||||||
|
tmp = {2 3}
|
||||||
|
v.put("p", tmp)
|
||||||
|
v.put("u",2)
|
||||||
|
}
|
||||||
|
|
||||||
# Get the number of keys in the structure
|
# Get the number of keys in the structure
|
||||||
log("The vstig has ", vt.size(), " elements")
|
log("The vstig has ", v.size(), " elements")
|
||||||
|
table_print(v.get("p"))
|
||||||
|
log(v.get("u"))
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed once when the robot (or the simulator) is reset.
|
# Executed once when the robot (or the simulator) is reset.
|
Binary file not shown.
Binary file not shown.
|
@ -0,0 +1,38 @@
|
||||||
|
####################################################################################################
|
||||||
|
# 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)
|
||||||
|
}
|
||||||
|
|
||||||
|
function init(){
|
||||||
|
s = swarm.create(1)
|
||||||
|
s.join()
|
||||||
|
v = stigmergy.create(5)
|
||||||
|
#t= {}
|
||||||
|
#v.put("p",t)
|
||||||
|
v.put("u",1)
|
||||||
|
}
|
||||||
|
|
||||||
|
function step() {
|
||||||
|
log("Swarm size: ",ROBOTS)
|
||||||
|
log("The vstig has ", v.size(), " elements")
|
||||||
|
log(v.get("u"))
|
||||||
|
if (id==1) {
|
||||||
|
#tmp = { .x=3 }
|
||||||
|
#v.put("p",tmp)
|
||||||
|
v.put("u",2)
|
||||||
|
}
|
||||||
|
#log(v.get("p"))
|
||||||
|
}
|
||||||
|
|
||||||
|
# Executed once when the robot (or the simulator) is reset.
|
||||||
|
function reset() {
|
||||||
|
}
|
||||||
|
|
||||||
|
# Executed once at the end of experiment.
|
||||||
|
function destroy() {
|
||||||
|
}
|
|
@ -66,29 +66,29 @@ namespace buzz_utility{
|
||||||
//make_table(&t);
|
//make_table(&t);
|
||||||
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
||||||
/* Register table as global symbol */
|
/* Register table as global symbol */
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||||
buzzvm_tget(VM);
|
buzzvm_tget(VM);*/
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
||||||
//buzzvm_gload(VM);
|
|
||||||
buzzvm_push(VM, t);
|
buzzvm_push(VM, t);
|
||||||
buzzvm_pushi(VM, 2);
|
buzzvm_gstore(VM);
|
||||||
buzzvm_callc(VM);
|
//buzzvm_pushi(VM, 2);
|
||||||
|
//buzzvm_callc(VM);
|
||||||
return VM->state;
|
return VM->state;
|
||||||
}
|
}
|
||||||
|
|
||||||
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
|
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
|
||||||
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
||||||
/* Get users "p" table */
|
/* Get users "p" table */
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
|
||||||
buzzvm_tget(VM);
|
buzzvm_tget(VM);*/
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
||||||
//buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzvm_pushi(VM, 1);
|
//buzzvm_pushi(VM, 1);
|
||||||
buzzvm_callc(VM);
|
//buzzvm_callc(VM);
|
||||||
buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
|
buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
|
||||||
buzzobj_t nbr = buzzvm_stack_at(VM, 1);
|
buzzobj_t nbr = buzzvm_stack_at(VM, 1);
|
||||||
/* Get "data" field */
|
/* Get "data" field */
|
||||||
|
@ -129,7 +129,7 @@ namespace buzz_utility{
|
||||||
buzzvm_tput(VM);
|
buzzvm_tput(VM);
|
||||||
ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
|
ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
|
||||||
// forcing the new table into the stigmergy....
|
// forcing the new table into the stigmergy....
|
||||||
buzzobj_t newt = buzzvm_stack_at(VM, 0);
|
/*buzzobj_t newt = buzzvm_stack_at(VM, 0);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||||
|
@ -137,7 +137,7 @@ namespace buzz_utility{
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
||||||
buzzvm_push(VM, nbr);
|
buzzvm_push(VM, nbr);
|
||||||
buzzvm_pushi(VM, 2);
|
buzzvm_pushi(VM, 2);
|
||||||
buzzvm_callc(VM);
|
buzzvm_callc(VM);*/
|
||||||
//buzzvm_gstore(VM);
|
//buzzvm_gstore(VM);
|
||||||
return VM->state;
|
return VM->state;
|
||||||
}
|
}
|
||||||
|
@ -358,7 +358,6 @@ static int create_stig_tables() {
|
||||||
|
|
||||||
// usersvstig = stigmergy.create(123)
|
// usersvstig = stigmergy.create(123)
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||||
//buzzvm_gstore(VM);
|
|
||||||
// get the stigmergy table from the global scope
|
// get the stigmergy table from the global scope
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
|
@ -368,19 +367,23 @@ static int create_stig_tables() {
|
||||||
// value of the stigmergy id
|
// value of the stigmergy id
|
||||||
buzzvm_pushi(VM, 5);
|
buzzvm_pushi(VM, 5);
|
||||||
// call the stigmergy.create() method
|
// call the stigmergy.create() method
|
||||||
|
buzzvm_dump(VM);
|
||||||
// buzzvm_closure_call(VM, 1);
|
// buzzvm_closure_call(VM, 1);
|
||||||
buzzvm_pushi(VM, 1);
|
buzzvm_pushi(VM, 1);
|
||||||
buzzvm_callc(VM);
|
buzzvm_callc(VM);
|
||||||
buzzvm_gstore(VM);
|
buzzvm_gstore(VM);
|
||||||
|
buzzvm_dump(VM);
|
||||||
|
|
||||||
//buzzusers_reset();
|
//buzzusers_reset();
|
||||||
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
||||||
|
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||||
buzzvm_tget(VM);
|
buzzvm_tget(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
||||||
buzzvm_push(VM, t);
|
buzzvm_push(VM, t);
|
||||||
|
buzzvm_dump(VM);
|
||||||
// buzzvm_closure_call(VM, 2);
|
// buzzvm_closure_call(VM, 2);
|
||||||
buzzvm_pushi(VM, 2);
|
buzzvm_pushi(VM, 2);
|
||||||
buzzvm_callc(VM);
|
buzzvm_callc(VM);
|
||||||
|
@ -451,7 +454,7 @@ static int create_stig_tables() {
|
||||||
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
/* Create vstig tables */
|
/* Create vstig tables
|
||||||
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
||||||
buzzvm_destroy(&VM);
|
buzzvm_destroy(&VM);
|
||||||
buzzdebug_destroy(&DBG_INFO);
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
|
@ -459,7 +462,12 @@ static int create_stig_tables() {
|
||||||
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
||||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}*/
|
||||||
|
|
||||||
|
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
||||||
|
buzzvm_push(VM, t);
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
|
||||||
/* Save bytecode file name */
|
/* Save bytecode file name */
|
||||||
BO_FNAME = strdup(bo_filename);
|
BO_FNAME = strdup(bo_filename);
|
||||||
|
@ -626,7 +634,6 @@ static int create_stig_tables() {
|
||||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||||
buzzuav_closures::update_neighbors(VM);
|
buzzuav_closures::update_neighbors(VM);
|
||||||
update_users();
|
update_users();
|
||||||
buzzuav_closures::buzzuav_update_userspos(VM);
|
|
||||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -645,13 +652,14 @@ static int create_stig_tables() {
|
||||||
/* Process out messages */
|
/* Process out messages */
|
||||||
buzzvm_process_outmsgs(VM);
|
buzzvm_process_outmsgs(VM);
|
||||||
/*Print swarm*/
|
/*Print swarm*/
|
||||||
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||||
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
|
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
|
||||||
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
|
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
|
||||||
|
set_robot_var(buzzdict_size(VM->swarmmembers)+1);
|
||||||
|
|
||||||
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
|
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
|
||||||
int status = 1;
|
//int status = 1;
|
||||||
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
//buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
|
@ -19,7 +19,6 @@ namespace buzzuav_closures{
|
||||||
static float batt[3];
|
static float batt[3];
|
||||||
static float obst[5]={0,0,0,0,0};
|
static float obst[5]={0,0,0,0,0};
|
||||||
static double cur_pos[3];
|
static double cur_pos[3];
|
||||||
static double users_pos[3];
|
|
||||||
static uint8_t status;
|
static uint8_t status;
|
||||||
static int cur_cmd = 0;
|
static int cur_cmd = 0;
|
||||||
static int rc_cmd=0;
|
static int rc_cmd=0;
|
||||||
|
@ -275,19 +274,7 @@ namespace buzzuav_closures{
|
||||||
(it->second).z);
|
(it->second).z);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
void set_userspos(double latitude, double longitude, double altitude){
|
|
||||||
/*buzz_utility::Pos_struct pos_arr;
|
|
||||||
pos_arr.x=latitude;
|
|
||||||
pos_arr.y=longitude;
|
|
||||||
pos_arr.z=altitude;
|
|
||||||
map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
|
|
||||||
if(it!=users_map.end())
|
|
||||||
users_map.erase(it);
|
|
||||||
users_map.insert(make_pair(id, pos_arr));*/
|
|
||||||
users_pos[0]=latitude;
|
|
||||||
users_pos[1]=longitude;
|
|
||||||
users_pos[2]=altitude;
|
|
||||||
}
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
int buzzuav_update_currentpos(buzzvm_t vm) {
|
int buzzuav_update_currentpos(buzzvm_t vm) {
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
|
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
|
||||||
|
@ -307,30 +294,6 @@ namespace buzzuav_closures{
|
||||||
buzzvm_gstore(vm);
|
buzzvm_gstore(vm);
|
||||||
return vm->state;
|
return vm->state;
|
||||||
}
|
}
|
||||||
buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
|
|
||||||
/*map< int, buzz_utility::Pos_struct >::iterator it;
|
|
||||||
for (it=users_map.begin(); it!=users_map.end(); ++it){
|
|
||||||
}*/
|
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
|
|
||||||
buzzvm_pusht(vm);
|
|
||||||
buzzvm_dup(vm);
|
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
|
|
||||||
buzzvm_pushf(vm, users_pos[0]);
|
|
||||||
buzzvm_tput(vm);
|
|
||||||
buzzvm_dup(vm);
|
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
|
|
||||||
buzzvm_pushf(vm, users_pos[1]);
|
|
||||||
buzzvm_tput(vm);
|
|
||||||
buzzvm_dup(vm);
|
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
|
|
||||||
buzzvm_pushf(vm, users_pos[2]);
|
|
||||||
buzzvm_tput(vm);
|
|
||||||
buzzobj_t tbl = buzzvm_stack_at(vm, 0);
|
|
||||||
buzzvm_gstore(vm);
|
|
||||||
|
|
||||||
return tbl;
|
|
||||||
//return vm->state;
|
|
||||||
}
|
|
||||||
|
|
||||||
void flight_status_update(uint8_t state){
|
void flight_status_update(uint8_t state){
|
||||||
status=state;
|
status=state;
|
||||||
|
|
|
@ -13,7 +13,9 @@ namespace rosbzz_node{
|
||||||
/*Initialize publishers, subscribers and client*/
|
/*Initialize publishers, subscribers and client*/
|
||||||
Initialize_pub_sub(n_c);
|
Initialize_pub_sub(n_c);
|
||||||
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
||||||
Compile_bzz();
|
std::string fname = Compile_bzz(bzzfile_name);
|
||||||
|
bcfname = fname + ".bo";
|
||||||
|
dbgfname = fname + ".bdb";
|
||||||
set_bzz_file(bzzfile_name.c_str());
|
set_bzz_file(bzzfile_name.c_str());
|
||||||
/*Initialize variables*/
|
/*Initialize variables*/
|
||||||
// Solo things
|
// Solo things
|
||||||
|
@ -68,7 +70,8 @@ namespace rosbzz_node{
|
||||||
/* Set the Buzz bytecode */
|
/* Set the Buzz bytecode */
|
||||||
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
|
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
|
||||||
fprintf(stdout, "Bytecode file found and set\n");
|
fprintf(stdout, "Bytecode file found and set\n");
|
||||||
init_update_monitor(bcfname.c_str(),stand_by.c_str());
|
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
|
||||||
|
init_update_monitor(bcfname.c_str(),standby_bo.c_str());
|
||||||
///////////////////////////////////////////////////////
|
///////////////////////////////////////////////////////
|
||||||
// MAIN LOOP
|
// MAIN LOOP
|
||||||
//////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////
|
||||||
|
@ -95,7 +98,7 @@ namespace rosbzz_node{
|
||||||
//no_of_robots=get_number_of_robots();
|
//no_of_robots=get_number_of_robots();
|
||||||
get_number_of_robots();
|
get_number_of_robots();
|
||||||
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
|
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
|
||||||
buzz_utility::set_robot_var(no_of_robots);
|
//buzz_utility::set_robot_var(no_of_robots);
|
||||||
/*Set no of robots for updates*/
|
/*Set no of robots for updates*/
|
||||||
updates_set_robots(no_of_robots);
|
updates_set_robots(no_of_robots);
|
||||||
/*run once*/
|
/*run once*/
|
||||||
|
@ -258,28 +261,35 @@ namespace rosbzz_node{
|
||||||
/*--------------------------------------------------------
|
/*--------------------------------------------------------
|
||||||
/ Create Buzz bytecode from the bzz script inputed
|
/ Create Buzz bytecode from the bzz script inputed
|
||||||
/-------------------------------------------------------*/
|
/-------------------------------------------------------*/
|
||||||
void roscontroller::Compile_bzz(){
|
std::string roscontroller::Compile_bzz(std::string bzzfile_name){
|
||||||
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
|
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
|
||||||
/*Compile the buzz code .bzz to .bo*/
|
/*Compile the buzz code .bzz to .bo*/
|
||||||
stringstream bzzfile_in_compile;
|
stringstream bzzfile_in_compile;
|
||||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
|
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||||
bzzfile_in_compile<<path<<"/";
|
//bzzfile_in_compile << path << "/";
|
||||||
path = bzzfile_in_compile.str();
|
//path = bzzfile_in_compile.str();
|
||||||
bzzfile_in_compile.str("");
|
//bzzfile_in_compile.str("");
|
||||||
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||||
name = name.substr(0,name.find_last_of("."));
|
name = name.substr(0,name.find_last_of("."));
|
||||||
bzzfile_in_compile << "bzzc "<<bzzfile_name; //<<" "<<path<< name<<".basm";
|
bzzfile_in_compile << "bzzc -I " << path << "script/include/"; //<<" "<<path<< name<<".basm";
|
||||||
system(bzzfile_in_compile.str().c_str());
|
|
||||||
//bzzfile_in_compile.str("");
|
//bzzfile_in_compile.str("");
|
||||||
//bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
|
//bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
|
||||||
//system(bzzfile_in_compile.str().c_str());
|
//system(bzzfile_in_compile.str().c_str());
|
||||||
bzzfile_in_compile.str("");
|
//bzzfile_in_compile.str("");
|
||||||
bzzfile_in_compile <<path<<name<<".bo";
|
bzzfile_in_compile << " -b " << path << name << ".bo";
|
||||||
bcfname = bzzfile_in_compile.str();
|
//bcfname = bzzfile_in_compile.str();
|
||||||
bzzfile_in_compile.str("");
|
//std::string tmp_bcfname = path + name + ".bo";
|
||||||
bzzfile_in_compile <<path<<name<<".bdb";
|
//bzzfile_in_compile.str("");
|
||||||
dbgfname = bzzfile_in_compile.str();
|
bzzfile_in_compile << " -d " << path << name << ".bdb ";
|
||||||
|
//bzzfile_in_compile << " -a " << path << name << ".asm ";
|
||||||
|
bzzfile_in_compile << bzzfile_name;
|
||||||
|
//std::string tmp_dbgfname = path + name + ".bdb";
|
||||||
|
|
||||||
|
ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str());
|
||||||
|
|
||||||
|
system(bzzfile_in_compile.str().c_str());
|
||||||
|
|
||||||
|
return path + name;
|
||||||
}
|
}
|
||||||
/*----------------------------------------------------
|
/*----------------------------------------------------
|
||||||
/ Publish neighbours pos and id in neighbours pos topic
|
/ Publish neighbours pos and id in neighbours pos topic
|
||||||
|
@ -311,14 +321,13 @@ namespace rosbzz_node{
|
||||||
void roscontroller::Arm(){
|
void roscontroller::Arm(){
|
||||||
mavros_msgs::CommandBool arming_message;
|
mavros_msgs::CommandBool arming_message;
|
||||||
arming_message.request.value = armstate;
|
arming_message.request.value = armstate;
|
||||||
ROS_INFO("FC Arm Service called!------------------------------------------------------");
|
|
||||||
if(arm_client.call(arming_message)) {
|
if(arm_client.call(arming_message)) {
|
||||||
if(arming_message.response.success==1)
|
if(arming_message.response.success==1)
|
||||||
ROS_INFO("FC Arm Service called!");
|
ROS_WARN("FC Arm Service called!");
|
||||||
else
|
else
|
||||||
ROS_INFO("FC Arm Service call failed!");
|
ROS_WARN("FC Arm Service call failed!");
|
||||||
} else {
|
} else {
|
||||||
ROS_INFO("FC Arm Service call failed!");
|
ROS_WARN("FC Arm Service call failed!");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,144 +0,0 @@
|
||||||
!19
|
|
||||||
'updated
|
|
||||||
'update_ack
|
|
||||||
'init
|
|
||||||
'barrier
|
|
||||||
'stigmergy
|
|
||||||
'create
|
|
||||||
'put
|
|
||||||
'id
|
|
||||||
'barrier_val
|
|
||||||
'onconflict
|
|
||||||
'data
|
|
||||||
'stand_by
|
|
||||||
'get
|
|
||||||
'size
|
|
||||||
'neighbors
|
|
||||||
'listen
|
|
||||||
'update_no
|
|
||||||
'ROBOTS
|
|
||||||
'step
|
|
||||||
|
|
||||||
pushs 2
|
|
||||||
pushcn @__label_1
|
|
||||||
gstore
|
|
||||||
pushs 11
|
|
||||||
pushcn @__label_5
|
|
||||||
gstore
|
|
||||||
pushs 18
|
|
||||||
pushcn @__label_9
|
|
||||||
gstore
|
|
||||||
nop
|
|
||||||
|
|
||||||
@__label_0
|
|
||||||
pushs 0 |1,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 1 |1,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gstore |1,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
|
|
||||||
@__exitpoint
|
|
||||||
done
|
|
||||||
|
|
||||||
@__label_1
|
|
||||||
pushs 3 |4,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 4 |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 5 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |4,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 101 |4,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 1 |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gstore |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 3 |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 6 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |5,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 7 |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 1 |5,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 2 |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 8 |6,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 0 |6,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gstore |6,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 3 |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 9 |7,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |7,18,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushl @__label_2 |7,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 1 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret0 |12,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
|
|
||||||
@__label_2
|
|
||||||
lload 3 |8,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 10 |8,6,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |8,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
lload 2 |8,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 10 |8,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
lt |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
lload 3 |8,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 10 |8,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |8,33,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
lload 2 |8,37,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 10 |8,39,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
eq |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
or |8,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
jumpz @__label_3 |8,47,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
lload 2 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret1 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
jump @__label_4 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
@__label_3 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
lload 3 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret1 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
@__label_4 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret0 |10,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
|
|
||||||
@__label_5
|
|
||||||
pushs 3 |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 12 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |16,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 7 |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 1 |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 8 |17,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 3 |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 13 |17,22,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |17,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 0 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gstore |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 14 |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 15 |19,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
tget |19,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 0 |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushl @__label_6 |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret0 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
|
|
||||||
@__label_6
|
|
||||||
lload 2 |21,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 16 |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
eq |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
jumpz @__label_7 |21,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 8 |21,38,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushs 17 |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gstore |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
@__label_7 |22,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret0 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
|
|
||||||
@__label_9
|
|
||||||
pushs 11 |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
gload |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
pushi 0 |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
callc |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
||||||
ret0 |32,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz
|
|
BIN
src/stand_by.bdb
BIN
src/stand_by.bdb
Binary file not shown.
Binary file not shown.
BIN
src/stand_by.bo
BIN
src/stand_by.bo
Binary file not shown.
Loading…
Reference in New Issue