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

added batt/state of all neighbor in webcontrol, enhance serialization for vstig

This commit is contained in:
dave 2018-11-04 16:27:11 -05:00
parent ec47973fef
commit b62789a583
9 changed files with 236 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
buzz_scripts/include/act/states.bzz
View File

@ -13,6 +13,7 @@ include "act/neighborcomm.bzz"
TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
WP_STIG = 8
path_it = 0
pic_time = 0
@ -100,7 +101,7 @@ firsttimeinwp = 1
wpreached = 1
function indiWP() {
if(firsttimeinwp) {
v_wp = stigmergy.create(8)
v_wp = stigmergy.create(WP_STIG)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0
}
@ -111,21 +112,27 @@ function indiWP() {
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
return
} else {
v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0})
var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
v_wp.put(rc_goto.id,ls)
reset_rc()
}
}
wp = v_wp.get(id)
if(wp!=nil) {
#log(wp.pro,wpreached)
if(wp.pro == 0) {
wpreached = 0
storegoal(wp.lat, wp.lon, pose.position.altitude)
v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1})
return
}
}
#if(vstig_buzzy == 0) {
wpi = v_wp.get(id)
if(wpi!=nil) {
#log(wp.pro,wpreached)
wp = unpackWP2i(wpi)
table_print(wp)
if(wp.pro == 0) {
wpreached = 0
storegoal(wp.lat, wp.lon, pose.position.altitude)
var ls = packWP2i(wp.lat, wp.lon, 1)
v_wp.put(id,ls)
return
}
}
#}
if(wpreached!=1)
goto_gps(indiWP_done)

15
buzz_scripts/include/plan/mapmatrix.bzz
View File

@ -102,21 +102,6 @@ function is_in(dictionary, x, y){
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)) {

55
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -6,6 +6,7 @@ include "taskallocate/graphs/shapes_P.bzz"
include "taskallocate/graphs/shapes_O.bzz"
include "taskallocate/graphs/shapes_L.bzz"
include "taskallocate/graphs/shapes_Y.bzz"
#include "utils/table.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
@ -82,60 +83,6 @@ 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
#

16
buzz_scripts/include/utils/conversions.bzz
View File

@ -61,3 +61,19 @@ function gps_from_vec(vec) {
return Lgoal
}
GPSoffset = {.lat=45.50, .lon=-73.62}
function packWP2i(in_lat, in_long, processed) {
var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
var dlon = math.round((in_long - GPSoffset.lon)*1000000)
return dlat*100000+dlon*10+processed
}
function unpackWP2i(wp_int){
dlat = (wp_int-wp_int%100000)/100000
wp_int=wp_int-dlat*100000
dlon = (wp_int-wp_int%10)/10
wp_int=wp_int-dlon*10
return {.lat=dlat/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=wp_int}
}

115
buzz_scripts/include/utils/table.bzz Normal file
View File

@ -0,0 +1,115 @@
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
}
#
#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"
}
else if(value==6){
return "TURNEDOFF"
}
else if(value==7){
return "LAND"
}
else if(value==8){
return "BARRIERWAIT"
}
else if(value==9){
return "INDIWP"
}
else if(value==10){
return "TASK_ALLOCATE"
}
else if(value==11){
return "LAUNCH"
}
else if(value==12){
return "STOP"
}
else {
return "UNDETERMINED"
}
}
#
#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
}
else if(value=="TURNEDOFF"){
return 6
}
else if(value=="LAND"){
return 7
}
else if(value=="BARRIERWAIT"){
return 8
}
else if(value=="INDIWP"){
return 9
}
else if(value=="TASK_ALLOCATE"){
return 10
}
else if(value=="LAUNCH"){
return 11
}
else if(value=="STOP"){
return 12
}
else
return 0
}

85
buzz_scripts/include/vstigenv.bzz
View File

@ -8,15 +8,14 @@
#
STATUS_VSTIG = 20
GROUND_VSTIG = 21
HIGHEST_ROBOTID = 14
WAIT4STEP = 10
WAIT4STEP = 5
vstig_buzzy = 0
#
# Init var
#
var v_status = {}
var v_ground = {}
b_updating = 0
vstig_counter = WAIT4STEP
@ -27,36 +26,36 @@ function init_swarm() {
function init_stig() {
v_status = stigmergy.create(STATUS_VSTIG)
v_ground = stigmergy.create(GROUND_VSTIG)
#v_ground = stigmergy.create(GROUND_VSTIG)
}
function uav_updatestig() {
# TODO: Push values on update only.
# 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)
vstig_buzzy = 1
var ls = battery.capacity*10 + s2i(BVMSTATE)
#log("Pushing ", ls, "on vstig with id:", id)
v_status.put(id, ls)
vstig_counter=WAIT4STEP
} else if(vstig_counter==WAIT4STEP-1){ # ensure comm. delay between fetch and update stig
vstig_buzzy = 1
vstig_counter=vstig_counter-1
stattab_send()
} else {
b_updating=0
vstig_buzzy = 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 unpackstatus(recv_value,state_struct){
#state_struct.gp=(recv_value-recv_value%1000000)/1000000
#recv_value=recv_value-state_struct.gp*1000000
state_struct.ba=(recv_value-recv_value%10)/10
recv_value=recv_value-state_struct.ba*10
#state_struct.xb=(recv_value-recv_value%10)/10
#recv_value=recv_value-state_struct.xb*10
state_struct.st=recv_value
return state_struct
}
function checkusers() {
@ -95,37 +94,29 @@ function usertab_print(t) {
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)
var state_struct = {.id = -1, .gp = -1, .ba = -1, .xb = -1, .st = -1}
neighbors.foreach(function(rid, data) {
var nei_state = v_status.get(rid)
if(nei_state!=nil){
state_struct.id=u
state_struct = unpackstatus(nei_state, state_struct)
table_print(state_struct)
}
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)
var state_struct = {.id = -1, .gp = -1, .ba = -1, .xb = -1, .st = 0}
neighbors.foreach(function(rid, data) {
var nei_state = v_status.get(rid)
if(nei_state!=nil){
state_struct.id=rid
state_struct = unpackstatus(nei_state,state_struct)
add_neighborStatus(state_struct)
}
u=u+1
}
}
} )
}
}

6
buzz_scripts/main.bzz
View File

@ -2,6 +2,7 @@ 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 "utils/table.bzz"
include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "taskallocate/bidding.bzz"
@ -27,9 +28,6 @@ LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barr
# 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() {
@ -53,7 +51,7 @@ function step() {
rc_cmd_listen()
# update the vstig (status/net/batt/...)
# uav_updatestig()
#uav_updatestig()
#
# State machine

6
src/buzz_utility.cpp
View File

@ -291,6 +291,12 @@ static int testing_buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
return VM->state;
}

57
src/buzzuav_closures.cpp
View File

@ -54,7 +54,7 @@ int buzzros_print(buzzvm_t vm)
----------------------------------------------------------- */
{
std::ostringstream buffer(std::ostringstream::ate);
buffer << "[" << buzz_utility::get_robotid() << "] ";
buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] ";
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{
buzzvm_lload(vm, index);
@ -288,23 +288,42 @@ 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);
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); // state table
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t t = buzzvm_stack_at(vm, 1);
if(buzzdict_size(t->t.value) != 5) {
ROS_WARN("Wrong neighbor status size.");
return vm->state;
}
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;
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
buzzvm_tget(vm);
uint8_t id = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "ba", 1));
buzzvm_tget(vm);
newRS.batt_lvl = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "gp", 1));
buzzvm_tget(vm);
newRS.gps_strenght = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "xb", 1));
buzzvm_tget(vm);
newRS.xbee = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "st", 1));
buzzvm_tget(vm);
newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
if (it != neighbors_status_map.end())
neighbors_status_map.erase(it);
@ -417,8 +436,10 @@ void set_gpsgoal(double goal[3])
rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0) {
goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
}
ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
} else
ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
}
int buzzuav_arm(buzzvm_t vm)