From 6018b681d71a4e9d5c8bd6d58c1f19ee2d92a6bf Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 27 Nov 2017 22:55:32 -0500 Subject: [PATCH] more fixes to rrt, added Vivek fix to MSG size and fix crashes due to UAVSTATE variable --- buzz_scripts/empty.bzz | 17 ++++++++ buzz_scripts/graphformGPS.bzz | 15 +++---- buzz_scripts/include/barrier.bzz | 4 +- buzz_scripts/include/rrtstar.bzz | 63 ++++++++++++++++++------------ buzz_scripts/include/uavstates.bzz | 44 ++++++++++++++++----- src/buzz_utility.cpp | 26 ++++++++---- 6 files changed, 117 insertions(+), 52 deletions(-) create mode 100644 buzz_scripts/empty.bzz diff --git a/buzz_scripts/empty.bzz b/buzz_scripts/empty.bzz new file mode 100644 index 0000000..3bb5b12 --- /dev/null +++ b/buzz_scripts/empty.bzz @@ -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() { +} diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 138b320..d4e5aae 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -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(iBS: ", 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 diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index f6c39ef..b9e84a4 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -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 } diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index bb2d617..a789600 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -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) } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 06cd665..90d5dfa 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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 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);