more fixes to rrt, added Vivek fix to MSG size and fix crashes due to UAVSTATE variable
This commit is contained in:
dave
parent
89b263e4df
commit
6018b681d7
|
|
@ -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() {
|
||||
}
|
||||
|
|
@ -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"
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue