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

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() {
}

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"
}

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

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) {
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))
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) {
@ -141,17 +156,17 @@ 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
}

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)
}

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);