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

more fixes to rrt, added Vivek fix to MSG size and fix crashes due to UAVSTATE variable

This commit is contained in:
dave 2017-11-27 22:55:32 -05:00
parent 89b263e4df
commit 6018b681d7
6 changed files with 117 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
buzz_scripts/empty.bzz Normal file
View File

@ -0,0 +1,17 @@
include "update.bzz"
# Executed once at init time.
function init() {
}
# Executed at each time step.
function step() {
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

15
buzz_scripts/graphformGPS.bzz
View File

@ -237,7 +237,7 @@ function unpackmessage(recv_value){
#unpack guide message
#
function unpack_guide_msg(recv_value){
log(id,"I pass value=",recv_value)
#log(id,"I pass value=",recv_value)
var qian=(recv_value-recv_value%1000)/1000
recv_value=recv_value-qian*1000
var bai=(recv_value-recv_value%100)/100
@ -497,10 +497,9 @@ function DoAsking(){
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
log("received label = ",m_MessageReqLabel[i])
#log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
log("get response")
psResponse=i
}}
if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){
@ -604,13 +603,12 @@ function DoJoined(){
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLabel=m_MessageReqLabel[i]
log("ReqLabel var:",ReqLabel)
log("M_vec var",m_vecNodes[ReqLabel].State)
#log("ReqLabel var:",ReqLabel)
#log("M_vec var",m_vecNodes[ReqLabel].State)
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
#is a request, store the index
mapRequests[j]=i
log("Into if m_nLabel")
j=j+1
}
}
@ -678,8 +676,7 @@ function DoJoined(){
#if(v_tag.size()==ROBOTS){
# TransitionToLock()
#}
#barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
barrier_wait(6, TransitionToLock, DoJoined, -1)
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
}
#
#Do Lock
@ -732,7 +729,7 @@ function init() {
#v_tag = stigmergy.create(m_lockstig)
#uav_initstig()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 20.0 + id * 2.5
#TARGET_ALTITUDE = 20.0 + id * 2.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}

4
buzz_scripts/include/barrier.bzz
View File

@ -40,7 +40,7 @@ function barrier_set(threshold, transf, resumef, bdt) {
# Make yourself ready
#
function barrier_ready() {
log("BARRIER READY -------")
#log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put("d", 0)
}
@ -52,7 +52,7 @@ function barrier_wait(threshold, transf, resumef, bdt) {
barrier.put(id, 1)
barrier.get(id)
log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
#log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
barrier.put("d", 1)
timeW = 0

57
buzz_scripts/include/rrtstar.bzz
View File

@ -5,23 +5,25 @@
#####
include "mapmatrix.bzz"
RRT_TIMEOUT = 200
RRT_TIMEOUT = 500
map = nil
cur_cell = {}
nei_cell = {}
function UAVpathPlanner(m_navigation) {
function UAVinit_map(m_navigation) {
# create a map big enough for the goal
init_map(2*math.round(math.vec2.length(m_navigation))+2)
init_map(2*math.round(math.vec2.length(m_navigation))+10)
# center the robot on the grid
cur_cell = math.vec2.new(math.round(len/2.0),math.round(len/2.0))
cur_cell = math.vec2.new(math.round(map.nb_col/2.0),math.round(map.nb_row/2.0))
}
function UAVpathPlanner(m_navigation, m_pos) {
# place the robot on the grid
update_curcell(m_pos,0)
# create the goal in the map grid
mapgoal = math.vec2.add(m_navigation,cur_cell)
# add all neighbors as obstacles in the grid
neighbors.foreach(function(rid, data) {
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
})
# TODO: add proximity sensor obstacles to the grid
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
# search for a path
return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0))
}
@ -32,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) {
m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
# place the robot on the grid
update_curcell(m_pos)
update_curcell(m_pos,1)
log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
log("Going to cell: ", m_goal.x, " ", m_goal.y)
@ -42,9 +44,14 @@ function Kh4pathPlanner(m_goal, m_pos) {
return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
}
function update_curcell(m_curpos) {
function update_curcell(m_curpos, kh4) {
if(kh4) { # for khepera playground
cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y))
} else { # for uav map relative to home
cur_cell = math.vec2.add(cur_cell, m_curpos)
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
}
if(cur_cell.x>map.nb_row)
cur_cell.x=map.nb_row
if(cur_cell.y>map.nb_col)
@ -55,9 +62,17 @@ function update_curcell(m_curpos) {
cur_cell.y=1
}
function UAVgetneiobs (m_curpos) {
update_curcell(m_curpos,0)
# add all neighbors as obstacles in the grid
neighbors.foreach(function(rid, data) {
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
})
}
function getneiobs (m_curpos) {
foundobstacle = 0
update_curcell(m_curpos)
update_curcell(m_curpos,1)
old_nei = table_copy(nei_cell)
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
@ -103,7 +118,7 @@ function getneiobs (m_curpos) {
function getproxobs (m_curpos) {
foundobstacle = 0
update_curcell(m_curpos)
update_curcell(m_curpos,1)
reduce(proximity,
function(key, value, acc) {
@ -143,15 +158,15 @@ function getproxobs (m_curpos) {
return foundobstacle
}
function check_path(m_path, goal_l, m_curpos) {
function check_path(m_path, goal_l, m_curpos, kh4) {
pathisblocked = 0
nb=goal_l
update_curcell(m_curpos)
#m_curpos = math.vec2.sub(m_curpos,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
#Cvec = math.vec2.new(math.round(m_curpos.x*CM2KH4.x), math.round(m_curpos.y*CM2KH4.y))
update_curcell(m_curpos,kh4)
Cvec = cur_cell
while(nb < m_path.nb_row and nb <= goal_l+1){
Nvec = getvec(m_path, nb)
if(kh4==0)
Nvec=vec_from_gps(Nvec.x,Nvec.y)
if(doesItIntersect(Cvec, Nvec)){
log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
pathisblocked = 1
@ -293,7 +308,7 @@ function RRTSTAR(GOAL,TOL) {
}
if(goalReached){
log("Goal found(",numberOfPoints,")!")
Path = getPath(Q,numberOfPoints)
Path = getPathGPS(Q,numberOfPoints)
print_pos(Path)
} else {
log("FAILED TO FIND A PATH!!!")
@ -395,8 +410,8 @@ function getPathGPS(Q,nb){
while(nb!=1){
npt=npt+1
path.nb_row=npt
path.mat[npt*2]=rmat(Q,nb,1)
path.mat[npt*2+1]=rmat(Q,nb,2)
path.mat[(npt-1)*2]=rmat(Q,nb,1)
path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
nb = rmat(Q,nb,3);
}
@ -434,6 +449,6 @@ function getPath(Q,nb){
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y
npt = npt - 1
}
log("Double-check path: ", check_path(pathR, 1, cur_cell))
#log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
return pathR
}

44
buzz_scripts/include/uavstates.bzz
View File

@ -5,7 +5,7 @@
########################################
include "rrtstar.bzz"
TARGET_ALTITUDE = 5.0 # m.
TARGET_ALTITUDE = 15.0 # m.
UAVSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2 # m/steps
@ -14,6 +14,7 @@ GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad.
cur_goal_l = 0
rc_State = 0
homegps = {.lat=0, .long=0}
function uav_initswarm() {
s = swarm.create(1)
@ -35,8 +36,7 @@ function takeoff() {
statef=takeoff
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
#barrier_set(ROBOTS, action, land, -1)
barrier_set(6, action, land, -1)
barrier_set(ROBOTS, action, land, -1)
barrier_ready()
} else {
log("Altitude: ", position.altitude)
@ -53,8 +53,7 @@ function land() {
uav_land()
if(flight.status != 2 and flight.status != 3) {
#barrier_set(ROBOTS,turnedoff,land, 21)
barrier_set(6,turnedoff,land, 21)
barrier_set(ROBOTS,turnedoff,land, 21)
barrier_ready()
}
}
@ -62,7 +61,7 @@ function land() {
function set_goto(transf) {
UAVSTATE = "GOTOGPS"
statef=function() {
gotoWP(transf);
gotoWP(transf)
}
if(rc_goto.id==id){
@ -90,21 +89,48 @@ function picture() {
ptime=ptime+1
}
#
# still requires to be tuned, replaning takes too much time..
# DS 23/11/2017
function gotoWPRRT(transf) {
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
print(" has to move ", math.vec2.length(rc_goal))
homegps.lat = position.latitude
homegps.long = position.longitude
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5)
log("Sorry this is too far.")
else {
if(Path==nil){
Path = UAVpathPlanner(rc_goal)
# create the map
if(map==nil)
UAVinit_map(rc_goal)
# add proximity sensor and neighbor obstacles to the map
while(Path==nil) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
Path = UAVpathPlanner(rc_goal, m_pos)
}
cur_goal_l = 1
} else if(cur_goal_l<=Path.nb_row) {
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude
cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
print(" first to ", cur_pt.x,cur_pt.y)
print(" heading to ", cur_pt.x,cur_pt.y)
if(math.vec2.length(cur_pt)>GOTODIST_TOL) {
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
UAVgetneiobs(m_pos)
if(check_path(Path,cur_goal_l,m_pos,0)) {
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
Path = nil
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
while(Path == nil) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
Path = UAVpathPlanner(rc_goal, m_pos)
}
cur_goal_l = 1
}
cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
}

26
src/buzz_utility.cpp
View File

@ -17,7 +17,6 @@ namespace buzz_utility{
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
@ -229,7 +228,7 @@ void in_message_process(){
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
//ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) {
if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
@ -467,11 +466,9 @@ int create_stig_tables() {
if(VM) buzzvm_destroy(&VM);
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
ROS_INFO(" Robot ID -1: %i" , robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
ROS_INFO(" Robot ID -2: %i" , robot_id);
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
@ -490,7 +487,6 @@ int create_stig_tables() {
return 0;
}
fclose(fd);
ROS_INFO(" Robot ID -3: %i" , robot_id);
/* Read debug information */
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM);
@ -498,7 +494,6 @@ int create_stig_tables() {
perror(bdbg_filename);
return 0;
}
ROS_INFO(" Robot ID -4: %i" , robot_id);
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -506,7 +501,6 @@ int create_stig_tables() {
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
return 0;
}
ROS_INFO(" Robot ID -5: %i" , robot_id);
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -514,7 +508,11 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
ROS_INFO(" Robot ID -6: %i" , robot_id);
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
@ -578,6 +576,12 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
return 0;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -634,6 +638,12 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
return 0;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);