From 6371581b66dc322699139d0b9323edbbab0ca1bf Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 19 Dec 2017 18:36:10 -0500 Subject: [PATCH 01/77] debug new matrix form for RRT and add blob check for intersection --- buzz_scripts/include/mapmatrix.bzz | 52 +++++-- buzz_scripts/include/rrtstar.bzz | 158 ++++++++++++-------- buzz_scripts/include/uavstates.bzz | 52 ++++--- buzz_scripts/{testalone.bzz => testRRT.bzz} | 12 +- buzz_scripts/testaloneWP.bzz | 2 - 5 files changed, 167 insertions(+), 109 deletions(-) rename buzz_scripts/{testalone.bzz => testRRT.bzz} (76%) diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index b99432a..79ad775 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -5,8 +5,8 @@ robot_marker = "X" # copy a full matrix row function mat_copyrow(out,ro,in,ri){ out[ro] = {} - var icr = 0 - while(icr < size(in[ri])) { + var icr = 1 + while(icr <= size(in[ri])) { out[ro][icr]=in[ri][icr] icr = icr + 1 } @@ -18,11 +18,11 @@ function getvec(t,row){ function init_test_map(len){ map = {} - var indexR = 0 - while(indexR < len) { + var indexR = 1 + while(indexR <= len) { map[indexR] = {} - var indexC = 0 - while(indexC < len) { + var indexC = 1 + while(indexC <= len) { map[indexR][indexC] = 1.0 indexC = indexC + 1 } @@ -33,22 +33,22 @@ function init_test_map(len){ map[6][5] = 0.0 map[4][5] = 0.0 - log("Occupancy grid initialized (",len,"x",len,") with obstacles.") + log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.") } function init_map(len){ map = {} - var indexR = 0 - while(indexR < len) { + var indexR = 1 + while(indexR <= len) { map[indexR] = {} - var indexC = 0 - while(indexC < len) { + var indexC = 1 + while(indexC <= len) { map[indexR][indexC] = 1.0 indexC = indexC + 1 } indexR = indexR + 1 } - log("Occupancy grid initialized (",len,"x",len,").") + log("Occupancy grid initialized (",size(map),"x",size(map[1]),").") } function add_obstacle(pos, off, inc_trust) { @@ -57,7 +57,7 @@ function add_obstacle(pos, off, inc_trust) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { #log("Add obstacle in cell: ", xi, " ", yi) - old=map[xi][yi] + var old=map[xi][yi] if(old-inc_trust > 0.0) map[xi][yi] = old-inc_trust else @@ -71,7 +71,7 @@ function remove_obstacle(pos, off, dec_trust) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ #log("Remove obstacle in cell: ", xi, " ", yi) - old=map[xi][yi] + var old=map[xi][yi] if(old + dec_trust < 1.0) #x,y map[xi][yi] = old+dec_trust else @@ -79,6 +79,30 @@ function remove_obstacle(pos, off, dec_trust) { } } + +function get_occupied_cells(cur_cell){ + var i = 1 + var occupied_cells = {} + occupied_cells[0] = cur_cell + occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y) + occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y) + occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1) + occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1) + return occupied_cells +} + +function is_in(dictionary, x, y){ + var i = 0 + while(i < size(dictionary)){ + if(dictionary[i].x == x and dictionary[i].y == y){ + return 1 + } + i = i + 1 + } + return 0 +} + + function table_print(t) { foreach(t, function(key, value) { log(key, " -> ", value) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index ff0630c..b84fec9 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -14,12 +14,12 @@ function UAVinit_map(m_navigation) { # create a map big enough for the goal init_map(2*math.round(math.vec2.length(m_navigation))+10) # center the robot on the grid - cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0)) + cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) } function UAVpathPlanner(m_navigation, m_pos) { # place the robot on the grid - update_curcell(m_pos,0) + cur_cell = getcell(m_pos,0) # create the goal in the map grid mapgoal = math.vec2.add(m_navigation,cur_cell) mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) @@ -34,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,1) + cur_cell = getcell(m_pos,1) log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) log("Going to cell: ", m_goal.x, " ", m_goal.y) @@ -44,26 +44,35 @@ 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, kh4) { +function getcell(m_curpos, kh4) { + var cell = {} 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)) + cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(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)) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + cell = math.vec2.add(cell, m_curpos) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) } - if(cur_cell.x > size(map)) - cur_cell.x = size(map) - if(cur_cell.y > size(map[1])) - cur_cell.y = size(map[1]) - if(cur_cell.x < 1) - cur_cell.x = 1 - if(cur_cell.y < 1) - cur_cell.y = 1 + if(cell.x > size(map)) + cell.x = size(map) + if(cell.y > size(map[1])) + cell.y = size(map[1]) + if(cell.x < 1) + cell.x = 1 + if(cell.y < 1) + cell.y = 1 + + return cell +} + +function updateMap(m_pos) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) } function UAVgetneiobs (m_curpos) { - update_curcell(m_curpos,0) + cur_cell = getcell(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) @@ -72,7 +81,7 @@ function UAVgetneiobs (m_curpos) { function getneiobs (m_curpos) { var foundobstacle = 0 - update_curcell(m_curpos,1) + cur_cell = getcell(m_curpos,1) var old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -118,7 +127,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - update_curcell(m_curpos,1) + cur_cell = getcell(m_curpos,1) reduce(proximity, function(key, value, acc) { @@ -159,19 +168,21 @@ function getproxobs (m_curpos) { } function check_path(m_path, goal_l, m_curpos, kh4) { - pathisblocked = 0 - nb=goal_l - update_curcell(m_curpos,kh4) - Cvec = cur_cell - while(nb < size(m_path) 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, ")") + var pathisblocked = 0 + var nb = goal_l + cur_cell = getcell(m_curpos,kh4) + var Cvec = cur_cell + while(nb <= size(m_path) and nb <= goal_l+1) { + var Nvec = getvec(m_path, nb) + if(kh4 == 0) { + Nvec = vec_from_gps(Nvec.x,Nvec.y,1) + Nvec = getcell(Nvec,kh4) + } + if(doesItIntersect(Cvec, Nvec)) { + log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") pathisblocked = 1 } - Cvec=Nvec + Cvec = Nvec nb = nb + 1 } @@ -184,12 +195,16 @@ function RRTSTAR(GOAL,TOL) { RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive - goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} + var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} #table_print(goalBoundary) - numberOfPoints = 1 - arrayOfPoints = {} + var numberOfPoints = 1 + var arrayOfPoints = {} + arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} Path = {} + mat_copyrow(Path,1,arrayOfPoints,1) + # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance Q = {} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} goalReached = 0; timeout = 0 @@ -202,7 +217,7 @@ function RRTSTAR(GOAL,TOL) { pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) #log("Point generated: ", pt.x, " ", pt.y) - pointList = findPointsInRadius(pt,Q,RADIUS); + var pointList = findPointsInRadius(pt,Q,RADIUS); # Find connection that provides the least cost to come nbCount = 0; @@ -210,7 +225,7 @@ function RRTSTAR(GOAL,TOL) { minCounter = 0; if(size(pointList) != 0) { - #log("Found ", size(pointList), " close point:", pointList.mat) + log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) ipt=1 while(ipt <= size(pointList)) { pointNumber = {} @@ -224,7 +239,7 @@ function RRTSTAR(GOAL,TOL) { nbCount = nbCount + 1; if(intersects != 1) { #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") - distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] + var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] if(distance < minCounted) { minCounted = distance; @@ -235,9 +250,11 @@ function RRTSTAR(GOAL,TOL) { } if(minCounter > 0) { numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][2]=pt.y + Q[numberOfPoints] = {} Q[numberOfPoints][1] = pt.x Q[numberOfPoints][2] = pt.y Q[numberOfPoints][3] = pointList[minCounter][4] @@ -249,9 +266,10 @@ function RRTSTAR(GOAL,TOL) { # Now check to see if any of the other points can be redirected nbCount = 0; ipt = 1 - while(ipt < size(pointList)) { + while(ipt <= size(pointList)) { pointNumber = {} mat_copyrow(pointNumber,1,pointList,ipt) + #log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4]) # Follow the line to see if it intersects anything intersects = doesItIntersect(pt,getvec(pointNumber,1)); @@ -262,6 +280,7 @@ function RRTSTAR(GOAL,TOL) { if(intersects != 1) { # If the alternative path is shorter than change it tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt)) + #log("Q: ", size(Q), "x", size(Q[1])) if(tmpdistance < Q[pointNumber[1][4]][5]) { Q[pointNumber[1][4]][3] = numberOfPoints Q[pointNumber[1][4]][5] = tmpdistance @@ -285,9 +304,11 @@ function RRTSTAR(GOAL,TOL) { # If there is no intersection we need to add to the tree if(intersects != 1) { numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][2]=pt.y + Q[numberOfPoints] = {} Q[numberOfPoints][1] = pt.x Q[numberOfPoints][2] = pt.y Q[numberOfPoints][3] = pointNum @@ -320,51 +341,51 @@ function RRTSTAR(GOAL,TOL) { function findClosestPoint(point,aPt) { # Go through each points and find the distances between them and the # target point - distance = 999 - pointNumber = -1 - ifcp=1 + var distance = 999 + var pointNb = -1 + var ifcp=1 while(ifcp <= size(aPt)) { - range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) if(range < distance) { distance = range; - pointNumber = ifcp; + pointNb = ifcp; } ifcp = ifcp + 1 } - return pointNumber + return pointNb } # Find the closest point in the tree function findPointsInRadius(point,q,r) { - pointList = {} + var ptList = {} var counted = 0; var iir = 1 while(iir <= size(q)) { - tmpvv = getvec(q,iir) + var tmpvv = getvec(q,iir) #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) - distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) if(distance < r) { counted = counted+1; - mat_copyrow(pointList,counted,q,iir) + mat_copyrow(ptList,counted,q,iir) } iir = iir + 1 } - return pointList + return ptList } # check if the line between 2 point intersect an obstacle function doesItIntersect(point,vector) { #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) - dif = math.vec2.sub(point,vector) - distance = math.vec2.length(dif) - if(distance==0.0){ + var dif = math.vec2.sub(point,vector) + var distance = math.vec2.length(dif) + if(distance == 0.0){ # Find what block we're in right now var xi = math.round(point.x) #+1 var yi = math.round(point.y) #+1 - if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) { if(map[xi][yi] > 0.5) return 1 else @@ -372,21 +393,22 @@ function doesItIntersect(point,vector) { } else return 0 } - vec = math.vec2.scale(dif,1.0/distance) - pathstep = size(map[1]) - 2 + var vec = math.vec2.scale(dif,1.0/distance) + var pathstep = size(map) - 2 idii = 1.0 while(idii <= pathstep) { - range = distance*idii/pathstep - pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); + var range = distance*idii/pathstep + var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); # Find what block we're in right now var xi = math.round(pos_chk.x) #+1 var yi = math.round(pos_chk.y) #+1 - #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) + #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")") - if(xi!=cur_cell.x and yi!=cur_cell.y){ - if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) { + if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){ + #if(xi!=cur_cell.x and yi!=cur_cell.y) { + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { if(map[xi][yi] < 0.5) { # because obstacle use trust values #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!") return 1; @@ -403,20 +425,28 @@ function doesItIntersect(point,vector) { } function getPathGPS(Q,nb){ + #log("-----------CONVERTING PATH!!!") path={} var npt=0 + var isrecursive = 0 # get the path from the point list - while(nb!=1){ + while(nb > 1 and isrecursive!=1){ npt=npt+1 path[npt] = {} path[npt][1]=Q[nb][1] path[npt][2]=Q[nb][2] - nb = Q[nb][3]; + if(nb != Q[Q[nb][3]][3]) + nb = Q[nb][3]; + else { + isrecursive = 1 + path={} + log("ERROR - Recursive path !!!") + } } # re-order the list and make the path points absolute pathR={} - var totpt = npt + var totpt = npt + 1 while(npt > 0){ tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) pathR[totpt-npt] = {} @@ -433,7 +463,7 @@ function getPath(Q,nb){ var npt=0 # log("get the path from the point list") - while(nb!=1){ + while(nb > 1){ npt=npt+1 path[npt] = {} path[npt][1]=Q[nb][1] @@ -444,7 +474,7 @@ function getPath(Q,nb){ # log("re-order the list") # table_print(path.mat) pathR={} - var totpt = npt + var totpt = npt + 1 while(npt > 0){ tmpgoal = getvec(path,npt) pathR[totpt-npt] = {} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 843f34f..501416d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,6 +3,7 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## +include "vec2.bzz" include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. @@ -14,7 +15,6 @@ 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) @@ -34,6 +34,7 @@ function idle() { function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff + homegps = {.lat=position.latitude, .long=position.longitude} if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) @@ -61,7 +62,7 @@ function land() { function set_goto(transf) { UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf) + gotoWPRRT(transf) } if(rc_goto.id==id){ @@ -93,10 +94,8 @@ function picture() { # 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) - homegps.lat = position.latitude - homegps.long = position.longitude - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-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) @@ -104,35 +103,36 @@ function gotoWPRRT(transf) { else { if(Path==nil){ # create the map - if(map==nil) + if(map==nil) { UAVinit_map(rc_goal) + homegps.lat = position.latitude + homegps.long = position.longitude + } # add proximity sensor and neighbor obstacles to the map while(Path==nil) { - #getproxobs(m_pos) - UAVgetneiobs (m_pos) + updateMap(m_pos) Path = UAVpathPlanner(rc_goal, m_pos) } cur_goal_l = 1 } else if(cur_goal_l <= size(Path)) { - cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude - cur_pt=vec_from_gps(cur_gps.x,cur_gps.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)) { + var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude + var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) + print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) + if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { + updateMap(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) + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) while(Path == nil) { - #getproxobs(m_pos) - UAVgetneiobs (m_pos) + updateMap(m_pos) Path = UAVpathPlanner(rc_goal, m_pos) } cur_goal_l = 1 + } else { + cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) } - 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) } else cur_goal_l = cur_goal_l + 1 @@ -262,14 +262,18 @@ function LimitAngle(angle){ return angle } -function vec_from_gps(lat, lon) { - d_lon = lon - position.longitude +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - position.longitude d_lat = lat - position.latitude + if(home_ref == 1) { + d_lon = lon - homegps.long + d_lat = lat - homegps.lat + } ned_x = d_lat/180*math.pi * 6371000.0; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) + return math.vec2.new(ned_x,ned_y) } function gps_from_vec(vec) { diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testRRT.bzz similarity index 76% rename from buzz_scripts/testalone.bzz rename to buzz_scripts/testRRT.bzz index 7460de4..7590470 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testRRT.bzz @@ -1,4 +1,3 @@ -include "vec2.bzz" include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. @@ -6,15 +5,18 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5085,-73.1531935979886,5.0) - set_goto(picture) + uav_storegoal(45.5088103899,-73.1540826153,25.0) + set_goto(idle) } # Executed once at init time. function init() { - statef=turnedoff - UAVSTATE = "TURNEDOFF" uav_initstig() + uav_initswarm() + #statef=turnedoff + #UAVSTATE = "TURNEDOFF" + statef = takeoff + UAVSTATE = "TAKEOFF" } # Executed at each time step. diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 97b92bf..e438fb1 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -2,7 +2,6 @@ include "vec2.bzz" include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" function action() { uav_storegoal(-1.0,-1.0,-1.0) @@ -13,7 +12,6 @@ function action() { function init() { statef=turnedoff UAVSTATE = "TURNEDOFF" - uav_initstig() TARGET_ALTITUDE = 15.0 } From d532dd22512c94d709b5c47702df15e641df4ab7 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 12:20:43 -0500 Subject: [PATCH 02/77] generalized some parts of RRT and complete export occ_grid --- CMakeLists.txt | 1 + buzz_scripts/include/rrtstar.bzz | 124 ++++++++++------------------- buzz_scripts/include/uavstates.bzz | 8 +- include/buzzuav_closures.h | 4 + include/roscontroller.h | 5 ++ package.xml | 2 + src/buzzuav_closures.cpp | 35 ++++---- src/roscontroller.cpp | 44 +++++++++- 8 files changed, 125 insertions(+), 98 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b7e4b0..672d68d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs mavros_msgs sensor_msgs + nav_msgs message_generation ) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index b84fec9..9777dde 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -6,54 +6,41 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 +GSCALE = {.x=1, .y=1} map = nil cur_cell = {} nei_cell = {} -function UAVinit_map(m_navigation) { +# create a map with its length to fit the goal (rel. pos.) and the current position as the center +function dyn_init_map(m_navigation) { # create a map big enough for the goal - init_map(2*math.round(math.vec2.length(m_navigation))+10) + init_map(math.round(2*math.vec2.length(m_navigation))+10) # center the robot on the grid cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) } -function UAVpathPlanner(m_navigation, m_pos) { +# start the RRT* to reach the goal (rel. pos.) +function pathPlanner(m_goal, m_pos, kh4) { # place the robot on the grid - cur_cell = getcell(m_pos,0) + cur_cell = getcell(m_pos) # create the goal in the map grid - mapgoal = math.vec2.add(m_navigation,cur_cell) - mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) + mapgoal = getcell(math.vec2.add(m_goal, m_pos)) + + #print_map(map) + #export_map(map) # search for a path - return RRTSTAR(mapgoal,math.vec2.new(5,5)) #size(map[1])/20.0,size(map[1])/20.0)) + return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) } -function Kh4pathPlanner(m_goal, m_pos) { - # move 0,0 to a corner instead of the center - m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - 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 - cur_cell = getcell(m_pos,1) - log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) - log("Going to cell: ", m_goal.x, " ", m_goal.y) - - # search for a path - print_map(map) - # print_map_argos(map) - return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) -} - -function getcell(m_curpos, kh4) { +function getcell(m_curpos) { var cell = {} - if(kh4) { # for khepera playground - cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y)) - } else { # for uav map relative to home - cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) - cell = math.vec2.add(cell, m_curpos) - cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) - } + # relative to center (start position) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y) + cell = math.vec2.add(cell, offset) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) + if(cell.x > size(map)) cell.x = size(map) if(cell.y > size(map[1])) @@ -72,7 +59,7 @@ function updateMap(m_pos) { } function UAVgetneiobs (m_curpos) { - cur_cell = getcell(m_curpos,0) + cur_cell = getcell(m_curpos) # 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) @@ -81,7 +68,7 @@ function UAVgetneiobs (m_curpos) { function getneiobs (m_curpos) { var foundobstacle = 0 - cur_cell = getcell(m_curpos,1) + cur_cell = getcell(m_curpos) var old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -127,7 +114,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - cur_cell = getcell(m_curpos,1) + cur_cell = getcell(m_curpos) reduce(proximity, function(key, value, acc) { @@ -170,13 +157,13 @@ function getproxobs (m_curpos) { function check_path(m_path, goal_l, m_curpos, kh4) { var pathisblocked = 0 var nb = goal_l - cur_cell = getcell(m_curpos,kh4) + cur_cell = getcell(m_curpos) var Cvec = cur_cell while(nb <= size(m_path) and nb <= goal_l+1) { var Nvec = getvec(m_path, nb) if(kh4 == 0) { Nvec = vec_from_gps(Nvec.x,Nvec.y,1) - Nvec = getcell(Nvec,kh4) + Nvec = getcell(Nvec) } if(doesItIntersect(Cvec, Nvec)) { log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") @@ -225,7 +212,7 @@ function RRTSTAR(GOAL,TOL) { minCounter = 0; if(size(pointList) != 0) { - log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) + #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) ipt=1 while(ipt <= size(pointList)) { pointNumber = {} @@ -329,8 +316,7 @@ function RRTSTAR(GOAL,TOL) { } if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPathGPS(Q,numberOfPoints) - print_pos(Path) + Path = getPath(Q,numberOfPoints, 1) } else { log("FAILED TO FIND A PATH!!!") Path = nil @@ -424,62 +410,40 @@ function doesItIntersect(point,vector) { return 0 } -function getPathGPS(Q,nb){ +# create a table with only the path's points in order +function getPath(Q,nb,gps){ #log("-----------CONVERTING PATH!!!") - path={} + var pathL={} var npt=0 - var isrecursive = 0 # get the path from the point list - while(nb > 1 and isrecursive!=1){ + while(nb > 1) { npt=npt+1 - path[npt] = {} - path[npt][1]=Q[nb][1] - path[npt][2]=Q[nb][2] + pathL[npt] = {} + pathL[npt][1]=Q[nb][1] + pathL[npt][2]=Q[nb][2] if(nb != Q[Q[nb][3]][3]) nb = Q[nb][3]; else { - isrecursive = 1 - path={} log("ERROR - Recursive path !!!") + return nil } } - # re-order the list and make the path points absolute - pathR={} - var totpt = npt + 1 - while(npt > 0){ - tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) - pathR[totpt-npt] = {} - pathR[totpt-npt][1]=tmpgoal.latitude - pathR[totpt-npt][2]=tmpgoal.longitude - npt = npt - 1 - } - return pathR -} - -# create a table with only the path's points in order -function getPath(Q,nb){ - path={} - var npt=0 - - # log("get the path from the point list") - while(nb > 1){ - npt=npt+1 - path[npt] = {} - path[npt][1]=Q[nb][1] - path[npt][2]=Q[nb][2] - nb = Q[nb][3]; - } - # log("re-order the list") # table_print(path.mat) - pathR={} + var pathR={} var totpt = npt + 1 while(npt > 0){ - tmpgoal = getvec(path,npt) pathR[totpt-npt] = {} - pathR[totpt-npt][1]=tmpgoal.x - pathR[totpt-npt][2]=tmpgoal.y + if(gps) { + tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell)) + pathR[totpt-npt][1]=tmpgoal.latitude + pathR[totpt-npt][2]=tmpgoal.longitude + } else { + tmpgoal = getvec(path,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y + } npt = npt - 1 } #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 501416d..d3b015d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -104,15 +104,16 @@ function gotoWPRRT(transf) { if(Path==nil){ # create the map if(map==nil) { - UAVinit_map(rc_goal) + dyn_init_map(rc_goal) homegps.lat = position.latitude homegps.long = position.longitude } # add proximity sensor and neighbor obstacles to the map while(Path==nil) { updateMap(m_pos) - Path = UAVpathPlanner(rc_goal, m_pos) + Path = pathPlanner(rc_goal, m_pos) } + print_pos(Path) cur_goal_l = 1 } else if(cur_goal_l <= size(Path)) { var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude @@ -126,8 +127,9 @@ function gotoWPRRT(transf) { rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) while(Path == nil) { updateMap(m_pos) - Path = UAVpathPlanner(rc_goal, m_pos) + Path = pathPlanner(rc_goal, m_pos) } + print_pos(Path) cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 56f0bd6..f14e1a7 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -94,6 +94,10 @@ void set_currentpos(double latitude, double longitude, double altitude); * returns the current go to position */ double* getgoto(); +/* + * returns the current grid + */ +std::map> getgrid(); /* * returns the current Buzz state */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 17533a4..0e9e547 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" @@ -124,6 +125,7 @@ private: ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; ros::Publisher uavstate_pub; + ros::Publisher grid_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; @@ -179,6 +181,9 @@ private: /*UAVState publisher*/ void uavstate_publisher(); + /*Grid publisher*/ + void grid_publisher(); + /*BVM message payload publisher*/ void send_MPpayload(); diff --git a/package.xml b/package.xml index 6f4fc98..0fa6866 100644 --- a/package.xml +++ b/package.xml @@ -48,6 +48,8 @@ mavros_msgs sensor_msgs sensor_msgs + nav_msgs + nav_msgs message_generation message_runtime diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6b380d6..980948b 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -36,6 +36,7 @@ std::map targets_map; std::map wplist_map; std::map neighbors_map; std::map neighbors_status_map; +std::map> grid; /****************************************/ /****************************************/ @@ -178,26 +179,24 @@ int buzz_exportmap(buzzvm_t vm) / Buzz closure to export a 2D map /----------------------------------------*/ { - /* Make sure one parameter has been passed */ buzzvm_lnum_assert(vm, 1); - /* Get the parameter */ + // Get the parameter buzzvm_lload(vm, 1); - buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // matrix - /* Get the table */ + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzobj_t t = buzzvm_stack_at(vm, 1); - /* Copy the values into a vector */ - std::vector mat; - for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i) - { - /* Duplicate the table */ + for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) { buzzvm_dup(vm); - /* Push the index */ buzzvm_pushi(vm, i); - /* Get the value */ buzzvm_tget(vm); - /* Store it in the vector (assume all values are float, no mistake...) */ - mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value); - /* Get rid of the value, now useless */ + std::map row; + for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { + buzzvm_dup(vm); + buzzvm_pushi(vm, j); + buzzvm_tget(vm); + row.insert(std::pair(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + buzzvm_pop(vm); + } + grid.insert(std::pair>(i,row)); buzzvm_pop(vm); } return buzzvm_ret0(vm); @@ -457,6 +456,14 @@ double* getgoto() return goto_pos; } +std::map> getgrid() +/* +/ return the grid +/-------------------------------------------------------------*/ +{ + return grid; +} + float* getgimbal() /* / return current gimbal commands diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9fe34f9..c5a4354 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -114,8 +114,10 @@ void roscontroller::RosControllerRun() // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { - // Neighbours of the robot published with id in respective topic + // Publish topics neighbours_pos_publisher(); + uavstate_publisher(); + grid_publisher(); send_MPpayload(); // Check updater state and step code update_routine(); @@ -337,6 +339,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); uavstate_pub = n_c.advertise("uavstate", 5); + grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); // Service Clients @@ -447,6 +450,45 @@ void roscontroller::uavstate_publisher() uavstate_pub.publish(uavstate_msg); } +void roscontroller::grid_publisher() +/* +/ Publish current Grid from Buzz script +/----------------------------------------------------*/ +{ + std::map> grid = buzzuav_closures::getgrid(); + std::map>::iterator itr = grid.begin(); + int g_w = itr->second.size(); + int g_h = grid.size(); + + if(g_w!=0 && g_h!=0) { + auto current_time = ros::Time::now(); + nav_msgs::OccupancyGrid grid_msg; + grid_msg.header.frame_id = "/world"; + grid_msg.header.stamp = current_time; + grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. + //grid_msg.info.resolution = gridMap.getResolution(); + grid_msg.info.width = g_w; + grid_msg.info.height = g_h; + grid_msg.info.origin.position.x = 0.0; + grid_msg.info.origin.position.y = 0.0; + grid_msg.info.origin.position.z = 0.0; + grid_msg.info.origin.orientation.x = 0.0; + grid_msg.info.origin.orientation.y = 0.0; + grid_msg.info.origin.orientation.z = 0.0; + grid_msg.info.origin.orientation.w = 1.0; + grid_msg.data.resize(g_w*g_h); + + for (itr=grid.begin(); itr!=grid.end(); ++itr) { + std::map::iterator itc = itr->second.begin(); + for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { + grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0); + } + } + grid_pub.publish(grid_msg); + } +} + + void roscontroller::Arm() /* / Functions handling the local MAV ROS flight controller From 50368d84d33fd63833dfa6f57dad8ff6bb1354ff Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 13:45:22 -0500 Subject: [PATCH 03/77] fix rviz res --- buzz_scripts/include/rrtstar.bzz | 2 +- src/buzzuav_closures.cpp | 6 ++++-- src/roscontroller.cpp | 9 ++++++--- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 9777dde..2c2a6d5 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -27,7 +27,7 @@ function pathPlanner(m_goal, m_pos, kh4) { mapgoal = getcell(math.vec2.add(m_goal, m_pos)) #print_map(map) - #export_map(map) + export_map(map) # search for a path return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 980948b..a58c8f2 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -184,12 +184,12 @@ int buzz_exportmap(buzzvm_t vm) buzzvm_lload(vm, 1); buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzobj_t t = buzzvm_stack_at(vm, 1); - for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) { + for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) { buzzvm_dup(vm); buzzvm_pushi(vm, i); buzzvm_tget(vm); std::map row; - for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { + for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { buzzvm_dup(vm); buzzvm_pushi(vm, j); buzzvm_tget(vm); @@ -199,6 +199,8 @@ int buzz_exportmap(buzzvm_t vm) grid.insert(std::pair>(i,row)); buzzvm_pop(vm); } + // DEBUG + // ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value)); return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c5a4354..2bf5cab 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -461,12 +461,14 @@ void roscontroller::grid_publisher() int g_h = grid.size(); if(g_w!=0 && g_h!=0) { + // DEBUG + //ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w); auto current_time = ros::Time::now(); nav_msgs::OccupancyGrid grid_msg; grid_msg.header.frame_id = "/world"; grid_msg.header.stamp = current_time; grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. - //grid_msg.info.resolution = gridMap.getResolution(); + grid_msg.info.resolution = 0.1;//gridMap.getResolution(); grid_msg.info.width = g_w; grid_msg.info.height = g_h; grid_msg.info.origin.position.x = 0.0; @@ -476,12 +478,13 @@ void roscontroller::grid_publisher() grid_msg.info.origin.orientation.y = 0.0; grid_msg.info.origin.orientation.z = 0.0; grid_msg.info.origin.orientation.w = 1.0; - grid_msg.data.resize(g_w*g_h); + grid_msg.data.resize(g_w * g_h); for (itr=grid.begin(); itr!=grid.end(); ++itr) { std::map::iterator itc = itr->second.begin(); for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { - grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0); + //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0)); + grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0); } } grid_pub.publish(grid_msg); From 63f9111d2f56636de8138d357630572660919412 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 14:33:23 -0500 Subject: [PATCH 04/77] made rrtstar a state --- buzz_scripts/include/barrier.bzz | 2 +- buzz_scripts/include/rrtstar.bzz | 156 ++++++++++++++--------------- buzz_scripts/include/uavstates.bzz | 117 +++++++++++----------- buzz_scripts/testRRT.bzz | 4 +- src/buzzuav_closures.cpp | 2 +- 5 files changed, 136 insertions(+), 145 deletions(-) diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index e280a66..31a452e 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -32,7 +32,7 @@ function barrier_set(threshold, transf, resumef, bdt) { statef = function() { barrier_wait(threshold, transf, resumef, bdt); } - UAVSTATE = "BARRIERWAIT" + BVMSTATE = "BARRIERWAIT" barrier_create() } diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 2c2a6d5..86d1ce0 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -6,10 +6,13 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 +RRT_RUNSTEP = 3 +PROX_SENSOR_THRESHOLD = 0.11 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} nei_cell = {} +mapgoal = {} # create a map with its length to fit the goal (rel. pos.) and the current position as the center function dyn_init_map(m_navigation) { @@ -29,8 +32,25 @@ function pathPlanner(m_goal, m_pos, kh4) { #print_map(map) export_map(map) + # Initialize RRTStar var + log("--------Initialize RRTStar--------------") + HEIGHT = size(map) + WIDTH = size(map[1]) + RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive + goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0} + #table_print(goalBoundary) + numberOfPoints = 1 + arrayOfPoints = {} + arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} + # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance + Q = {} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} + goalReached = 0 + timeout = 0 + # search for a path - return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) + old_statef = statef + rrtstar() } function getcell(m_curpos) { @@ -53,12 +73,13 @@ function getcell(m_curpos) { return cell } -function updateMap(m_pos) { +function populateGrid(m_pos) { #getproxobs(m_pos) - UAVgetneiobs (m_pos) + getneiobs (m_pos) } -function UAVgetneiobs (m_curpos) { +# TODO: populate the map with a blob instead? +function getneiobs (m_curpos) { cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { @@ -66,54 +87,47 @@ function UAVgetneiobs (m_curpos) { }) } -function getneiobs (m_curpos) { - var foundobstacle = 0 - cur_cell = getcell(m_curpos) - var old_nei = table_copy(nei_cell) - #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) +# old function tested with the khepera for reference +# DS 20/11 +# function getneiobs (m_curpos) { +# var foundobstacle = 0 +# cur_cell = getcell(m_curpos) +# var old_nei = table_copy(nei_cell) - neighbors.foreach(function(rid, data) { - #log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg") - Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) - #log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm") - Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) - nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} - #log("Neighbor in : ", Ncell.x, " ", Ncell.y) - if(old_nei[rid]!=nil) { - if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { - #log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) - add_obstacle(Ncell, 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) - foundobstacle = 1 - } - } else { - add_obstacle(Ncell, 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) - foundobstacle = 1 - } - }) - - #if(foundobstacle) { - #print_map(map) - #} - - return foundobstacle -} +# neighbors.foreach(function(rid, data) { +# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) +# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) +# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) +# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} +# if(old_nei[rid]!=nil) { +# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# } else { +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# }) +# return foundobstacle +# } +# populate a line in front of the sensor using plateform independant proximity sensing function getproxobs (m_curpos) { - foundobstacle = 0 + var foundobstacle = 0 cur_cell = getcell(m_curpos) reduce(proximity, @@ -125,7 +139,7 @@ function getproxobs (m_curpos) { obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value > IR_SENSOR_THRESHOLD) { + if(value.value > PROX_SENSOR_THRESHOLD) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) { add_obstacle(obs, 0, 0.25) add_obstacle(obs2, 0, 0.25) @@ -142,15 +156,6 @@ function getproxobs (m_curpos) { return acc }, math.vec2.new(0, 0)) - #if(foundobstacle) { - # reduce(proximity, - # function(key, value, acc){ - # log(value.value, ", ", value.angle) - # return acc - # }, math.vec2.new(0, 0)) - # print_map(map) - #} - return foundobstacle } @@ -176,29 +181,16 @@ function check_path(m_path, goal_l, m_curpos, kh4) { return pathisblocked } -function RRTSTAR(GOAL,TOL) { - HEIGHT = size(map) - WIDTH = size(map[1]) - RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive +function rrtstar() { + # update state machine variables + statef = rrtstar + BVMSTATE = "RRTSTAR" - - var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} - #table_print(goalBoundary) - var numberOfPoints = 1 - var arrayOfPoints = {} - arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} - Path = {} - mat_copyrow(Path,1,arrayOfPoints,1) - # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance - Q = {} - Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} - - goalReached = 0; - timeout = 0 ## # main search loop ## - while(goalReached == 0 and timeout < RRT_TIMEOUT) { + step_timeout = 0 + while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { # Point generation only as grid cell centers pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) @@ -313,15 +305,19 @@ function RRTSTAR(GOAL,TOL) { log(numberOfPoints, " points processed. Still looking for goal."); } timeout = timeout + 1 + step_timeout = step_timeout + 1 } + if(goalReached){ log("Goal found(",numberOfPoints,")!") Path = getPath(Q,numberOfPoints, 1) + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef } else { - log("FAILED TO FIND A PATH!!!") + #log("FAILED TO FIND A PATH!!!") Path = nil } - return Path } function findClosestPoint(point,aPt) { diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index d3b015d..10f1cfd 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -7,7 +7,7 @@ include "vec2.bzz" include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. -UAVSTATE = "TURNEDOFF" +BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 2 # m/steps GOTO_MAXDIST = 150 # m. @@ -23,16 +23,16 @@ function uav_initswarm() { function turnedoff() { statef=turnedoff - UAVSTATE = "TURNEDOFF" + BVMSTATE = "TURNEDOFF" } function idle() { statef=idle - UAVSTATE = "IDLE" + BVMSTATE = "IDLE" } function takeoff() { - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" statef=takeoff homegps = {.lat=position.latitude, .long=position.longitude} @@ -47,7 +47,7 @@ function takeoff() { } function land() { - UAVSTATE = "LAND" + BVMSTATE = "LAND" statef=land neighbors.broadcast("cmd", 21) @@ -60,7 +60,7 @@ function land() { } function set_goto(transf) { - UAVSTATE = "GOTOGPS" + BVMSTATE = "GOTOGPS" statef=function() { gotoWPRRT(transf) } @@ -79,7 +79,7 @@ function set_goto(transf) { ptime=0 function picture() { statef=picture - UAVSTATE="PICTURE" + BVMSTATE="PICTURE" uav_setgimbal(0.0, 0.0, -90.0, 20.0) if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize uav_takepicture() @@ -98,50 +98,45 @@ function gotoWPRRT(transf) { m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-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) + if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) { log("Sorry this is too far.") - else { - if(Path==nil){ - # create the map - if(map==nil) { - dyn_init_map(rc_goal) - homegps.lat = position.latitude - homegps.long = position.longitude + return + } + # create the map + if(map==nil) { + dyn_init_map(rc_goal) + homegps.lat = position.latitude + homegps.long = position.longitude + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + pathPlanner(rc_goal, m_pos) + cur_goal_l = 1 + } else if(cur_goal_l <= size(Path)) { + var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude + var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) + print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) + if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { + populateGrid(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,0) + # re-start the planner + pathPlanner(rc_goal, m_pos) + cur_goal_l = 1 + } else { + cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) } - # add proximity sensor and neighbor obstacles to the map - while(Path==nil) { - updateMap(m_pos) - Path = pathPlanner(rc_goal, m_pos) - } - print_pos(Path) - cur_goal_l = 1 - } else if(cur_goal_l <= size(Path)) { - var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude - var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) - print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) - if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { - updateMap(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,0) - while(Path == nil) { - updateMap(m_pos) - Path = pathPlanner(rc_goal, m_pos) - } - print_pos(Path) - cur_goal_l = 1 - } else { - cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) - } - } - else - cur_goal_l = cur_goal_l + 1 - } else { - Path = nil - transf() - } + } else + cur_goal_l = cur_goal_l + 1 + } else { + Path = nil + transf() } } @@ -162,7 +157,7 @@ function gotoWP(transf) { function follow() { if(size(targets)>0) { - UAVSTATE = "FOLLOW" + BVMSTATE = "FOLLOW" statef=follow attractor=math.vec2.newp(0,0) foreach(targets, function(id, tab) { @@ -181,18 +176,18 @@ function uav_rccmd() { log("cmd 22") flight.rc_cmd=0 statef = takeoff - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { log("cmd 21") log("To land") flight.rc_cmd=0 statef = land - UAVSTATE = "LAND" + BVMSTATE = "LAND" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "GOTOGPS" + BVMSTATE = "GOTOGPS" statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 @@ -227,16 +222,16 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") - if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") { statef=takeoff - UAVSTATE = "TAKEOFF" - } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { + BVMSTATE = "TAKEOFF" + } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") { statef=land - UAVSTATE = "LAND" - } else if(value==400 and UAVSTATE=="TURNEDOFF") { + BVMSTATE = "LAND" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { uav_arm() - } else if(value==401 and UAVSTATE=="TURNEDOFF"){ + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ uav_disarm() } else if(value==900){ rc_State = 0 @@ -246,7 +241,7 @@ function uav_neicmd() { rc_State = 2 } else if(value==903){ rc_State = 3 - } else if(value==16 and UAVSTATE=="IDLE"){ + } else if(value==16 and BVMSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) # # if(gt.id == id) statef=goto diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index 7590470..7478529 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -16,7 +16,7 @@ function init() { #statef=turnedoff #UAVSTATE = "TURNEDOFF" statef = takeoff - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -25,7 +25,7 @@ function step() { statef() - log("Current state: ", UAVSTATE) + log("Current state: ", BVMSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a58c8f2..c61d9a3 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -480,7 +480,7 @@ string getuavstate() ---------------------------------------*/ { static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); buzzvm_pop(VM); From df11060f84b269a57db13b359e853b46c969aa94 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 14:51:00 -0500 Subject: [PATCH 05/77] add comments and TODOs --- buzz_scripts/include/rrtstar.bzz | 43 +++++++++++++++++++------------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 86d1ce0..89fb106 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -15,6 +15,7 @@ nei_cell = {} mapgoal = {} # create a map with its length to fit the goal (rel. pos.) and the current position as the center +# TODO: test with khepera/argos function dyn_init_map(m_navigation) { # create a map big enough for the goal init_map(math.round(2*math.vec2.length(m_navigation))+10) @@ -23,6 +24,7 @@ function dyn_init_map(m_navigation) { } # start the RRT* to reach the goal (rel. pos.) +# TODO: test with khepera/argos function pathPlanner(m_goal, m_pos, kh4) { # place the robot on the grid cur_cell = getcell(m_pos) @@ -53,6 +55,7 @@ function pathPlanner(m_goal, m_pos, kh4) { rrtstar() } +# get the grid cell position (x,y) equivalent to the input position function getcell(m_curpos) { var cell = {} # relative to center (start position) @@ -78,7 +81,7 @@ function populateGrid(m_pos) { getneiobs (m_pos) } -# TODO: populate the map with a blob instead? +# TODO: populate the map with neighors as blobs instead ? function getneiobs (m_curpos) { cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid @@ -126,6 +129,7 @@ function getneiobs (m_curpos) { # } # populate a line in front of the sensor using plateform independant proximity sensing +# TODO: adapt to M100 stereo function getproxobs (m_curpos) { var foundobstacle = 0 cur_cell = getcell(m_curpos) @@ -159,6 +163,8 @@ function getproxobs (m_curpos) { return foundobstacle } +# check if any obstacle blocks the way +# TODO: remove the kh4 bool for retro-compatilibty function check_path(m_path, goal_l, m_curpos, kh4) { var pathisblocked = 0 var nb = goal_l @@ -181,14 +187,15 @@ function check_path(m_path, goal_l, m_curpos, kh4) { return pathisblocked } +## +# main search loop as a state +## function rrtstar() { # update state machine variables statef = rrtstar BVMSTATE = "RRTSTAR" - ## - # main search loop - ## + step_timeout = 0 while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { @@ -309,20 +316,23 @@ function rrtstar() { } if(goalReached){ - log("Goal found(",numberOfPoints,")!") - Path = getPath(Q,numberOfPoints, 1) - # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef - } else { - #log("FAILED TO FIND A PATH!!!") + log("Goal found(",numberOfPoints,")!") + Path = getPath(Q,numberOfPoints, 1) + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef + } else if(timeout >= RRT_TIMEOUT) { + log("FAILED TO FIND A PATH!!!") Path = nil + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef } } +# Go through each points and find the distances between them and the +# target point function findClosestPoint(point,aPt) { - # Go through each points and find the distances between them and the - # target point var distance = 999 var pointNb = -1 var ifcp=1 @@ -388,6 +398,7 @@ function doesItIntersect(point,vector) { var yi = math.round(pos_chk.y) #+1 #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")") + # TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){ #if(xi!=cur_cell.x and yi!=cur_cell.y) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { @@ -408,7 +419,6 @@ function doesItIntersect(point,vector) { # create a table with only the path's points in order function getPath(Q,nb,gps){ - #log("-----------CONVERTING PATH!!!") var pathL={} var npt=0 # get the path from the point list @@ -419,14 +429,13 @@ function getPath(Q,nb,gps){ pathL[npt][2]=Q[nb][2] if(nb != Q[Q[nb][3]][3]) nb = Q[nb][3]; - else { + else { # TODO: why is this happening? log("ERROR - Recursive path !!!") return nil } } - # log("re-order the list") - # table_print(path.mat) + # Re-Order the list. var pathR={} var totpt = npt + 1 while(npt > 0){ From b30bd6201830b383635e119b61f564db6ebae4fc Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 16:10:30 -0500 Subject: [PATCH 06/77] fix proxmity topic --- buzz_scripts/testRRT.bzz | 14 ++++++++++---- launch/launch_config/topics.yaml | 1 + src/roscontroller.cpp | 9 +++++---- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index 7478529..755bc4f 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -13,10 +13,10 @@ function action() { function init() { uav_initstig() uav_initswarm() - #statef=turnedoff - #UAVSTATE = "TURNEDOFF" - statef = takeoff - BVMSTATE = "TAKEOFF" + statef=turnedoff + BVMSTATE = "TURNEDOFF" + #statef = takeoff + #BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -26,6 +26,12 @@ function step() { statef() log("Current state: ", BVMSTATE) + log("Obstacles: ") + reduce(proximity, + function(key, value, acc) { + log(key, " - ", value.angle, value.value) + return acc + }, math.vec2.new(0, 0)) } # Executed once when the robot (or the simulator) is reset. diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index e7358cd..9e33d37 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -10,3 +10,4 @@ topics: localpos: local_position/pose stream: set_stream_rate altitude: global_position/rel_alt + obstacles: obstacles diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2bf5cab..11d5913 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -318,7 +318,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) system("rosnode kill rosbuzz_node"); } - node_handle.getParam("obstacles", obstacles_topic); + node_handle.getParam("topics/obstacles", obstacles_topic); } void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) @@ -468,7 +468,7 @@ void roscontroller::grid_publisher() grid_msg.header.frame_id = "/world"; grid_msg.header.stamp = current_time; grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. - grid_msg.info.resolution = 0.1;//gridMap.getResolution(); + grid_msg.info.resolution = 0.01;//gridMap.getResolution(); grid_msg.info.width = g_w; grid_msg.info.height = g_h; grid_msg.info.origin.position.x = 0.0; @@ -483,8 +483,9 @@ void roscontroller::grid_publisher() for (itr=grid.begin(); itr!=grid.end(); ++itr) { std::map::iterator itc = itr->second.begin(); for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { - //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0)); - grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0); + grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second; + // DEBUG + //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]); } } grid_pub.publish(grid_msg); From 292c4fda8a53446cd67cf34762c7ab5e442e9000 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 21 Dec 2017 12:20:26 -0500 Subject: [PATCH 07/77] fix subscriber format, added NED yaw, changed position to absolute_position and fixed proximity obstacles detection --- buzz_scripts/include/rrtstar.bzz | 18 ++--- buzz_scripts/include/uavstates.bzz | 32 ++++----- buzz_scripts/testRRT.bzz | 24 ++++--- include/buzzuav_closures.h | 2 +- include/roscontroller.h | 56 +++++++-------- src/buzzuav_closures.cpp | 11 ++- src/roscontroller.cpp | 108 ++++++++++++++++------------- 7 files changed, 131 insertions(+), 120 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 89fb106..4e3c138 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -7,7 +7,7 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 -PROX_SENSOR_THRESHOLD = 0.11 +PROX_SENSOR_THRESHOLD_M = 10 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} @@ -77,7 +77,7 @@ function getcell(m_curpos) { } function populateGrid(m_pos) { - #getproxobs(m_pos) + getproxobs(m_pos) getneiobs (m_pos) } @@ -131,19 +131,19 @@ function getneiobs (m_curpos) { # populate a line in front of the sensor using plateform independant proximity sensing # TODO: adapt to M100 stereo function getproxobs (m_curpos) { - var foundobstacle = 0 + var updated_obstacle = 0 cur_cell = getcell(m_curpos) reduce(proximity, function(key, value, acc) { - obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) - obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) + obs = getcell(math.vec2.newp(value.value, value.angle + absolute_position.yaw)) + obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw)) per = math.vec2.sub(obs,cur_cell) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value > PROX_SENSOR_THRESHOLD) { + if(value.value < PROX_SENSOR_THRESHOLD_M) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) { add_obstacle(obs, 0, 0.25) add_obstacle(obs2, 0, 0.25) @@ -151,16 +151,16 @@ function getproxobs (m_curpos) { add_obstacle(obsr2, 0, 0.25) add_obstacle(obsl, 0, 0.25) add_obstacle(obsl2, 0, 0.25) - foundobstacle = 1 + updated_obstacle = 1 } } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) { remove_obstacle(obs, 0, 0.05) - foundobstacle = 1 + updated_obstacle = 1 } return acc }, math.vec2.new(0, 0)) - return foundobstacle + return updated_obstacle } # check if any obstacle blocks the way diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 10f1cfd..8e347bf 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -34,13 +34,13 @@ function idle() { function takeoff() { BVMSTATE = "TAKEOFF" statef=takeoff - homegps = {.lat=position.latitude, .long=position.longitude} + homegps = {.lat=absolute_position.latitude, .long=absolute_position.longitude} - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + if( flight.status == 2 and absolute_position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", position.altitude) + log("Altitude: ", absolute_position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } @@ -105,8 +105,8 @@ function gotoWPRRT(transf) { # create the map if(map==nil) { dyn_init_map(rc_goal) - homegps.lat = position.latitude - homegps.long = position.longitude + homegps.lat = absolute_position.latitude + homegps.long = absolute_position.longitude } if(Path==nil) { @@ -122,7 +122,7 @@ function gotoWPRRT(transf) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { populateGrid(m_pos) if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) + uav_moveto(0.0, 0.0, rc_goto.altitude - absolute_position.altitude) Path = nil rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) # re-start the planner @@ -130,7 +130,7 @@ function gotoWPRRT(transf) { cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - absolute_position.altitude) } } else cur_goal_l = cur_goal_l + 1 @@ -148,11 +148,11 @@ function gotoWP(transf) { log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) } function follow() { @@ -260,8 +260,8 @@ function LimitAngle(angle){ } function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - position.longitude - d_lat = lat - position.latitude + d_lon = lon - absolute_position.longitude + d_lat = lat - absolute_position.latitude if(home_ref == 1) { d_lon = lon - homegps.long d_lat = lat - homegps.lat @@ -277,17 +277,17 @@ function gps_from_vec(vec) { Lgoal = {.latitude=0, .longitude=0} Vrange = math.vec2.length(vec) Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude) - latR = position.latitude*math.pi/180.0; - lonR = position.longitude*math.pi/180.0; +# print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude) + latR = absolute_position.latitude*math.pi/180.0; + lonR = absolute_position.longitude*math.pi/180.0; target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); Lgoal.latitude = target_lat*180.0/math.pi; Lgoal.longitude = target_lon*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + position.latitude; + #goal.latitude = d_lat + absolute_position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + position.longitude; + #goal.longitude = d_lon + absolute_position.longitude; return Lgoal } diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index 755bc4f..f586fe5 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -5,7 +5,7 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5088103899,-73.1540826153,25.0) + uav_storegoal(45.5088103899,-73.1540826153,5.0) set_goto(idle) } @@ -13,10 +13,12 @@ function action() { function init() { uav_initstig() uav_initswarm() - statef=turnedoff - BVMSTATE = "TURNEDOFF" - #statef = takeoff - #BVMSTATE = "TAKEOFF" + + TARGET_ALTITUDE = 5.0 # m. + # statef=turnedoff + # BVMSTATE = "TURNEDOFF" + statef = takeoff + BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -26,12 +28,12 @@ function step() { statef() log("Current state: ", BVMSTATE) - log("Obstacles: ") - reduce(proximity, - function(key, value, acc) { - log(key, " - ", value.angle, value.value) - return acc - }, math.vec2.new(0, 0)) + # log("Obstacles: ") + # reduce(proximity, + # function(key, value, acc) { + # log(key, " - ", value.angle, value.value) + # return acc + # }, math.vec2.new(0, 0)) } # Executed once when the robot (or the simulator) is reset. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f14e1a7..e6c3f48 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -89,7 +89,7 @@ void set_filtered_packet_loss(float value); /* * sets current position */ -void set_currentpos(double latitude, double longitude, double altitude); +void set_currentpos(double latitude, double longitude, float altitude, float yaw); /* * returns the current go to position */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 0e9e547..7599c58 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,5 +1,6 @@ #pragma once #include +#include #include #include #include @@ -51,7 +52,6 @@ class roscontroller public: roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); ~roscontroller(); - // void RosControllerInit(); void RosControllerRun(); static const string CAPTURE_SRV_DEFAULT_NAME; @@ -70,25 +70,27 @@ private: Num_robot_count count_robots; - struct gps + struct POSE { double longitude = 0.0; double latitude = 0.0; float altitude = 0.0; + // NED coordinates + float x = 0.0; + float y = 0.0; + float z = 0.0; + float yaw = 0.0; }; - typedef struct gps GPS; + typedef struct POSE ros_pose; - GPS target, home, cur_pos; - double cur_rel_altitude; + ros_pose target, home, cur_pos; uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; - // std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; int timer_step = 0; int robot_id = 0; std::string robot_name = ""; - // int oldcmdID=0; int rc_cmd; float fcu_timeout; @@ -96,7 +98,9 @@ private: int barrier; int message_number = 0; uint8_t no_of_robots = 0; - /*tmp to be corrected*/ + bool rcclient; + bool xbeeplugged = false; + bool multi_msg; uint8_t no_cnt = 0; uint8_t old_val = 0; std::string bzzfile_name; @@ -107,20 +111,18 @@ private: std::string bcfname, dbgfname; std::string out_payload; std::string in_payload; - std::string obstacles_topic; std::string stand_by; std::string xbeesrv_name; std::string capture_srv_name; std::string setpoint_name; std::string stream_client_name; - std::string relative_altitude_sub_name; std::string setpoint_nonraw; - bool rcclient; - bool xbeeplugged = false; - bool multi_msg; + + // ROS service, publishers and subscribers ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; ros::ServiceClient capture_srv; + ros::ServiceClient stream_client; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; @@ -137,23 +139,16 @@ private: ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; ros::Subscriber relative_altitude_sub; - - std::string local_pos_sub_name; ros::Subscriber local_pos_sub; - double local_pos_new[3]; - ros::ServiceClient stream_client; + std::map m_smTopic_infos; int setpoint_counter; - double my_x = 0, my_y = 0; std::ofstream log; - /*Commands for flight controller*/ - // mavros_msgs::CommandInt cmd_srv; + // Commands for flight controller mavros_msgs::CommandLong cmd_srv; - std::vector m_sMySubscriptions; - std::map m_smTopic_infos; mavros_msgs::CommandBool m_cmdBool; ros::ServiceClient arm_client; @@ -161,7 +156,7 @@ private: mavros_msgs::SetMode m_cmdSetMode; ros::ServiceClient mode_client; - /*Initialize publisher and subscriber, done in the constructor*/ + // Initialize publisher and subscriber, done in the constructor void Initialize_pub_sub(ros::NodeHandle& n_c); std::string current_mode; @@ -204,8 +199,8 @@ private: /*convert from spherical to cartesian coordinate system callback */ float constrainAngle(float x); - void gps_rb(GPS nei_pos, double out[]); - void gps_ned_cur(float& ned_x, float& ned_y, GPS t); + void gps_rb(POSE nei_pos, double out[]); + void gps_ned_cur(float& ned_x, float& ned_y, POSE t); void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, double gps_r_lat); @@ -219,10 +214,10 @@ private: void flight_status_update(const mavros_msgs::State::ConstPtr& msg); /*current position callback*/ - void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); + void global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg); /*current relative altitude callback*/ - void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); + void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg); /*payload callback callback*/ void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); @@ -234,7 +229,7 @@ private: void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); /*Obstacle distance table callback*/ - void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); + void obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg); /*Get publisher and subscriber from YML file*/ void GetSubscriptionParameters(ros::NodeHandle& node_handle); @@ -250,8 +245,6 @@ private: void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); - // void WaypointMissionSetup(float lat, float lng, float alt); - void fc_command_setup(); void SetLocalPosition(float x, float y, float z, float yaw); @@ -262,6 +255,7 @@ private: void get_number_of_robots(); + // functions related to Xbee modules information void GetRobotId(); bool GetDequeFull(bool& result); bool GetRssi(float& result); @@ -269,7 +263,7 @@ private: bool GetAPIRssi(const uint8_t short_id, float& result); bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); - void get_xbee_status(); + }; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index c61d9a3..425c7f1 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -18,7 +18,7 @@ static double rc_goto_pos[3]; static float rc_gimbal[4]; static float batt[3]; static float obst[5] = { 0, 0, 0, 0, 0 }; -static double cur_pos[3]; +static double cur_pos[4]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd = 0; @@ -637,7 +637,7 @@ int buzzuav_update_xbee_status(buzzvm_t vm) return vm->state; } -void set_currentpos(double latitude, double longitude, double altitude) +void set_currentpos(double latitude, double longitude, float altitude, float yaw) /* / update interface position array -----------------------------------*/ @@ -645,6 +645,7 @@ void set_currentpos(double latitude, double longitude, double altitude) cur_pos[0] = latitude; cur_pos[1] = longitude; cur_pos[2] = altitude; + cur_pos[3] = yaw; } // adds neighbours position void neighbour_pos_callback(int id, float range, float bearing, float elevation) @@ -677,7 +678,7 @@ int buzzuav_update_currentpos(buzzvm_t vm) / Update the BVM position table /------------------------------------------------------*/ { - buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); + buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); @@ -691,6 +692,10 @@ int buzzuav_update_currentpos(buzzvm_t vm) buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 1)); + buzzvm_pushf(vm, cur_pos[3]); + buzzvm_tput(vm); buzzvm_gstore(vm); return vm->state; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 11d5913..a59aac4 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -249,30 +249,37 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) /Obtains publisher, subscriber and services from yml config file /-----------------------------------------------------------------------------------*/ { - m_sMySubscriptions.clear(); - std::string gps_topic; - if (node_handle.getParam("topics/gps", gps_topic)) + std::string topic; + if (node_handle.getParam("topics/gps", topic)) ; else { - ROS_ERROR("Provide a gps topic in Launch file"); + ROS_ERROR("Provide a gps topic in YAML file"); system("rosnode kill rosbuzz_node"); } - m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/NavSatFix")); + if (node_handle.getParam("topics/localpos", topic)) + ; + else + { + ROS_ERROR("Provide a localpos name in YAML file"); + system("rosnode kill rosbuzz_node"); + } + m_smTopic_infos.insert(pair(topic, "geometry_msgs/PoseStamped")); - std::string battery_topic; - node_handle.getParam("topics/battery", battery_topic); - m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); + node_handle.getParam("topics/obstacles", topic); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/LaserScan")); - std::string status_topic; - node_handle.getParam("topics/status", status_topic); - m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); - node_handle.getParam("topics/estatus", status_topic); - m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); + node_handle.getParam("topics/battery", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/BatteryStatus")); - std::string altitude_topic; - node_handle.getParam("topics/altitude", altitude_topic); - m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); + node_handle.getParam("topics/status", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/ExtendedState")); + + node_handle.getParam("topics/altitude", topic); + m_smTopic_infos.insert(pair(topic, "std_msgs/Float64")); // Obtain required topic and service names from the parameter server if (node_handle.getParam("topics/fcclient", fcclient_name)) @@ -310,15 +317,6 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } - if (node_handle.getParam("topics/localpos", local_pos_sub_name)) - ; - else - { - ROS_ERROR("Provide a localpos name in YAML file"); - system("rosnode kill rosbuzz_node"); - } - - node_handle.getParam("topics/obstacles", obstacles_topic); } void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) @@ -332,8 +330,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); - obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); - // publishers payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); @@ -353,8 +349,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) capture_srv = n_c.serviceClient(capture_srv_name); stream_client = n_c.serviceClient(stream_client_name); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); - multi_msg = true; } @@ -379,11 +373,19 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c) } else if (it->second == "sensor_msgs/NavSatFix") { - current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::global_gps_callback, this); } else if (it->second == "std_msgs/Float64") { - relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::rel_alt_callback, this); + } + else if (it->second == "geometry_msgs/PoseStamped") + { + local_pos_sub = n_c.subscribe(it->first, 5, &roscontroller::local_pos_callback, this); + } + else if (it->second == "sensor_msgs/LaserScan") + { + obstacle_sub = n_c.subscribe(it->first, 5, &roscontroller::obstacle_dist_callback, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -798,7 +800,7 @@ float roscontroller::constrainAngle(float x) return x; } -void roscontroller::gps_rb(GPS nei_pos, double out[]) +void roscontroller::gps_rb(POSE nei_pos, double out[]) /* / Compute Range and Bearing of a neighbor in a local reference frame / from GPS coordinates @@ -814,7 +816,7 @@ void roscontroller::gps_rb(GPS nei_pos, double out[]) out[2] = 0.0; } -void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, GPS t) +void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) /* / Get GPS from NED and a reference GPS point (struct input) ----------------------------------------------------------- */ @@ -868,39 +870,47 @@ void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedSta buzzuav_closures::flight_status_update(msg->landed_state); } -void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) +void roscontroller::global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) /* / Update current GPS position into BVM from subscriber /-------------------------------------------------------------*/ { - // DEBUG - // ROS_INFO("Altitude out: %f", cur_rel_altitude); + // reset timeout counter fcu_timeout = TIMEOUT; - set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); - buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); + set_cur_pos(msg->latitude, msg->longitude, cur_pos.z); + buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_pos.z, cur_pos.yaw); } -void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose) +void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) /* / Update current position for flight controller NED offset /-------------------------------------------------------------*/ { - local_pos_new[0] = pose->pose.position.x; - local_pos_new[1] = pose->pose.position.y; - local_pos_new[2] = pose->pose.position.z; + cur_pos.x = msg->pose.position.x; + cur_pos.y = msg->pose.position.y; + // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead + tf::Quaternion q( + msg->pose.orientation.x, + msg->pose.orientation.y, + msg->pose.orientation.z, + msg->pose.orientation.w); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + cur_pos.yaw = yaw; } -void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg) +void roscontroller::rel_alt_callback(const std_msgs::Float64::ConstPtr& msg) /* / Update altitude into BVM from subscriber /-------------------------------------------------------------*/ { // DEBUG // ROS_INFO("Altitude in: %f", msg->data); - cur_rel_altitude = (double)msg->data; + cur_pos.z = (double)msg->data; } -void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg) +void roscontroller::obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg) /* /Set obstacle Obstacle distance table into BVM from subscriber /-------------------------------------------------------------*/ @@ -925,9 +935,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.header.frame_id = 1; // DEBUG - // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); - moveMsg.pose.position.x = local_pos_new[0] + y; - moveMsg.pose.position.y = local_pos_new[1] + x; + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y); + moveMsg.pose.position.x = cur_pos.x + y; + moveMsg.pose.position.y = cur_pos.y + x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -1038,7 +1048,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1], neighbours_pos_payload[2]); - GPS nei_pos; + POSE nei_pos; nei_pos.latitude = neighbours_pos_payload[0]; nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; From 4e4bacb25d40e490639036d921c75f036afc734d Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 21 Dec 2017 13:28:06 -0500 Subject: [PATCH 08/77] changed absolute_position to fit argos implementation --- buzz_scripts/include/rrtstar.bzz | 4 +-- buzz_scripts/include/uavstates.bzz | 32 +++++++++++----------- src/buzzuav_closures.cpp | 44 +++++++++++++++++++++++------- 3 files changed, 52 insertions(+), 28 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 4e3c138..7cc5235 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -136,8 +136,8 @@ function getproxobs (m_curpos) { reduce(proximity, function(key, value, acc) { - obs = getcell(math.vec2.newp(value.value, value.angle + absolute_position.yaw)) - obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw)) + obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw)) + obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw)) per = math.vec2.sub(obs,cur_cell) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 8e347bf..316d281 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -34,13 +34,13 @@ function idle() { function takeoff() { BVMSTATE = "TAKEOFF" statef=takeoff - homegps = {.lat=absolute_position.latitude, .long=absolute_position.longitude} + homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} - if( flight.status == 2 and absolute_position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", absolute_position.altitude) + log("Altitude: ", pose.position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } @@ -105,8 +105,8 @@ function gotoWPRRT(transf) { # create the map if(map==nil) { dyn_init_map(rc_goal) - homegps.lat = absolute_position.latitude - homegps.long = absolute_position.longitude + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude } if(Path==nil) { @@ -122,7 +122,7 @@ function gotoWPRRT(transf) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { populateGrid(m_pos) if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude - absolute_position.altitude) + uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude) Path = nil rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) # re-start the planner @@ -130,7 +130,7 @@ function gotoWPRRT(transf) { cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - absolute_position.altitude) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude) } } else cur_goal_l = cur_goal_l + 1 @@ -148,11 +148,11 @@ function gotoWP(transf) { log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } function follow() { @@ -260,8 +260,8 @@ function LimitAngle(angle){ } function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - absolute_position.longitude - d_lat = lat - absolute_position.latitude + d_lon = lon - pose.position.longitude + d_lat = lat - pose.position.latitude if(home_ref == 1) { d_lon = lon - homegps.long d_lat = lat - homegps.lat @@ -277,17 +277,17 @@ function gps_from_vec(vec) { Lgoal = {.latitude=0, .longitude=0} Vrange = math.vec2.length(vec) Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude) - latR = absolute_position.latitude*math.pi/180.0; - lonR = absolute_position.longitude*math.pi/180.0; +# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) + latR = pose.position.latitude*math.pi/180.0; + lonR = pose.position.longitude*math.pi/180.0; target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); Lgoal.latitude = target_lat*180.0/math.pi; Lgoal.longitude = target_lon*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + absolute_position.latitude; + #goal.latitude = d_lat + pose.position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + absolute_position.longitude; + #goal.longitude = d_lon + pose.position.longitude; return Lgoal } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 425c7f1..3c58403 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -678,25 +678,49 @@ int buzzuav_update_currentpos(buzzvm_t vm) / Update the BVM position table /------------------------------------------------------*/ { - buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1)); + buzzvm_pushs(vm, buzzvm_string_register(vm, "pose", 1)); buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzobj_t tPoseTable = buzzvm_stack_at(vm, 1); + buzzvm_gstore(vm); + + // Create table for i-th read + buzzvm_pusht(vm); + buzzobj_t tPosition = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 0)); buzzvm_pushf(vm, cur_pos[0]); buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 0)); buzzvm_pushf(vm, cur_pos[1]); buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0)); buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 1)); + // Store read table in the proximity table + buzzvm_push(vm, tPoseTable); + buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0)); + buzzvm_push(vm, tPosition); + buzzvm_tput(vm); + + // Create table for i-th read + buzzvm_pusht(vm); + buzzobj_t tOrientation = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tOrientation); + buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 0)); buzzvm_pushf(vm, cur_pos[3]); buzzvm_tput(vm); - buzzvm_gstore(vm); + // Store read table in the proximity table + buzzvm_push(vm, tPoseTable); + buzzvm_pushs(vm, buzzvm_string_register(vm, "orientation", 0)); + buzzvm_push(vm, tOrientation); + buzzvm_tput(vm); + return vm->state; } From a626c556d99642191d9e311a9574ed27fb6bc370 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 21 Dec 2017 21:41:57 -0500 Subject: [PATCH 09/77] add collision boxes for gazebo buildings and debugged collision detection for rrt --- buzz_scripts/include/mapmatrix.bzz | 6 +-- buzz_scripts/include/rrtstar.bzz | 76 +++++++++++++++++++----------- buzz_scripts/include/uavstates.bzz | 2 +- buzz_scripts/testRRT.bzz | 4 +- src/buzzuav_closures.cpp | 3 +- src/roscontroller.cpp | 4 +- 6 files changed, 58 insertions(+), 37 deletions(-) diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index 79ad775..c3d3393 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -56,7 +56,7 @@ function add_obstacle(pos, off, inc_trust) { var yi = math.round(pos.y) if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { - #log("Add obstacle in cell: ", xi, " ", yi) + # log("Add obstacle in cell: ", xi, " ", yi) var old=map[xi][yi] if(old-inc_trust > 0.0) map[xi][yi] = old-inc_trust @@ -69,8 +69,8 @@ function remove_obstacle(pos, off, dec_trust) { var xi = math.round(pos.x) var yi = math.round(pos.y) - if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ - #log("Remove obstacle in cell: ", xi, " ", yi) + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { + # log("Remove obstacle in cell: ", xi, " ", yi) var old=map[xi][yi] if(old + dec_trust < 1.0) #x,y map[xi][yi] = old+dec_trust diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 7cc5235..3fa686c 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -7,7 +7,7 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 -PROX_SENSOR_THRESHOLD_M = 10 +PROX_SENSOR_THRESHOLD_M = 8.0 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} @@ -46,7 +46,7 @@ function pathPlanner(m_goal, m_pos, kh4) { arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance Q = {} - Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0} goalReached = 0 timeout = 0 @@ -56,6 +56,7 @@ function pathPlanner(m_goal, m_pos, kh4) { } # get the grid cell position (x,y) equivalent to the input position +# input should be relative to home position (planing start point) function getcell(m_curpos) { var cell = {} # relative to center (start position) @@ -79,6 +80,7 @@ function getcell(m_curpos) { function populateGrid(m_pos) { getproxobs(m_pos) getneiobs (m_pos) + export_map(map) } # TODO: populate the map with neighors as blobs instead ? @@ -136,26 +138,43 @@ function getproxobs (m_curpos) { reduce(proximity, function(key, value, acc) { - obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw)) - obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw)) - per = math.vec2.sub(obs,cur_cell) - obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) - obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) - obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) - obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value < PROX_SENSOR_THRESHOLD_M) { - if(map[math.round(obs.x)][math.round(obs.y)]!=0) { - add_obstacle(obs, 0, 0.25) - add_obstacle(obs2, 0, 0.25) - add_obstacle(obsr, 0, 0.25) - add_obstacle(obsr2, 0, 0.25) - add_obstacle(obsl, 0, 0.25) - add_obstacle(obsl2, 0, 0.25) - updated_obstacle = 1 + if (value.angle != -1) { # down sensor of M100 + if(value.value < PROX_SENSOR_THRESHOLD_M) { + obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw))) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs) + # obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs))) + # obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) + # obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) + if(map[math.round(obs.x)][math.round(obs.y)]!=0) { + add_obstacle(obs, 0, 0.25) + add_obstacle(obsr, 0, 0.25) + add_obstacle(obsl, 0, 0.25) + add_obstacle(obsr2, 0, 0.25) + add_obstacle(obsl2, 0, 0.25) + # add_obstacle(obs2, 0, 0.25) + # add_obstacle(obsr2, 0, 0.25) + # add_obstacle(obsl2, 0, 0.25) + updated_obstacle = 1 + } + } else { + var line_length = PROX_SENSOR_THRESHOLD_M; + while(line_length > 0) { + obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw))) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs) + remove_obstacle(obs, 0, 0.05) + remove_obstacle(obsr, 0, 0.05) + remove_obstacle(obsl, 0, 0.05) + line_length = line_length - 1; + } } - } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) { - remove_obstacle(obs, 0, 0.05) - updated_obstacle = 1 } return acc }, math.vec2.new(0, 0)) @@ -225,7 +244,7 @@ function rrtstar() { nbCount = nbCount + 1; if(intersects != 1) { #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") - var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] + var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5] if(distance < minCounted) { minCounted = distance; @@ -422,17 +441,18 @@ function getPath(Q,nb,gps){ var pathL={} var npt=0 # get the path from the point list - while(nb > 1) { + while(nb > 0) { npt=npt+1 pathL[npt] = {} pathL[npt][1]=Q[nb][1] pathL[npt][2]=Q[nb][2] - if(nb != Q[Q[nb][3]][3]) - nb = Q[nb][3]; - else { # TODO: why is this happening? - log("ERROR - Recursive path !!!") - return nil + if( nb > 1) { + if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening? + log("ERROR - Recursive path !!!") + return nil + } } + nb = Q[nb][3]; } # Re-Order the list. diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 316d281..8a27c38 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -9,7 +9,7 @@ include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2 # m/steps +GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index f586fe5..3ad4cc6 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -5,7 +5,7 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5088103899,-73.1540826153,5.0) + uav_storegoal(45.5088103899,-73.1540826153,2.5) set_goto(idle) } @@ -14,7 +14,7 @@ function init() { uav_initstig() uav_initswarm() - TARGET_ALTITUDE = 5.0 # m. + TARGET_ALTITUDE = 2.5 # m # statef=turnedoff # BVMSTATE = "TURNEDOFF" statef = takeoff diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 3c58403..20e0456 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -179,6 +179,7 @@ int buzz_exportmap(buzzvm_t vm) / Buzz closure to export a 2D map /----------------------------------------*/ { + grid.clear(); buzzvm_lnum_assert(vm, 1); // Get the parameter buzzvm_lload(vm, 1); @@ -193,7 +194,7 @@ int buzz_exportmap(buzzvm_t vm) buzzvm_dup(vm); buzzvm_pushi(vm, j); buzzvm_tget(vm); - row.insert(std::pair(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + row.insert(std::pair(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0))); buzzvm_pop(vm); } grid.insert(std::pair>(i,row)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index a59aac4..cfd47c0 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -473,8 +473,8 @@ void roscontroller::grid_publisher() grid_msg.info.resolution = 0.01;//gridMap.getResolution(); grid_msg.info.width = g_w; grid_msg.info.height = g_h; - grid_msg.info.origin.position.x = 0.0; - grid_msg.info.origin.position.y = 0.0; + grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution; + grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution; grid_msg.info.origin.position.z = 0.0; grid_msg.info.origin.orientation.x = 0.0; grid_msg.info.origin.orientation.y = 0.0; From 6cd162bff4be2c7aca47e2e692437c89082ff0eb Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 22 Dec 2017 16:48:39 -0500 Subject: [PATCH 10/77] restructured bzz for path planning --- buzz_scripts/include/{ => act}/barrier.bzz | 28 +- buzz_scripts/include/act/states.bzz | 234 ++++++++++++++ buzz_scripts/include/{ => plan}/mapmatrix.bzz | 0 buzz_scripts/include/{ => plan}/rrtstar.bzz | 29 +- buzz_scripts/include/shapes.bzz | 111 ------- .../taskallocate}/graphformGPS.bzz | 0 .../graphs/images}/Graph_droneL.graph | 0 .../graphs/images}/Graph_droneO.graph | 0 .../graphs/images}/Graph_droneP.graph | 0 .../graphs/images}/Graph_droneY.graph | 0 .../graphs/images}/frame_01186.png | Bin .../graphs/images}/frame_01207.png | Bin .../graphs/images}/frame_01394.png | Bin .../graphs/images}/frame_01424.png | Bin .../{ => taskallocate}/graphs/shapes_L.bzz | 0 .../{ => taskallocate}/graphs/shapes_O.bzz | 0 .../{ => taskallocate}/graphs/shapes_P.bzz | 0 .../{ => taskallocate}/graphs/shapes_Y.bzz | 0 .../graphs/shapes_square.bzz | 0 .../{graphs => taskallocate}/waypointlist.csv | 0 buzz_scripts/include/uavstates.bzz | 293 ------------------ buzz_scripts/include/utilities.bzz | 152 --------- buzz_scripts/include/utils/conversions.bzz | 63 ++++ buzz_scripts/include/{ => utils}/string.bzz | 0 buzz_scripts/include/{ => utils}/vec2.bzz | 0 buzz_scripts/include/vstigenv.bzz | 8 +- buzz_scripts/main.bzz | 81 +++++ buzz_scripts/testRRT.bzz | 45 --- src/buzz_utility.cpp | 4 +- 29 files changed, 402 insertions(+), 646 deletions(-) rename buzz_scripts/include/{ => act}/barrier.bzz (68%) create mode 100644 buzz_scripts/include/act/states.bzz rename buzz_scripts/include/{ => plan}/mapmatrix.bzz (100%) rename buzz_scripts/include/{ => plan}/rrtstar.bzz (96%) delete mode 100644 buzz_scripts/include/shapes.bzz rename buzz_scripts/{ => include/taskallocate}/graphformGPS.bzz (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneL.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneO.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneP.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneY.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01186.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01207.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01394.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01424.png (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_L.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_O.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_P.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_Y.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_square.bzz (100%) rename buzz_scripts/include/{graphs => taskallocate}/waypointlist.csv (100%) delete mode 100644 buzz_scripts/include/uavstates.bzz delete mode 100644 buzz_scripts/include/utilities.bzz create mode 100644 buzz_scripts/include/utils/conversions.bzz rename buzz_scripts/include/{ => utils}/string.bzz (100%) rename buzz_scripts/include/{ => utils}/vec2.bzz (100%) create mode 100644 buzz_scripts/main.bzz delete mode 100644 buzz_scripts/testRRT.bzz diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/act/barrier.bzz similarity index 68% rename from buzz_scripts/include/barrier.bzz rename to buzz_scripts/include/act/barrier.bzz index 31a452e..8b56f3c 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -28,9 +28,9 @@ function barrier_create() { barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef, bdt) { +function barrier_set(threshold, transf, resumef) { statef = function() { - barrier_wait(threshold, transf, resumef, bdt); + barrier_wait(threshold, transf, resumef); } BVMSTATE = "BARRIERWAIT" barrier_create() @@ -48,7 +48,7 @@ function barrier_ready() { # # Executes the barrier # -function barrier_wait(threshold, transf, resumef, bdt) { +function barrier_wait(threshold, transf, resumef) { barrier.put(id, 1) barrier.get(id) @@ -56,31 +56,13 @@ function barrier_wait(threshold, transf, resumef, bdt) { if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { barrier.put("d", 1) timeW = 0 - transf() + BVMSTATE = transf } else if(timeW >= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") barrier=nil timeW = 0 - resumef() + BVMSTATE = resumef } - if(bdt!=-1) - neighbors.broadcast("cmd", bdt) - timeW = timeW+1 } - -# get the lowest id of the fleet, but requires too much bandwidth... -function getlowest(){ - Lid = 15; - u=15 - while(u>=0){ - tab = barrier.get(u) - if(tab!=nil){ - if(tab LOWEST ID:",Lid) -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz new file mode 100644 index 0000000..d8e3790 --- /dev/null +++ b/buzz_scripts/include/act/states.bzz @@ -0,0 +1,234 @@ +######################################## +# +# FLIGHT-RELATED FUNCTIONS +# +######################################## +include "utils/vec2.bzz" +include "plan/rrtstar.bzz" +include "act/barrier.bzz" +include "utils/conversions.bzz" + +TARGET_ALTITUDE = 15.0 # m. +BVMSTATE = "TURNEDOFF" +PICTURE_WAIT = 20 # steps +GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.5 # m. +GOTOANG_TOL = 0.1 # rad. +path_it = 0 +rc_State = 0 +pic_time = 0 +g_it = 0 + +function turnedoff() { + BVMSTATE = "TURNEDOFF" +} + +function idle() { + BVMSTATE = "IDLE" +} + +function launch() { + BVMSTATE = "LAUNCH" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} + if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_ready() + } else { + log("Altitude: ", pose.position.altitude) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } + } else { + barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_ready() + } +} + +function stop() { + BVMSTATE = "STOP" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + neighbors.broadcast("cmd", 21) + uav_land() + if(flight.status != 2 and flight.status != 3) { + barrier_set(ROBOTS,"TURNEDOFF","STOP") + barrier_ready() + } + } else { + barrier_set(ROBOTS,"TURNEDOFF","STOP") + barrier_ready() + } +} + +function take_picture() { + BVMSTATE="PICTURE" + uav_setgimbal(0.0, 0.0, -90.0, 20.0) + if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize + uav_takepicture() + } else if(pic_time>=PICTURE_WAIT) { # wait for the picture + BVMSTATE="IDLE" + pic_time=0 + } + pic_time=pic_time+1 +} + +# +# Work in progress.... +function navigate() { + BVMSTATE="NAVIGATE" + if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz + uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) + } + + print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) + if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { + log("Sorry this is too far.") + return + } + + # create the map + if(map==nil) { + dyn_init_map(cur_goal) + if(V_TYPE == 0) { + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude + } + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else if(path_it <= size(Path)) { + var cur_path_pt = convert_pt(getvec(Path, path_it)) + print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) + if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, path_it, m_pos, 0)) { + # stop + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + Path = nil + if(V_TYPE == 0) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else { + cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + } + } else + path_it = path_it + 1 + } else { + Path = nil + BVMSTATE="IDLE" + } +} + +# function goto_direct(transf) { +# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) +# print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + +# if(math.vec2.length(m_navigation)>GOTO_MAXDIST) +# log("Sorry this is too far.") +# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity +# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) +# } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination +# transf() +# else +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) +# } + +function follow() { + if(size(targets)>0) { + BVMSTATE = "FOLLOW" + attractor=math.vec2.newp(0,0) + foreach(targets, function(id, tab) { + force=(0.05)*(tab.range)^4 + attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) + }) + uav_moveto(attractor.x, attractor.y, 0.0) + } else { + log("No target in local table!") + BVMSTATE = "IDLE" + } +} + +function rc_cmd_listen() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + BVMSTATE = "LAUNCH" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + flight.rc_cmd=0 + BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + BVMSTATE = "PATHPLAN" + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + rc_State = 0 + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + rc_State = 1 + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + rc_State = 2 + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + rc_State = 3 + neighbors.broadcast("cmd", 903) + } +} + +function nei_cmd_listen() { + neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") { + BVMSTATE = "LAUNCH" + } else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") { + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + uav_arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + uav_disarm() + } else if(value==900){ + rc_State = 0 + } else if(value==901){ + rc_State = 1 + } else if(value==902){ + rc_State = 2 + } else if(value==903){ + rc_State = 3 + } else if(value==16 and BVMSTATE=="IDLE"){ + # neighbors.listen("gt",function(vid, value, rid) { + # print("Got (", vid, ",", value, ") from robot #", rid) + # # if(gt.id == id) statef=goto + # }) + } + }) +} diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz similarity index 100% rename from buzz_scripts/include/mapmatrix.bzz rename to buzz_scripts/include/plan/mapmatrix.bzz diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz similarity index 96% rename from buzz_scripts/include/rrtstar.bzz rename to buzz_scripts/include/plan/rrtstar.bzz index 3fa686c..6d40626 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -3,7 +3,7 @@ # # map table-based matrix ##### -include "mapmatrix.bzz" +include "plan/mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 @@ -78,7 +78,7 @@ function getcell(m_curpos) { } function populateGrid(m_pos) { - getproxobs(m_pos) + # getproxobs(m_pos) getneiobs (m_pos) export_map(map) } @@ -211,8 +211,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) { ## function rrtstar() { # update state machine variables - statef = rrtstar - BVMSTATE = "RRTSTAR" + BVMSTATE = "PATHPLAN" step_timeout = 0 @@ -336,16 +335,14 @@ function rrtstar() { if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPath(Q,numberOfPoints, 1) + Path = convert_path(getPath(Q,numberOfPoints)) # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef + BVMSTATE = "NAVIGATE" } else if(timeout >= RRT_TIMEOUT) { log("FAILED TO FIND A PATH!!!") Path = nil # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef + BVMSTATE = "NAVIGATE" } } @@ -437,7 +434,7 @@ function doesItIntersect(point,vector) { } # create a table with only the path's points in order -function getPath(Q,nb,gps){ +function getPath(Q,nb){ var pathL={} var npt=0 # get the path from the point list @@ -460,15 +457,9 @@ function getPath(Q,nb,gps){ var totpt = npt + 1 while(npt > 0){ pathR[totpt-npt] = {} - if(gps) { - tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell)) - pathR[totpt-npt][1]=tmpgoal.latitude - pathR[totpt-npt][2]=tmpgoal.longitude - } else { - tmpgoal = getvec(path,npt) - pathR[totpt-npt][1]=tmpgoal.x - pathR[totpt-npt][2]=tmpgoal.y - } + var tmpgoal = getvec(pathL,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y npt = npt - 1 } #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) diff --git a/buzz_scripts/include/shapes.bzz b/buzz_scripts/include/shapes.bzz deleted file mode 100644 index 53cdd3b..0000000 --- a/buzz_scripts/include/shapes.bzz +++ /dev/null @@ -1,111 +0,0 @@ -#Table of the nodes in the graph -m_vecNodes={} -m_vecNodes_fixed={} -m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = 0, # Lable of the point - .Pred = -1, # Lable of its predecessor - .distance = -1, # distance to the predecessor - .bearing = -1, # bearing form the predecessor to this dot - .height = 3000, # height of this dot - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[1] = { - .Lable = 1, - .Pred = 0, - .distance = 1000, - .bearing = 0.0, - .height = 5000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[2] = { - .Lable = 2, - .Pred = 0, - .distance = 1000, - .bearing = 1.57, - .height = 7000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[3] = { - .Lable = 3, - .Pred = 0, - .distance = 1000, - .bearing = 3.14, - .height = 9000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[4] = { - .Lable = 4, - .Pred = 0, - .distance = 1000, - .bearing = 4.71, - .height = 11000, - .State="UNASSIGNED", - .StateAge=0 -} - -# -# Graph parsing -# -function parse_graph(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = string.toint(rrec[0]), # Lable of the point - .Pred = string.toint(rrec[1]), # Lable of its predecessor - .distance = string.tofloat(rrec[2]), # distance to the predecessor - .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot - .height = string.tofloat(rrec[4]), # height of this dot - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} - -function parse_graph_fixed(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2 - .Pred1 = string.toint(rrec[1]), # Pred 1 lable - .Pred2 = string.toint(rrec[3]), # Pred 2 lable - .d1 = string.tofloat(rrec[2]), # Pred 1 distance - .d2 = string.tofloat(rrec[4]), # Pred 2 distance - .Lable=string.toint(rrec[0]), - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} \ No newline at end of file diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz similarity index 100% rename from buzz_scripts/graphformGPS.bzz rename to buzz_scripts/include/taskallocate/graphformGPS.bzz diff --git a/buzz_scripts/include/graphs/Graph_droneL.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneL.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph diff --git a/buzz_scripts/include/graphs/Graph_droneO.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneO.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph diff --git a/buzz_scripts/include/graphs/Graph_droneP.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneP.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph diff --git a/buzz_scripts/include/graphs/Graph_droneY.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneY.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph diff --git a/buzz_scripts/include/graphs/frame_01186.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01186.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01186.png diff --git a/buzz_scripts/include/graphs/frame_01207.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01207.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01207.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01207.png diff --git a/buzz_scripts/include/graphs/frame_01394.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01394.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01394.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01394.png diff --git a/buzz_scripts/include/graphs/frame_01424.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01424.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01424.png diff --git a/buzz_scripts/include/graphs/shapes_L.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_L.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_L.bzz diff --git a/buzz_scripts/include/graphs/shapes_O.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_O.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_O.bzz diff --git a/buzz_scripts/include/graphs/shapes_P.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_P.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_P.bzz diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_Y.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz diff --git a/buzz_scripts/include/graphs/shapes_square.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_square.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_square.bzz diff --git a/buzz_scripts/include/graphs/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv similarity index 100% rename from buzz_scripts/include/graphs/waypointlist.csv rename to buzz_scripts/include/taskallocate/waypointlist.csv diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz deleted file mode 100644 index 8a27c38..0000000 --- a/buzz_scripts/include/uavstates.bzz +++ /dev/null @@ -1,293 +0,0 @@ -######################################## -# -# FLIGHT-RELATED FUNCTIONS -# -######################################## -include "vec2.bzz" -include "rrtstar.bzz" - -TARGET_ALTITUDE = 15.0 # m. -BVMSTATE = "TURNEDOFF" -PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps -GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.5 # m. -GOTOANG_TOL = 0.1 # rad. -cur_goal_l = 0 -rc_State = 0 - -function uav_initswarm() { - s = swarm.create(1) - s.join() -} - -function turnedoff() { - statef=turnedoff - BVMSTATE = "TURNEDOFF" -} - -function idle() { - statef=idle - BVMSTATE = "IDLE" -} - -function takeoff() { - BVMSTATE = "TAKEOFF" - statef=takeoff - homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} - - if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, action, land, -1) - barrier_ready() - } else { - log("Altitude: ", pose.position.altitude) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} - -function land() { - BVMSTATE = "LAND" - statef=land - - neighbors.broadcast("cmd", 21) - uav_land() - - if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,turnedoff,land, 21) - barrier_ready() - } -} - -function set_goto(transf) { - BVMSTATE = "GOTOGPS" - statef=function() { - gotoWPRRT(transf) - } - - if(rc_goto.id==id){ - if(s!=nil){ - if(s.in()) - s.leave() - } - } else { - neighbors.broadcast("cmd", 16) - neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - } -} - -ptime=0 -function picture() { - statef=picture - BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) - if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() - } else if(ptime>=PICTURE_WAIT) { # wait for the picture - statef=action - ptime=0 - } - 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, 0) - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-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.") - return - } - # create the map - if(map==nil) { - dyn_init_map(rc_goal) - homegps.lat = pose.position.latitude - homegps.long = pose.position.longitude - } - - if(Path==nil) { - # add proximity sensor and neighbor obstacles to the map - populateGrid(m_pos) - # start the planner - pathPlanner(rc_goal, m_pos) - cur_goal_l = 1 - } else if(cur_goal_l <= size(Path)) { - var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude - var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) - print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) - if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { - populateGrid(m_pos) - if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude) - Path = nil - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - # re-start the planner - pathPlanner(rc_goal, m_pos) - cur_goal_l = 1 - } else { - cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude) - } - } else - cur_goal_l = cur_goal_l + 1 - } else { - Path = nil - transf() - } -} - -function gotoWP(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) - print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - - if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) - } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination - transf() - else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -} - -function follow() { - if(size(targets)>0) { - BVMSTATE = "FOLLOW" - statef=follow - attractor=math.vec2.newp(0,0) - foreach(targets, function(id, tab) { - force=(0.05)*(tab.range)^4 - attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) - }) - uav_moveto(attractor.x, attractor.y, 0.0) - } else { - log("No target in local table!") - #statef=idle - } -} - -function uav_rccmd() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - BVMSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - BVMSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - BVMSTATE = "GOTOGPS" - statef = goto - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } else if (flight.rc_cmd==666){ - flight.rc_cmd=0 - stattab_send() - } else if (flight.rc_cmd==900){ - flight.rc_cmd=0 - rc_State = 0 - neighbors.broadcast("cmd", 900) - } else if (flight.rc_cmd==901){ - flight.rc_cmd=0 - rc_State = 1 - neighbors.broadcast("cmd", 901) - } else if (flight.rc_cmd==902){ - flight.rc_cmd=0 - rc_State = 2 - neighbors.broadcast("cmd", 902) - } else if (flight.rc_cmd==903){ - flight.rc_cmd=0 - rc_State = 3 - neighbors.broadcast("cmd", 903) - } -} - -function uav_neicmd() { - neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") { - statef=takeoff - BVMSTATE = "TAKEOFF" - } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") { - statef=land - BVMSTATE = "LAND" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() - } else if(value==900){ - rc_State = 0 - } else if(value==901){ - rc_State = 1 - } else if(value==902){ - rc_State = 2 - } else if(value==903){ - rc_State = 3 - } else if(value==16 and BVMSTATE=="IDLE"){ - # neighbors.listen("gt",function(vid, value, rid) { - # print("Got (", vid, ",", value, ") from robot #", rid) - # # if(gt.id == id) statef=goto - # }) - } - }) -} - -function LimitAngle(angle){ - if(angle>2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - pose.position.longitude - d_lat = lat - pose.position.latitude - if(home_ref == 1) { - d_lon = lon - homegps.long - d_lat = lat - homegps.lat - } - ned_x = d_lat/180*math.pi * 6371000.0; - ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); - #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); - #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) -} - -function gps_from_vec(vec) { - Lgoal = {.latitude=0, .longitude=0} - Vrange = math.vec2.length(vec) - Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) - latR = pose.position.latitude*math.pi/180.0; - lonR = pose.position.longitude*math.pi/180.0; - target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); - target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); - Lgoal.latitude = target_lat*180.0/math.pi; - Lgoal.longitude = target_lon*180.0/math.pi; - #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + pose.position.latitude; - #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + pose.position.longitude; - - return Lgoal -} diff --git a/buzz_scripts/include/utilities.bzz b/buzz_scripts/include/utilities.bzz deleted file mode 100644 index 6c4dab6..0000000 --- a/buzz_scripts/include/utilities.bzz +++ /dev/null @@ -1,152 +0,0 @@ -# Utilities - -# Rads to degrees -function rtod(r) { - return (r*(180.0/math.pi)) -} - -# Copy a table -function table_deep_copy(new_t, old_t, depth) { - depth = depth - 1 - if (old_t != nil) { - foreach(old_t, function(key, value) { - new_t[key] = value - if(depth != 0) { - new_t[key] = {} - table_deep_copy(new_t[key], value, depth) - } - }) - } -} - -function table_copy(old_t, depth) { - var t = {}; - table_deep_copy(t, old_t, depth); - return t; -} - -# Print the contents of a table -function table_print(t, depth) { - depth = depth - 1 - if (t != nil) { - foreach(t, function(key, value) { - log(key, " -> ", value) - if(depth != 0) { - table_print(t[key], depth) - } - }) - } -} - -# Write a table as if it was a vector -function write_vector(k, index, val) { - var key = string.tostring(index) - k[key] = val -} - -# Read a table as if it was a vector -function read_vector(k, index) { - var key = string.tostring(index) - if (k[key] == nil) { - return -1 - } else { - return k[key] - } -} - -# Write a table as if it was a matrix -function write_matrix(k, row, col, val) { - var key = string.concat(string.tostring(row),"-",string.tostring(col)) - k[key] = val -} - -# Read a table as if it was a matrix -function read_matrix(k, row, col) { - var key = string.concat(string.tostring(row),"-",string.tostring(col)) - if (k[key] == nil) { - return -1 - } else { - return k[key] - } -} - -# Int to String -function itos(i) { - - log("Use 'string.tostring(OJB)' instead") - - if (i==0) { return "0" } - if (i==1) { return "1" } - if (i==2) { return "2" } - if (i==3) { return "3" } - if (i==4) { return "4" } - if (i==5) { return "5" } - if (i==6) { return "6" } - if (i==7) { return "7" } - if (i==8) { return "8" } - if (i==9) { return "9" } - - log("Function 'itos' out of bounds, returning the answer (42)") - return "42" -} - -# String to Int -function stoi(s) { - if (s=='0') { return 0 } - if (s=='1') { return 1 } - if (s=='2') { return 2 } - if (s=='3') { return 3 } - if (s=='4') { return 4 } - if (s=='5') { return 5 } - if (s=='6') { return 6 } - if (s=='7') { return 7 } - if (s=='8') { return 8 } - if (s=='9') { return 9 } - - log("Function 'stoi' out of bounds, returning the answer (42)") - return 42 - -} - -# Force angles in the (-pi,pi) interval -function radians_interval(a) { - var temp = a - while ((temp>2.0*math.pi) or (temp<0.0)) { - if (temp > 2.0*math.pi) { - temp = temp - 2.0*math.pi - } else if (temp < 0.0){ - temp = temp + 2.0*math.pi - } - } - if (temp > math.pi) { - temp = temp - 2.0*math.pi - } - return temp -} - -############################################ - -#base = {} - -#base.create = function() { -# return { -# .method = function(a,b) { -# return a + b -# } -# } -# } - -#x = base.create() -#x.method(3,4) # returns 7 - -#derived = {} - -#derived.create = function() { -# b = base.create() -# b.method = function(a,b) { -# return a * b -# } -#} - -#x = derived.create() -#x.method(3,4) # returns 12 diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz new file mode 100644 index 0000000..144cabf --- /dev/null +++ b/buzz_scripts/include/utils/conversions.bzz @@ -0,0 +1,63 @@ +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +# TODO: add other conversions.... +function convert_path(P) { + var pathR={} + if(V_TYPE == 0) { + var n = 1 + while(n <= size(P)){ + pathR[n] = {} + var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell)) + pathR[n][1]=tmpgoal.latitude + pathR[n][2]=tmpgoal.longitude + n = n + 1 + } + } + return pathR +} + +# TODO: add other conversions.... +function convert_pt(in) { + if(V_TYPE == 0) + return vec_from_gps(in.x, in.y, 0) +} + +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - pose.position.longitude + d_lat = lat - pose.position.latitude + if(home_ref == 1) { + d_lon = lon - homegps.long + d_lat = lat - homegps.lat + } + ned_x = d_lat/180*math.pi * 6371000.0; + ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.new(ned_x,ned_y) +} + +function gps_from_vec(vec) { + Lgoal = {.latitude=0, .longitude=0} + Vrange = math.vec2.length(vec) + Vbearing = LimitAngle(math.atan(vec.y, vec.x)) +# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) + latR = pose.position.latitude*math.pi/180.0; + lonR = pose.position.longitude*math.pi/180.0; + target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); + target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); + Lgoal.latitude = target_lat*180.0/math.pi; + Lgoal.longitude = target_lon*180.0/math.pi; + #d_lat = (vec.x / 6371000.0)*180.0/math.pi; + #goal.latitude = d_lat + pose.position.latitude; + #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; + #goal.longitude = d_lon + pose.position.longitude; + + return Lgoal +} \ No newline at end of file diff --git a/buzz_scripts/include/string.bzz b/buzz_scripts/include/utils/string.bzz similarity index 100% rename from buzz_scripts/include/string.bzz rename to buzz_scripts/include/utils/string.bzz diff --git a/buzz_scripts/include/vec2.bzz b/buzz_scripts/include/utils/vec2.bzz similarity index 100% rename from buzz_scripts/include/vec2.bzz rename to buzz_scripts/include/utils/vec2.bzz diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index 0a7b01f..4c0e06b 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -19,7 +19,13 @@ var v_ground = {} b_updating = 0 vstig_counter = WAIT4STEP -function uav_initstig() { + +function init_swarm() { + s = swarm.create(1) + s.join() +} + +function init_stig() { v_status = stigmergy.create(STATUS_VSTIG) v_ground = stigmergy.create(GROUND_VSTIG) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz new file mode 100644 index 0000000..6bf2856 --- /dev/null +++ b/buzz_scripts/main.bzz @@ -0,0 +1,81 @@ +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 "vstigenv.bzz" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +goal_list = { + .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} +} + +function action() { + statef=action + set_goto(idle) +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + # nei_cmd_listen() + + # Starting state + # BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCHED" +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # graph state machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCHED") # ends on navigate + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure + statef=makegraph # or bidding + else if(BVMSTATE=="PATHPLAN") # ends on navigate + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + statef() + + log("Current state: ", BVMSTATE) + +} + +# 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/testRRT.bzz b/buzz_scripts/testRRT.bzz deleted file mode 100644 index 3ad4cc6..0000000 --- a/buzz_scripts/testRRT.bzz +++ /dev/null @@ -1,45 +0,0 @@ -include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" - -function action() { - statef=action - uav_storegoal(45.5088103899,-73.1540826153,2.5) - set_goto(idle) -} - -# Executed once at init time. -function init() { - uav_initstig() - uav_initswarm() - - TARGET_ALTITUDE = 2.5 # m - # statef=turnedoff - # BVMSTATE = "TURNEDOFF" - statef = takeoff - BVMSTATE = "TAKEOFF" -} - -# Executed at each time step. -function step() { - uav_rccmd() - - statef() - - log("Current state: ", BVMSTATE) - # log("Obstacles: ") - # reduce(proximity, - # function(key, value, acc) { - # log(key, " - ", value.angle, value.value) - # return acc - # }, math.vec2.new(0, 0)) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index db43d6f..507e70c 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -201,7 +201,7 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); @@ -255,7 +255,7 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); From da86b4be817452a324a2cc0484774ca0267ab4fa Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 26 Feb 2018 13:26:33 -0500 Subject: [PATCH 11/77] added minimal ROSBuzz example script --- buzz_scripts/empty.bzz | 17 ----- buzz_scripts/include/act/states.bzz | 89 +++++---------------------- buzz_scripts/include/plan/rrtstar.bzz | 58 +++++++++++++++++ buzz_scripts/main.bzz | 17 +++-- buzz_scripts/minimal.bzz | 64 +++++++++++++++++++ 5 files changed, 145 insertions(+), 100 deletions(-) delete mode 100644 buzz_scripts/empty.bzz create mode 100644 buzz_scripts/minimal.bzz diff --git a/buzz_scripts/empty.bzz b/buzz_scripts/empty.bzz deleted file mode 100644 index 3bb5b12..0000000 --- a/buzz_scripts/empty.bzz +++ /dev/null @@ -1,17 +0,0 @@ -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/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index d8e3790..e987b60 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -4,7 +4,6 @@ # ######################################## include "utils/vec2.bzz" -include "plan/rrtstar.bzz" include "act/barrier.bzz" include "utils/conversions.bzz" @@ -33,7 +32,7 @@ function launch() { if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") barrier_ready() } else { log("Altitude: ", pose.position.altitude) @@ -41,7 +40,7 @@ function launch() { uav_takeoff(TARGET_ALTITUDE) } } else { - barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") barrier_ready() } } @@ -73,78 +72,20 @@ function take_picture() { pic_time=pic_time+1 } -# -# Work in progress.... -function navigate() { - BVMSTATE="NAVIGATE" - if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz - uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) - cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) - } - - print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) - if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { +function goto_gps(transf) { + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") - return - } - - # create the map - if(map==nil) { - dyn_init_map(cur_goal) - if(V_TYPE == 0) { - homegps.lat = pose.position.latitude - homegps.long = pose.position.longitude - } - } - - if(Path==nil) { - # add proximity sensor and neighbor obstacles to the map - populateGrid(m_pos) - # start the planner - path_it = 1 - pathPlanner(cur_goal, m_pos) - } else if(path_it <= size(Path)) { - var cur_path_pt = convert_pt(getvec(Path, path_it)) - print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) - if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { - populateGrid(m_pos) - if(check_path(Path, path_it, m_pos, 0)) { - # stop - goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) - Path = nil - if(V_TYPE == 0) - cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - # re-start the planner - path_it = 1 - pathPlanner(cur_goal, m_pos) - } else { - cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) - goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) - } - } else - path_it = path_it + 1 - } else { - Path = nil - BVMSTATE="IDLE" - } + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity + m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + transf() + else + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } -# function goto_direct(transf) { -# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) -# print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - -# if(math.vec2.length(m_navigation)>GOTO_MAXDIST) -# log("Sorry this is too far.") -# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity -# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) -# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -# } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination -# transf() -# else -# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -# } - function follow() { if(size(targets)>0) { BVMSTATE = "FOLLOW" @@ -208,9 +149,9 @@ function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") { + if(value==22 and BVMSTATE!="BARRIERWAIT") { BVMSTATE = "LAUNCH" - } else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") { + } else if(value==21 and BVMSTATE!="BARRIERWAIT") { BVMSTATE = "STOP" } else if(value==400 and BVMSTATE=="TURNEDOFF") { uav_arm() diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 6d40626..bfdd065 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -14,6 +14,64 @@ cur_cell = {} nei_cell = {} mapgoal = {} + +# +# Work in progress.... +function navigate() { + BVMSTATE="NAVIGATE" + if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz + uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) + } + + print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) + if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { + log("Sorry this is too far.") + return + } + + # create the map + if(map==nil) { + dyn_init_map(cur_goal) + if(V_TYPE == 0) { + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude + } + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else if(path_it <= size(Path)) { + var cur_path_pt = convert_pt(getvec(Path, path_it)) + print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) + if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, path_it, m_pos, 0)) { + # stop + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + Path = nil + if(V_TYPE == 0) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else { + cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + } + } else + path_it = path_it + 1 + } else { + Path = nil + BVMSTATE="IDLE" + } +} + # create a map with its length to fit the goal (rel. pos.) and the current position as the center # TODO: test with khepera/argos function dyn_init_map(m_navigation) { diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 6bf2856..2563c3e 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -2,8 +2,12 @@ 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 "plan/rrtstar.bzz" include "vstigenv.bzz" +#State launched after takeoff +AUTO_LAUNCH_STATE = "IDLE" + ##### # Vehicule type: # 0 -> outdoor flying vehicle @@ -16,11 +20,6 @@ goal_list = { .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} } -function action() { - statef=action - set_goto(idle) -} - # Executed once at init time. function init() { init_stig() @@ -31,7 +30,7 @@ function init() { # start the swarm command listener # nei_cmd_listen() - # Starting state + # Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup. # BVMSTATE = "TURNEDOFF" BVMSTATE = "LAUNCHED" } @@ -51,15 +50,15 @@ function step() { statef=turnedoff else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop - else if(BVMSTATE=="LAUNCHED") # ends on navigate + else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE statef=launch else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure statef=makegraph # or bidding - else if(BVMSTATE=="PATHPLAN") # ends on navigate + else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar statef=rrtstar - else if(BVMSTATE=="NAVIGATE") # ends on idle + else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar statef=navigate else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure statef=follow diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz new file mode 100644 index 0000000..8f0494b --- /dev/null +++ b/buzz_scripts/minimal.bzz @@ -0,0 +1,64 @@ +include "update.bzz" +include "act/states.bzz" +include "vstigenv.bzz" + +# State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +# Executed once at init time. +function init() { + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +function action() { + BVMSTATE = "ACTION" + # do some actions.... +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="ACTION") + statef=action + + statef() + + log("Current state: ", BVMSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From ed59c6de7c0653e736be4f31bf67dedf3a652a1d Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 2 Mar 2018 15:35:06 -0500 Subject: [PATCH 12/77] ported AGF to new rosbuzz file structure --- buzz_scripts/include/act/states.bzz | 8 +- .../include/taskallocate/graphformGPS.bzz | 188 +++++++----------- buzz_scripts/main.bzz | 33 ++- buzz_scripts/mainRRT.bzz | 80 ++++++++ 4 files changed, 182 insertions(+), 127 deletions(-) create mode 100644 buzz_scripts/mainRRT.bzz diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index e987b60..9dfc8fc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -73,17 +73,17 @@ function take_picture() { } function goto_gps(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } function follow() { @@ -94,7 +94,7 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - uav_moveto(attractor.x, attractor.y, 0.0) + goto_abs(attractor.x, attractor.y, 0.0) } else { log("No target in local table!") BVMSTATE = "IDLE" diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index d4e5aae..e35fe37 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -1,18 +1,11 @@ # # Include files # -include "string.bzz" -include "vec2.bzz" -include "update.bzz" -#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. -include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. -include "uavstates.bzz" # require an 'action' function to be defined here. - -include "graphs/shapes_P.bzz" -include "graphs/shapes_O.bzz" -include "graphs/shapes_L.bzz" -include "graphs/shapes_Y.bzz" +include "taskallocate/graphs/shapes_P.bzz" +include "taskallocate/graphs/shapes_O.bzz" +include "taskallocate/graphs/shapes_L.bzz" +include "taskallocate/graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -20,9 +13,6 @@ ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 old_state = -1 -# max velocity in cm/step -ROBOT_MAXVEL = 150.0 - # # Global variables # @@ -40,12 +30,12 @@ m_MessageBearing={}#store received neighbour message m_neighbourCount=0#used to cunt neighbours #Save message from one neighbour #the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing -m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} +m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} # #Save the message to send #The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} +m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} #navigation vector m_navigation={.x=0,.y=0} @@ -117,38 +107,38 @@ function count(table,value){ # function i2s(value){ if(value==1){ - return "STATE_FREE" + return "GRAPH_FREE" } else if(value==2){ - return "STATE_ASKING" + return "GRAPH_ASKING" } else if(value==3){ - return "STATE_JOINING" + return "GRAPH_JOINING" } else if(value==4){ - return "STATE_JOINED" + return "GRAPH_JOINED" } else if(value==5){ - return "STATE_LOCK" + return "GRAPH_LOCK" } } # #map from state to int # function s2i(value){ - if(value=="STATE_FREE"){ + if(value=="GRAPH_FREE"){ return 1 } - else if(value=="STATE_ASKING"){ + else if(value=="GRAPH_ASKING"){ return 2 } - else if(value=="STATE_JOINING"){ + else if(value=="GRAPH_JOINING"){ return 3 } - else if(value=="STATE_JOINED"){ + else if(value=="GRAPH_JOINED"){ return 4 } - else if(value=="STATE_LOCK"){ + else if(value=="GRAPH_LOCK"){ return 5 } } @@ -344,11 +334,11 @@ function UpdateNodeInfo(){ var i=0 while(i0) @@ -455,7 +446,7 @@ function DoFree() { var i=0 var j=0 while(i outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 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() { + init_stig() + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + # nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup. + # BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCHED" +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # graph state machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure + statef=makegraph # or bidding + else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + statef() + + log("Current state: ", BVMSTATE) + +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From b798abf2834b74c3eebb4164684aed3dd42c1002 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 3 Mar 2018 19:41:54 -0500 Subject: [PATCH 13/77] added yaw control and states pursuit, agreggation and formation (still to tweak) --- buzz_scripts/include/act/states.bzz | 75 ++++++++++++++++++- buzz_scripts/include/plan/rrtstar.bzz | 4 +- .../include/taskallocate/graphformGPS.bzz | 2 +- buzz_scripts/main.bzz | 10 ++- src/buzzuav_closures.cpp | 18 +++-- src/roscontroller.cpp | 14 ++-- 6 files changed, 101 insertions(+), 22 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9dfc8fc..02d1fdc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -66,7 +66,7 @@ function take_picture() { if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize uav_takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture - BVMSTATE="IDLE" + BVMSTATE="IDLE" pic_time=0 } pic_time=pic_time+1 @@ -79,11 +79,11 @@ function goto_gps(transf) { log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } function follow() { @@ -94,13 +94,80 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - goto_abs(attractor.x, attractor.y, 0.0) + goto_abs(attractor.x, attractor.y, 0.0, 0.0) } else { log("No target in local table!") BVMSTATE = "IDLE" } } +# converge to centroid +function aggregate() { + BVMSTATE="AGGREGATE" + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + math.vec2.scale(centroid, 1.0 / neighbors.count()) + if(math.vec2.length(centroid)>GOTO_MAXVEL) + centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid)) + goto_abs(centroid.x, centroid.y, 0.0, 0.0) +} + +# follow one another +rotang = 0.0 +function pursuit() { + BVMSTATE="PURSUIT" + insight = 0 + leader = math.vec2.newp(0.0, 0.0) + var cmdbin = math.vec2.newp(0.0, 0.0) + neighbors.foreach(function(rid, data) { + if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) { + insight = 1 + leader = math.vec2.newp(data.distance, data.azimuth) + } + }) + if(insight == 1) { + log("Leader in sight !") + #cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader)) + cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader)) + } else { + rotang = rotang + math.pi/60 + cmdbin = math.vec2.newp(2.0, rotang) + } + goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang) +} + +# Lennard-Jones interaction magnitude +TARGET = 8.0 +EPSILON = 0.000001 +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function formation() { + BVMSTATE="FORMATION" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + if(math.vec2.length(accum)>GOTO_MAXVEL*15) + accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum)) + goto_abs(accum.x, accum.y, 0.0, 0.0) +} + function rc_cmd_listen() { if(flight.rc_cmd==22) { log("cmd 22") diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index bfdd065..54c69ab 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -53,7 +53,7 @@ function navigate() { populateGrid(m_pos) if(check_path(Path, path_it, m_pos, 0)) { # stop - goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0) Path = nil if(V_TYPE == 0) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) @@ -62,7 +62,7 @@ function navigate() { pathPlanner(cur_goal, m_pos) } else { cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) - goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0) } } else path_it = path_it + 1 diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index e35fe37..0252971 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -694,7 +694,7 @@ function DoLock(){ m_navigation=motion_vector() } #move - goto_abs(m_navigation.x, m_navigation.y, 0.0) + goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() } # diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1cfd45c..d898bb3 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "PURSUIT" ##### # Vehicule type: @@ -26,7 +26,7 @@ function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 25.0 # m + TARGET_ALTITUDE = 10 + id*2.0 # m # start the swarm command listener nei_cmd_listen() @@ -54,6 +54,12 @@ function step() { statef=launch else if(BVMSTATE=="IDLE") statef=idle + else if(BVMSTATE=="AGGREGATE") + statef=aggregate + else if(BVMSTATE=="FORMATION") + statef=formation + else if(BVMSTATE=="PURSUIT") + statef=pursuit else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ? startGraph() } else if(BVMSTATE=="GRAPH_FREE") { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 20e0456..b55f9eb 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -13,7 +13,7 @@ namespace buzzuav_closures { // TODO: Minimize the required global variables and put them in the header // static const rosbzz_node::roscontroller* roscontroller_ptr; -static double goto_pos[3]; +static double goto_pos[4]; static double rc_goto_pos[3]; static float rc_gimbal[4]; static float batt[3]; @@ -207,22 +207,26 @@ int buzz_exportmap(buzzvm_t vm) int buzzuav_moveto(buzzvm_t vm) /* -/ Buzz closure to move following a 2D vector +/ Buzz closure to move following a 3D vector + Yaw /----------------------------------------*/ { - buzzvm_lnum_assert(vm, 3); + buzzvm_lnum_assert(vm, 4); buzzvm_lload(vm, 1); // dx buzzvm_lload(vm, 2); // dy - buzzvm_lload(vm, 3); //* dheight + buzzvm_lload(vm, 3); // dheight + buzzvm_lload(vm, 4); // dyaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dh = buzzvm_stack_at(vm, 1)->f.value; - float dy = buzzvm_stack_at(vm, 2)->f.value; - float dx = buzzvm_stack_at(vm, 3)->f.value; + float dyaw = buzzvm_stack_at(vm, 1)->f.value; + float dh = buzzvm_stack_at(vm, 2)->f.value; + float dy = buzzvm_stack_at(vm, 3)->f.value; + float dx = buzzvm_stack_at(vm, 4)->f.value; goto_pos[0] = dx; goto_pos[1] = dy; goto_pos[2] = height + dh; + goto_pos[3] = dyaw; // DEBUG // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // goto_pos[1], goto_pos[2]); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cfd47c0..cf54154 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -711,7 +711,7 @@ script case buzzuav_closures::COMMAND_MOVETO: goto_pos = buzzuav_closures::getgoto(); - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; case buzzuav_closures::COMMAND_GIMBAL: @@ -935,15 +935,17 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.header.frame_id = 1; // DEBUG - // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y); + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f, %.3f", cur_pos.x, cur_pos.y, x, y, yaw); moveMsg.pose.position.x = cur_pos.x + y; moveMsg.pose.position.y = cur_pos.y + x; moveMsg.pose.position.z = z; - moveMsg.pose.orientation.x = 0; - moveMsg.pose.orientation.y = 0; - moveMsg.pose.orientation.z = 0; - moveMsg.pose.orientation.w = 1; + tf::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + moveMsg.pose.orientation.x = q[0]; + moveMsg.pose.orientation.y = q[1]; + moveMsg.pose.orientation.z = q[2]; + moveMsg.pose.orientation.w = q[3]; // To prevent drifting from stable position, uncomment // if(fabs(x)>0.005 || fabs(y)>0.005) { From c9364666e651da88c56e30711aa7aaad8f29fcc2 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 3 Mar 2018 22:43:12 -0500 Subject: [PATCH 14/77] enhanced states switch and zooids rate --- buzz_scripts/include/act/states.bzz | 38 ++++++++------ .../include/taskallocate/graphformGPS.bzz | 52 +++++++++---------- buzz_scripts/main.bzz | 21 ++++---- 3 files changed, 56 insertions(+), 55 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 02d1fdc..fa6cffc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,12 +10,12 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXVEL = 2.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. path_it = 0 -rc_State = 0 +graphid = 0 pic_time = 0 g_it = 0 @@ -163,8 +163,8 @@ function formation() { var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) math.vec2.scale(accum, 1.0 / neighbors.count()) - if(math.vec2.length(accum)>GOTO_MAXVEL*15) - accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum)) + if(math.vec2.length(accum)>GOTO_MAXVEL*10) + accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum)) goto_abs(accum.x, accum.y, 0.0, 0.0) } @@ -195,19 +195,22 @@ function rc_cmd_listen() { stattab_send() } else if (flight.rc_cmd==900){ flight.rc_cmd=0 - rc_State = 0 + BVMSTATE = "TASK_ALLOCATE" neighbors.broadcast("cmd", 900) } else if (flight.rc_cmd==901){ flight.rc_cmd=0 - rc_State = 1 + destroyGraph() + BVMSTATE = "PURSUIT" neighbors.broadcast("cmd", 901) } else if (flight.rc_cmd==902){ flight.rc_cmd=0 - rc_State = 2 + destroyGraph() + BVMSTATE = "AGGREGATE" neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 - rc_State = 3 + destroyGraph() + BVMSTATE = "FORMATION" neighbors.broadcast("cmd", 903) } } @@ -224,14 +227,17 @@ function nei_cmd_listen() { uav_arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ uav_disarm() - } else if(value==900){ - rc_State = 0 - } else if(value==901){ - rc_State = 1 - } else if(value==902){ - rc_State = 2 - } else if(value==903){ - rc_State = 3 + } else if(value==900){ # Shapes + BVMSTATE = "TASK_ALLOCATE" + } else if(value==901){ # Pursuit + destroyGraph() + BVMSTATE = "PURSUIT" + } else if(value==902){ # Agreggate + destroyGraph() + BVMSTATE = "AGGREGATE" + } else if(value==903){ # Formation + destroyGraph() + BVMSTATE = "FORMATION" } else if(value==16 and BVMSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 0252971..826ee01 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -11,7 +11,6 @@ ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 -old_state = -1 # # Global variables @@ -401,7 +400,7 @@ function TransitionToJoined(){ m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) } # #Transistion to state Lock, lock the current formation @@ -422,10 +421,8 @@ function TransitionToLock(){ } m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x, m_navigation.y, 0.0) + goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) - # prepare to restart a new shape - old_state = rc_State #stop listening neighbors.ignore("m") } @@ -536,7 +533,7 @@ function DoAsking(){ m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) BroadcastGraph() } # @@ -666,20 +663,14 @@ function DoJoined(){ } } - #check if should to transists to lock - #write statues - #v_tag.get(m_nLabel) - #log(v_tag.size(), " of ", ROBOTS, " ready to lock") - #if(v_tag.size()==ROBOTS){ - # TransitionToLock() - #} barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED") BroadcastGraph() } # #Do Lock # -function DoLock(){ +timeout_graph = 40 +function DoLock() { UpdateGraph() m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.Label=m_nLabel @@ -693,19 +684,23 @@ function DoLock(){ if(m_nLabel!=0){ m_navigation=motion_vector() } - #move +# #move goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() -} -# -# Executed after takeoff -# -function startGraph(){ - BVMSTATE="GRAPH_FREE" - # reset the graph - statef=resetGraph + if(loop) { + if(timeout_graph==0) { + if(graphid < 3) + graphid = graphid + 1 + else + graphid = 0 + timeout_graph = 40 + BVMSTATE="TASK_ALLOCATE" + } + timeout_graph = timeout_graph - 1 + } } + # # Executed at init # @@ -750,6 +745,7 @@ function BroadcastGraph() { # Executed when reset # function resetGraph(){ + BVMSTATE="GRAPH_FREE" m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} m_navigation={.x=0,.y=0} @@ -766,16 +762,16 @@ function resetGraph(){ assign_id=-1 m_gotjoinedparent = 0 - if(rc_State==0){ + if(graphid==0){ log("Loading P graph") Read_GraphP() - } else if(rc_State==1) { + } else if(graphid==1) { log("Loading O graph") Read_GraphO() - } else if(rc_State==2) { + } else if(graphid==2) { log("Loading L graph") Read_GraphL() - } else if(rc_State==3) { + } else if(graphid==3) { log("Loading Y graph") Read_GraphY() } @@ -802,7 +798,7 @@ function destroyGraph() { #clear neighbour message m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) m_vecNodes={} #stop listening neighbors.ignore("m") diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d898bb3..7191155 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "PURSUIT" +AUTO_LAUNCH_STATE = "TASK_ALLOCATE" ##### # Vehicule type: @@ -27,6 +27,7 @@ function init() { init_swarm() TARGET_ALTITUDE = 10 + id*2.0 # m + loop = 1 # start the swarm command listener nei_cmd_listen() @@ -60,21 +61,19 @@ function step() { statef=formation else if(BVMSTATE=="PURSUIT") statef=pursuit - else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ? - startGraph() - } else if(BVMSTATE=="GRAPH_FREE") { + else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ? + statef=resetGraph + else if(BVMSTATE=="GRAPH_FREE") statef=DoFree - } else if(BVMSTATE=="GRAPH_ASKING") { + else if(BVMSTATE=="GRAPH_ASKING") statef=DoAsking - } else if(BVMSTATE=="GRAPH_JOINING") { + else if(BVMSTATE=="GRAPH_JOINING") statef=DoJoining - } else if(BVMSTATE=="GRAPH_JOINED") { + else if(BVMSTATE=="GRAPH_JOINED") statef=DoJoined - } else if(BVMSTATE=="GRAPH_TRANSTOLOCK") + else if(BVMSTATE=="GRAPH_TRANSTOLOCK") statef=TransitionToLock - else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State) #switch to a new graph - startGraph() - else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State) # move all together (TODO: not tested) + else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list statef=DoLock else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar statef=rrtstar From a1d13d312a3d97f4ab5a5ac020ad280c8c28eb29 Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 4 Mar 2018 21:44:39 -0500 Subject: [PATCH 15/77] fixed formation and enhanced aggregation --- buzz_scripts/include/act/states.bzz | 59 ++++++++----------- .../include/taskallocate/graphformGPS.bzz | 11 +--- buzz_scripts/include/utils/conversions.bzz | 6 ++ buzz_scripts/main.bzz | 2 +- 4 files changed, 35 insertions(+), 43 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index fa6cffc..3e27726 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -77,13 +77,12 @@ function goto_gps(transf) { print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) - } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() - else + else { + LimitSpeed(m_navigation, 1.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + } } function follow() { @@ -110,40 +109,34 @@ function aggregate() { }, {.x=0, .y=0}) if(neighbors.count() > 0) math.vec2.scale(centroid, 1.0 / neighbors.count()) - if(math.vec2.length(centroid)>GOTO_MAXVEL) - centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid)) - goto_abs(centroid.x, centroid.y, 0.0, 0.0) + cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS)) + cmdbin = LimitSpeed(cmdbin, 1.0/2.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # follow one another rotang = 0.0 function pursuit() { BVMSTATE="PURSUIT" - insight = 0 - leader = math.vec2.newp(0.0, 0.0) - var cmdbin = math.vec2.newp(0.0, 0.0) - neighbors.foreach(function(rid, data) { - if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) { - insight = 1 - leader = math.vec2.newp(data.distance, data.azimuth) - } - }) - if(insight == 1) { - log("Leader in sight !") - #cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader)) - cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader)) - } else { - rotang = rotang + math.pi/60 - cmdbin = math.vec2.newp(2.0, rotang) - } - goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang) + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + math.vec2.scale(centroid, 1.0 / neighbors.count()) + rotang = rotang + math.pi/50.0 + cmdbin = math.vec2.sub(centroid, math.vec2.newp(80.0, id * 2 * math.pi / ROBOTS + rotang)) + cmdbin = LimitSpeed(cmdbin, 1.0/5.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 0.000001 +EPSILON = 3.0 function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + # log(dist, (target / dist), (epsilon / dist), lj) + return lj } # Neighbor data to LJ interaction vector @@ -157,15 +150,15 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction +old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) - math.vec2.scale(accum, 1.0 / neighbors.count()) - if(math.vec2.length(accum)>GOTO_MAXVEL*10) - accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum)) - goto_abs(accum.x, accum.y, 0.0, 0.0) + math.vec2.scale(accum_lj, 1.0 / neighbors.count()) + old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) + goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) } function rc_cmd_listen() { diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 826ee01..61e77fd 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -78,14 +78,7 @@ m_fTargetDistanceTolerance=0 m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 4000 #13.5 the LJ parameter for other robots - -# Lennard-Jones interaction magnitude - -function FlockInteraction(dist,target,epsilon){ - var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) - return mag -} +EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots # #return the number of value in table @@ -264,7 +257,7 @@ function LJ_vec(i){ var target=target4label(nei_id) var cDir={.x=0.0,.y=0.0} if(target!="miss"){ - cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),ljbearing) + cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) } #log(id,"dis=",dis,"target=",target,"label=",nei_id) #log("x=",cDir.x,"y=",cDir.y) diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 144cabf..50a58bc 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -7,6 +7,12 @@ function LimitAngle(angle){ return angle } +function LimitSpeed(vel_vec, factor){ + if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) + vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) + return vel_vec +} + # TODO: add other conversions.... function convert_path(P) { var pathR={} diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 7191155..1927d66 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "AGGREGATE" ##### # Vehicule type: From 2f25df02f0ba38f3648c2e4d7ae38b30248b8018 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 5 Mar 2018 00:10:18 -0500 Subject: [PATCH 16/77] minor fixes and fixed pursuit (vector field guidance) --- buzz_scripts/include/act/states.bzz | 30 ++++++++++++++++------------- buzz_scripts/main.bzz | 2 +- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 3e27726..573a92f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -108,26 +108,31 @@ function aggregate() { return centroid }, {.x=0, .y=0}) if(neighbors.count() > 0) - math.vec2.scale(centroid, 1.0 / neighbors.count()) - cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS)) + centroid = math.vec2.scale(centroid, 1.0 / neighbors.count()) + cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS)) cmdbin = LimitSpeed(cmdbin, 1.0/2.0) goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # follow one another -rotang = 0.0 function pursuit() { BVMSTATE="PURSUIT" - centroid = neighbors.reduce(function(rid, data, centroid) { - centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) - return centroid + rd = 15.0 + pc = 3.2 + vd = 15.0 + r_vec = neighbors.reduce(function(rid, data, r_vec) { + r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth)) + return r_vec }, {.x=0, .y=0}) if(neighbors.count() > 0) - math.vec2.scale(centroid, 1.0 / neighbors.count()) - rotang = rotang + math.pi/50.0 - cmdbin = math.vec2.sub(centroid, math.vec2.newp(80.0, id * 2 * math.pi / ROBOTS + rotang)) - cmdbin = LimitSpeed(cmdbin, 1.0/5.0) - goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) + r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) + r = math.vec2.length(r_vec) + gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + dr = -gamma * (r-rd) + dT = gamma * pc + vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) + vfg = LimitSpeed(vfg, 2.0) + goto_abs(vfg.x, vfg.y, 0.0, 0.0) } # Lennard-Jones interaction magnitude @@ -135,7 +140,6 @@ TARGET = 8.0 EPSILON = 3.0 function lj_magnitude(dist, target, epsilon) { lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) - # log(dist, (target / dist), (epsilon / dist), lj) return lj } @@ -156,7 +160,7 @@ function formation() { # Calculate accumulator accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) - math.vec2.scale(accum_lj, 1.0 / neighbors.count()) + accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1927d66..e87f015 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "AGGREGATE" +AUTO_LAUNCH_STATE = "PURSUIT" ##### # Vehicule type: From e553d6a14642a518483ed93197240f6f948c9024 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 27 Apr 2018 10:31:24 -0400 Subject: [PATCH 17/77] fixes for Zooids demo view angle --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 573a92f..9668295 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -114,7 +114,7 @@ function aggregate() { goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } -# follow one another +# circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" rd = 15.0 From cd47fd3eb59fb8bea9d3a4857e5b23558912efc8 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 9 Mar 2018 09:37:56 -0600 Subject: [PATCH 18/77] Update readme.md --- readme.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/readme.md b/readme.md index 82d3941..d61cdb4 100644 --- a/readme.md +++ b/readme.md @@ -97,7 +97,7 @@ The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its st References ------ -* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 +* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 * Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. @@ -108,4 +108,4 @@ To activate highlights of the code in Visual Studio Code or Roboware add the fol "files.associations": { "*.bzz":"python" } -``` \ No newline at end of file +``` From 44212c8fcdfa6c9ceed760b48d436221535d8e52 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 27 Apr 2018 11:42:59 -0400 Subject: [PATCH 19/77] rosbuzz dev enabled in sim, fixed testaloneWP in dev --- buzz_scripts/testaloneWP.bzz | 42 ++++++++++++++++++++++++++++-------- src/buzzuav_closures.cpp | 2 +- 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index e438fb1..70531f4 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -1,27 +1,51 @@ -include "vec2.bzz" include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. +# 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 "vstigenv.bzz" + +#State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" function action() { + BVMSTATE = "ACTION" uav_storegoal(-1.0,-1.0,-1.0) - set_goto(picture) + goto_gps(picture) } # Executed once at init time. function init() { - statef=turnedoff - UAVSTATE = "TURNEDOFF" - TARGET_ALTITUDE = 15.0 + init_stig() + init_swarm() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" } # Executed at each time step. function step() { - uav_rccmd() + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="ACTION") + statef=action statef() - log("Current state: ", UAVSTATE) + log("Current state: ", BVMSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index b55f9eb..364f2f8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -96,7 +96,7 @@ void setWPlist(string path) / set the absolute path for a csv list of waypoints ----------------------------------------------------------- */ { - WPlistname = path + "include/graphs/waypointlist.csv"; + WPlistname = path + "include/taskallocate/waypointlist.csv"; } float constrainAngle(float x) From 6d52a57dcd29876b67dbdf4b4e8db9549dab9cdd Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 14 Jun 2018 13:08:32 -0400 Subject: [PATCH 20/77] battery topic type changed to sensor_msgs --- include/roscontroller.h | 3 ++- src/roscontroller.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 7599c58..5259343 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -12,6 +12,7 @@ #include "mavros_msgs/SetMode.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" +#include "sensor_msgs/BatteryState.h" #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" @@ -205,7 +206,7 @@ private: double gps_r_lat); /*battery status callback */ - void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); + void battery(const sensor_msgs::BatteryState::ConstPtr& msg); /*flight extended status callback*/ void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cf54154..9dc1374 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -836,12 +836,12 @@ void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); }; -void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) +void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg) /* / Update battery status into BVM from subscriber /------------------------------------------------------*/ { - buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining); + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); // DEBUG // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // msg->current, msg ->remaining); From a9038601aef3393634d79149056d273bf4e56e4f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 14 Jun 2018 13:37:00 -0400 Subject: [PATCH 21/77] added update changes and neighbour pos clear on dev --- include/buzz_update.h | 248 +++---- include/buzzuav_closures.h | 4 + include/roscontroller.h | 1 + src/buzz_update.cpp | 1293 ++++++++++++++++++------------------ src/buzzuav_closures.cpp | 5 + src/roscontroller.cpp | 28 +- 6 files changed, 799 insertions(+), 780 deletions(-) diff --git a/include/buzz_update.h b/include/buzz_update.h index 5680f85..4b43e86 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,161 +1,171 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H -/*Simulation or robot check*/ -//#define SIMULATION 1 // set in CMAKELIST #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include + #define delete_p(p) \ do \ { \ free(p); \ p = NULL; \ } while (0) - -static const uint16_t CODE_REQUEST_PADDING = 250; -static const uint16_t MIN_UPDATE_PACKET = 251; -static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; -static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; -/*********************/ -/* Updater states */ -/********************/ - -typedef enum { - CODE_RUNNING = 0, // Code executing - CODE_STANDBY, // Standing by for others to update -} code_states_e; - -/*********************/ -/*Message types */ -/********************/ - -typedef enum { - SENT_CODE = 0, // Broadcast code - RESEND_CODE, // ReBroadcast request -} code_message_e; - -/*************************/ -/*Updater message queue */ -/*************************/ - -struct updater_msgqueue_s +namespace buzz_update { - uint8_t* queue; - uint8_t* size; -}; -typedef struct updater_msgqueue_s* updater_msgqueue_t; + static const uint16_t CODE_REQUEST_PADDING = 250; + static const uint16_t MIN_UPDATE_PACKET = 251; + static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; + static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; + /*********************/ + /* Updater states */ + /********************/ -struct updater_code_s -{ - uint8_t* bcode; - uint8_t* bcode_size; -}; -typedef struct updater_code_s* updater_code_t; + typedef enum { + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update + } code_states_e; -/**************************/ -/*Updater data*/ -/**************************/ + /*********************/ + /*Message types */ + /********************/ -struct buzz_updater_elem_s -{ - /* robot id */ - // uint16_t robotid; - /*current Bytecode content */ - uint8_t* bcode; - /*old Bytecode name */ - const char* old_bcode; - /*current bcode size*/ - size_t* bcode_size; - /*Update patch*/ - uint8_t* patch; - /* Update patch size*/ - size_t* patch_size; - /*current Bytecode content */ - uint8_t* standby_bcode; - /*current bcode size*/ - size_t* standby_bcode_size; - /*updater out msg queue */ - updater_msgqueue_t outmsg_queue; - /*updater in msg queue*/ - updater_msgqueue_t inmsg_queue; - /*Current state of the updater one in code_states_e ENUM*/ - int* mode; - uint8_t* update_no; -}; -typedef struct buzz_updater_elem_s* buzz_updater_elem_t; + typedef enum { + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request + } code_message_e; -/**************************************************************************/ -/*Updater routine from msg processing to file checks to be called from main*/ -/**************************************************************************/ -void update_routine(); + /*************************/ + /*Updater message queue */ + /*************************/ -/************************************************/ -/*Initalizes the updater */ -/************************************************/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + struct updater_msgqueue_s + { + uint8_t* queue; + uint8_t* size; + }; + typedef struct updater_msgqueue_s* updater_msgqueue_t; -/*********************************************************/ -/*Appends buffer of given size to in msg queue of updater*/ -/*********************************************************/ + struct updater_code_s + { + uint8_t* bcode; + uint8_t* bcode_size; + }; + typedef struct updater_code_s* updater_code_t; -void code_message_inqueue_append(uint8_t* msg, uint16_t size); + /**************************/ + /*Updater data*/ + /**************************/ -/*********************************************************/ -/*Processes messages inside the queue of the updater*/ -/*********************************************************/ + struct buzz_updater_elem_s + { + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; + }; + typedef struct buzz_updater_elem_s* buzz_updater_elem_t; -void code_message_inqueue_process(); + /**************************************************************************/ + /*Updater routine from msg processing to file checks to be called from main*/ + /**************************************************************************/ + void update_routine(); -/*****************************************************/ -/* obtains messages from out msgs queue of the updater*/ -/*******************************************************/ -uint8_t* getupdater_out_msg(); + /************************************************/ + /*Initalizes the updater */ + /************************************************/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); -/******************************************************/ -/*obtains out msg queue size*/ -/*****************************************************/ -uint8_t* getupdate_out_msg_size(); + /*********************************************************/ + /*Appends buffer of given size to in msg queue of updater*/ + /*********************************************************/ -/**************************************************/ -/*destroys the out msg queue*/ -/*************************************************/ -void destroy_out_msg_queue(); + void code_message_inqueue_append(uint8_t* msg, uint16_t size); -/***************************************************/ -/*obatins updater state*/ -/***************************************************/ -// int get_update_mode(); + /*********************************************************/ + /*Processes messages inside the queue of the updater*/ + /*********************************************************/ -// buzz_updater_elem_t get_updater(); -/***************************************************/ -/*sets bzz file name*/ -/***************************************************/ -void set_bzz_file(const char* in_bzz_file); + void code_message_inqueue_process(); -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); + /*****************************************************/ + /*Obtains messages from out msgs queue of the updater*/ + /*******************************************************/ + uint8_t* getupdater_out_msg(); -/****************************************************/ -/*Destroys the updater*/ -/***************************************************/ + /******************************************************/ + /*Obtains out msg queue size*/ + /*****************************************************/ + uint8_t* getupdate_out_msg_size(); -void destroy_updater(); + /**************************************************/ + /*Destroys the out msg queue*/ + /*************************************************/ + void destroy_out_msg_queue(); -int is_msg_present(); + // buzz_updater_elem_t get_updater(); + /***************************************************/ + /*Sets bzz file name*/ + /***************************************************/ + void set_bzz_file(const char* in_bzz_file, bool dbg); -int get_update_status(); + /****************************************************/ + /*Tests the code from a buffer*/ + /***************************************************/ -void set_read_update_status(); + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); -int compile_bzz(std::string bzz_file); + /****************************************************/ + /*Destroys the updater*/ + /***************************************************/ -void updates_set_robots(int robots); + void destroy_updater(); -// void set_packet_id(int packet_id); + /****************************************************/ + /*Checks for updater message*/ + /***************************************************/ -// void collect_data(std::ofstream& logger); + int is_msg_present(); + + /****************************************************/ + /*Compiles a bzz script to bo and bdbg*/ + /***************************************************/ + + int compile_bzz(std::string bzz_file); + + /****************************************************/ + /*Set number of robots in the updater*/ + /***************************************************/ + + void updates_set_robots(int robots); +} #endif diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e6c3f48..7de79e1 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -118,6 +118,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation) * update neighbors from in msgs */ void update_neighbors(buzzvm_t vm); +/* + *Clear neighbours struct + */ +void clear_neighbours_pos(); /* * closure to add a neighbor status */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 5259343..a366bf4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -97,6 +97,7 @@ private: float fcu_timeout; int armstate; int barrier; + int update; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 1d169f9..8624c3a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -1,599 +1,633 @@ -#include -#include -#include -#include -#include +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + #include "buzz_update.h" -#include -#include -#include -#include -/*Temp for data collection*/ -// static int neigh=-1; -static struct timeval t1, t2; -static int timer_steps = 0; -// static clock_t end; +namespace buzz_update{ + /*Temp for data collection*/ + // static int neigh=-1; + static struct timeval t1, t2; + static int timer_steps = 0; + // static clock_t end; -/*Temp end */ -static int fd, wd = 0; -static int old_update = 0; -static buzz_updater_elem_t updater; -static int no_of_robot; -static const char* dbgf_name; -static const char* bcfname; -static const char* old_bcfname = "old_bcode.bo"; -static const char* bzz_file; -static int Robot_id = 0; -static int neigh = -1; -static int updater_msg_ready; -static uint16_t update_try_counter = 0; -static int updated = 0; -static const uint16_t MAX_UPDATE_TRY = 5; -static int packet_id_ = 0; -static size_t old_byte_code_size = 0; + /*Temp end */ + static int fd, wd = 0; + static int old_update = 0; + static buzz_updater_elem_t updater; + static int no_of_robot; + static const char* dbgf_name; + static const char* bcfname; + static const char* old_bcfname = "old_bcode.bo"; + static const char* bzz_file; + static int Robot_id = 0; + static int neigh = -1; + static int updater_msg_ready; + static uint16_t update_try_counter = 0; + static const uint16_t MAX_UPDATE_TRY = 5; + static size_t old_byte_code_size = 0; + static bool debug = false; -/*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) -{ - Robot_id = robot_id; - dbgf_name = dbgfname; - bcfname = bo_filename; - ROS_INFO("Initializing file monitor..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) + /*Initialize updater*/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 1*/ - if (SIMULATION == 1 && robot_id == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - else if (SIMULATION == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if (!fp) - { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bo_filename); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if (!fp) - { - perror(stand_by_script); - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) - { - perror(stand_by_script); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*)malloc(sizeof(size_t)); - updater->bcode_size = (size_t*)malloc(sizeof(size_t)); - updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) = 0; - *(size_t*)(updater->bcode_size) = bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; - updater->mode = (int*)malloc(sizeof(int)); - *(int*)(updater->mode) = CODE_RUNNING; - // no_of_robot=barrier; - updater_msg_ready = 0; - - /*Write the bcode to a file for rollback*/ - fp = fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); -} -/*Check for .bzz file chages*/ -int check_update() -{ - char buf[1024]; - int check = 0; - int i = 0; - int len = read(fd, buf, 1024); - while (i < len) - { - struct inotify_event* event = (struct inotify_event*)&buf[i]; - /* file was modified this flag is true in nano and self delet in gedit and other editors */ - // fprintf(stdout,"inside file monitor.\n"); - if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) - { - /*respawn watch if the file is self deleted */ - inotify_rm_watch(fd, wd); - close(fd); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); + if(debug) ROS_INFO("Initializing file monitor..."); fd = inotify_init1(IN_NONBLOCK); - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /* To mask multiple writes from editors*/ - if (!old_update) + if (fd < 0) { - check = 1; - old_update = 1; + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; + + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + return 1; + } + else{ + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } + } + /*Check for .bzz file chages*/ + int check_update() + { + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /*Report file changes and self deletes*/ + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*Respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /*To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /*Update index to start of next event*/ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; + } + + int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) + { + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*Write the patch to a file and call bsdiff to patch*/ + FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); + fwrite(patch, update_patch_size, 1, fp); + fclose(fp); + std::stringstream patch_exsisting; + patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 + << "tmp_patch.bo"; + if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str()); + if (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } + } + + updater_code_t obtain_patched_bo(std::string& path, std::string& name1) + { + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + if(debug){ + ROS_WARN("Simulation patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + if(debug) { + ROS_WARN("Robot patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + } + + void code_message_outqueue_append() + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; + } + + void outqueue_append_code_request(uint16_t update_no) + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter); + } + + void code_message_inqueue_append(uint8_t* msg, uint16_t size) + { + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; + } + /*Used for data logging*/ + /*void set_packet_id(int packet_id) + { + packet_id_ = packet_id; + }*/ + void code_message_inqueue_process() + { + int size = 0; + updater_code_t out_code = NULL; + if(debug) { + ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode)); + ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_WARN("[Debug] Updater received patch of size %u", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + } + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); + + if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + if(debug) ROS_WARN("Received patch PASSED tests!"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } } } - /* update index to start of next event */ - i += sizeof(struct inotify_event) + event->len; - } - if (!check) - old_update = 0; - return check; -} - -int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) -{ - if (SIMULATION == 1) - { - return 1; - } - else - { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file*/ - FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); - fwrite(patch, update_patch_size, 1, fp); - fclose(fp); - std::stringstream patch_exsisting; - patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 - << "tmp_patch.bo"; - fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); - if (system(patch_exsisting.str().c_str())) - return 0; - else - return 1; - } -} - -updater_code_t obtain_patched_bo(std::string& path, std::string& name1) -{ - if (SIMULATION == 1) - { - /*Read the exsisting file to simulate the patching*/ - std::stringstream read_patched; - read_patched << path << name1 << ".bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - - else - { - /*Read the new patched file*/ - std::stringstream read_patched; - read_patched << path << name1 << "-patched.bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - - /* delete old bo file & rename new bo file */ - remove((path + name1 + ".bo").c_str()); - rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); - - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } -} - -void code_message_outqueue_append() -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; - size += padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size) = size; - size = 0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); - size += sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); - // size += (uint16_t) * (size_t*)(updater->patch_size); - updater_msg_ready = 1; -} - -void outqueue_append_code_request(uint16_t update_no) -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size = 0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; - updater_msg_ready = 1; - ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); -} - -void code_message_inqueue_append(uint8_t* msg, uint16_t size) -{ - updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); - updater->inmsg_queue->queue = (uint8_t*)malloc(size); - updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memcpy(updater->inmsg_queue->queue, msg, size); - *(uint16_t*)(updater->inmsg_queue->size) = size; -} -/*Used for data logging*/ -/*void set_packet_id(int packet_id) -{ - packet_id_ = packet_id; -}*/ -void code_message_inqueue_process() -{ - int size = 0; - updater_code_t out_code = NULL; - - ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); - ROS_INFO("[Debug] Updater received patch of size %u \n", - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - - if (*(int*)(updater->mode) == CODE_RUNNING) - { - // fprintf(stdout,"[debug]Inside inmsg code running"); - if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) { + if(debug) ROS_WARN("Patch rebroadcast request received."); size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) { - // fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); size += sizeof(uint16_t); - uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { - out_code = obtain_patched_bo(path, name1); + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter); + code_message_outqueue_append(); + } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); + } + } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } - // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - // FILE *fp; - // fp=fopen("update.bo", "wb"); - // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - // fclose(fp); + void create_update_patch() + { + std::stringstream genpatch; - if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) - { - printf("TEST PASSED!\n"); - *(uint16_t*)updater->update_no = update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); - *(size_t*)(updater->patch_size) = update_patch_size; - // code_message_outqueue_append(); - neigh = 1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-update"; + + /*Launch bsdiff and create a patch*/ + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + /*Delete old files & rename new files*/ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; + + /* Delete the diff file */ + remove(patchfileName.str().c_str()); + } + + void update_routine() + { + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \tUpdating script ..."); + + if (compile_bzz(bzz_file)) + { + ROS_ERROR("Error in compiling script, resuming old script."); } else { - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); - update_try_counter++; - outqueue_append_code_request(update_no); + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh = -1; + if(debug) ROS_INFO("Broadcasting patch ..."); + code_message_outqueue_append(); + } + delete_p(BO_BUF); } } } - } - size = 0; - if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) - { - size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + + else { - size += sizeof(uint16_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) { - update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); - ROS_ERROR("Code appended! update try : %u \n", update_try_counter); - code_message_outqueue_append(); + ROS_WARN("Patch deployment successful, rolling forward"); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, rolling back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + update_try_counter = 0; + timer_steps = 0; } - if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: ROLL BACK !!"); } } - // fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); -} -void create_update_patch() -{ - std::stringstream genpatch; - - std::string bzzfile_name(bzz_file); - - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - - std::string name2 = name1 + "-update"; - - // CALL BSDIFF CMD - genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; - fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); - system(genpatch.str().c_str()); - - // BETTER: CALL THE FUNCTION IN BSDIFF.CPP - // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - - /* delete old files & rename new files */ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName << path << "diff.bo"; - - uint8_t* patch_buff = 0; - FILE* fp = fopen(patchfileName.str().c_str(), "rb"); - if (!fp) + uint8_t* getupdater_out_msg() { - perror(patchfileName.str().c_str()); + return (uint8_t*)updater->outmsg_queue->queue; } - fseek(fp, 0, SEEK_END); - size_t patch_size = ftell(fp); - rewind(fp); - patch_buff = (uint8_t*)malloc(patch_size); - if (fread(patch_buff, 1, patch_size, fp) < patch_size) + + uint8_t* getupdate_out_msg_size() { - perror(patchfileName.str().c_str()); + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; } - fclose(fp); - delete_p(updater->patch); - updater->patch = patch_buff; - *(size_t*)(updater->patch_size) = patch_size; - /* Delete the diff file */ - remove(patchfileName.str().c_str()); -} - -void update_routine() -{ - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if (*(int*)updater->mode == CODE_RUNNING) + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) { - buzzvm_function_call(VM, "updated_neigh", 0); - // Check for update - if (check_update()) + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) { - ROS_INFO("Update found \nUpdating script ...\n"); - - if (compile_bzz(bzz_file)) + if(debug) ROS_WARN("Initializtion test passed"); + if (buzz_utility::update_step_test()) { - ROS_WARN("Errors in comipilg script so staying on old script\n"); + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + if(debug) ROS_WARN("Step test passed"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; } + /*Unable to step something wrong*/ else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << path << name << "-update.bo"; - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit - if (!fp) + if (*(int*)(updater->mode) == CODE_RUNNING) { - perror(bzzfile_in_compile.str().c_str()); + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + else { - perror(bcfname); - } - fclose(fp); - if (test_set_code(BO_BUF, dbgf_name, bcode_size)) - { - uint16_t update_no = *(uint16_t*)(updater->update_no); - *(uint16_t*)(updater->update_no) = update_no + 1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + /*You will never reach here*/ + ROS_ERROR("Step test failed, returning to standby"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - neigh = -1; - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); } - delete_p(BO_BUF); + return 0; } } - } - - else - { - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n", tObj->i.value); - if (tObj->i.value == no_of_robot) - { - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - // collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - // buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - else if (timer_steps > TIMEOUT_FOR_ROLLBACK) - { - ROS_ERROR("TIME OUT Reached, decided to roll back"); - /*Time out hit deceided to roll back*/ - *(int*)(updater->mode) = CODE_RUNNING; - buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - } -} - -uint8_t* getupdater_out_msg() -{ - return (uint8_t*)updater->outmsg_queue->queue; -} - -uint8_t* getupdate_out_msg_size() -{ - // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; -} - -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) -{ - if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) - { - ROS_WARN("Initializtion of script test passed\n"); - if (buzz_utility::update_step_test()) - { - /*data logging*/ - old_byte_code_size = *(size_t*)updater->bcode_size; - /*data logging*/ - ROS_WARN("Step test passed\n"); - *(int*)(updater->mode) = CODE_STANDBY; - // fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - return 1; - } - /*Unable to step something wrong*/ else { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("step test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + ROS_ERROR("Initialization test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("step test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, returning to standby"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -604,118 +638,87 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) return 0; } } - else - { - if (*(int*)(updater->mode) == CODE_RUNNING) - { - ROS_ERROR("Initialization test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); - } - else - { - /*You will never reach here*/ - ROS_ERROR("Initialization test failed, Return to stand by\n"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - } - return 0; - } -} -void destroy_out_msg_queue() -{ - // fprintf(stdout,"out queue freed\n"); - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - updater_msg_ready = 0; -} -int get_update_status() -{ - return updated; -} -void set_read_update_status() -{ - updated = 0; -} -/*int get_update_mode() -{ - return (int)*(int*)(updater->mode); -}*/ - -int is_msg_present() -{ - return updater_msg_ready; -} -/*buzz_updater_elem_t get_updater() -{ - return updater; -}*/ -void destroy_updater() -{ - delete_p(updater->bcode); - delete_p(updater->bcode_size); - delete_p(updater->standby_bcode); - delete_p(updater->standby_bcode_size); - delete_p(updater->mode); - delete_p(updater->update_no); - if (updater->outmsg_queue) + void destroy_out_msg_queue() { + // fprintf(stdout,"out queue freed\n"); delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue); + updater_msg_ready = 0; } - if (updater->inmsg_queue) + + int is_msg_present() { - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); + return updater_msg_ready; + } + /*buzz_updater_elem_t get_updater() + { + return updater; + }*/ + void destroy_updater() + { + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) + { + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); } - inotify_rm_watch(fd, wd); - close(fd); -} -void set_bzz_file(const char* in_bzz_file) -{ - bzz_file = in_bzz_file; -} + void set_bzz_file(const char* in_bzz_file, bool dbg) + { + debug=dbg; + bzz_file = in_bzz_file; + } -void updates_set_robots(int robots) -{ - no_of_robot = robots; -} + void updates_set_robots(int robots) + { + no_of_robot = robots; + } -/*-------------------------------------------------------- -/ Create Buzz bytecode from the bzz script inputed -/-------------------------------------------------------*/ -int compile_bzz(std::string bzz_file) -{ - /*Compile the buzz code .bzz to .bo*/ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," - << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; -}*/ + /*void collect_data(std::ofstream& logger) + { + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // + Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; + }*/ +} \ No newline at end of file diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 364f2f8..6631ba1 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -678,6 +678,11 @@ void update_neighbors(buzzvm_t vm) } } +// Clear neighbours pos +void clear_neighbours_pos(){ + neighbors_map.clear(); +} + int buzzuav_update_currentpos(buzzvm_t vm) /* / Update the BVM position table diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9dc1374..e84025a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -27,7 +27,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) std::string fname = Compile_bzz(bzzfile_name); bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; - set_bzz_file(bzzfile_name.c_str()); + buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); // Initialize variables SetMode("LOITER", 0); @@ -107,7 +107,7 @@ void roscontroller::RosControllerRun() ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; // Intialize the update monitor - init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // DEBUG @@ -120,7 +120,7 @@ void roscontroller::RosControllerRun() grid_publisher(); send_MPpayload(); // Check updater state and step code - update_routine(); + if(update) buzz_update::update_routine(); if (time_step == BUZZRATE) { time_step = 0; @@ -144,15 +144,10 @@ void roscontroller::RosControllerRun() prepare_msg_and_publish(); // Call the flight controler service flight_controller_service_call(); - // Set multi message available after update - if (get_update_status()) - { - set_read_update_status(); - } // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - updates_set_robots(no_of_robots); + if(update) buzz_update::updates_set_robots(no_of_robots); // get_xbee_status(); // commented out because it may slow down the node too much, to be tested ros::spinOnce(); @@ -561,11 +556,11 @@ with size......... | / payload_pub.publish(payload_out); delete[] out; delete[] payload_out_ptr; - // Check for updater message if present send - if (is_msg_present()) + /// Check for updater message if present send + if (update && buzz_update::is_msg_present()) { uint8_t* buff_send = 0; - uint16_t updater_msgSize = *(uint16_t*)(getupdate_out_msg_size()); + uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size()); ; int tot = 0; mavros_msgs::Mavlink update_packets; @@ -578,10 +573,10 @@ with size......... | / *(uint16_t*)(buff_send + tot) = updater_msgSize; tot += sizeof(uint16_t); // Append updater msgs - memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); + memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize); tot += updater_msgSize; // Destroy the updater out msg queue - destroy_out_msg_queue(); + buzz_update::destroy_out_msg_queue(); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); uint64_t* payload_64 = new uint64_t[total_size]; memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); @@ -750,6 +745,7 @@ void roscontroller::maintain_pos(int tim_step) if (timer_step >= BUZZRATE) { neighbours_pos_map.clear(); + buzzuav_closures::clear_neighbours_pos(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear ! timer_step = 0; @@ -1031,8 +1027,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { - code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); - code_message_inqueue_process(); + buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + buzz_update::code_message_inqueue_process(); } free(pl); } From ece31509cb21c1085f450be7127be44a6962922f Mon Sep 17 00:00:00 2001 From: vivek Date: Thu, 14 Jun 2018 13:56:23 -0400 Subject: [PATCH 22/77] added solo config --- launch/launch_config/solo.yaml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 launch/launch_config/solo.yaml diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml new file mode 100644 index 0000000..b8a465d --- /dev/null +++ b/launch/launch_config/solo.yaml @@ -0,0 +1,22 @@ +topics: + gps : mavros/global_position/global + battery : mavros/battery + status : mavros/state + fcclient: mavros/cmd/command + setpoint: mavros/setpoint_position/local + armclient: mavros/cmd/arming + modeclient: mavros/set_mode + localpos: /mavros/local_position/pose + stream: mavros/set_stream_rate + altitude: mavros/global_position/rel_alt +type: + gps : sensor_msgs/NavSatFix + # for SITL Solo + # battery : mavros_msgs/BatteryState + # for solo + battery : mavros_msgs/BatteryStatus + status : mavros_msgs/State + altitude: std_msgs/Float64 +environment : + environment : solo-simulator + From 4845ff262d5a16f869ae7e23c4ec59fc6810ed49 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 15 Jun 2018 15:15:25 -0400 Subject: [PATCH 23/77] added launch_config arg and made BVM state optional on dev --- include/roscontroller.h | 1 + launch/rosbuzz.launch | 3 +- launch/rosbuzzd.launch | 3 +- src/roscontroller.cpp | 165 +++++++++++++++++++++------------------- 4 files changed, 93 insertions(+), 79 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index a366bf4..029eca5 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -98,6 +98,7 @@ private: int armstate; int barrier; int update; + int statepub_active; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index c2531f4..f632f68 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -6,9 +6,10 @@ + - + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch index de7857d..a89b1af 100644 --- a/launch/rosbuzzd.launch +++ b/launch/rosbuzzd.launch @@ -6,9 +6,10 @@ + - + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e84025a..413ccf9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -110,13 +110,24 @@ void roscontroller::RosControllerRun() update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // set ROS loop rate ros::Rate loop_rate(BUZZRATE); + // check for BVMSTATE variable + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1; + else + { + statepub_active = 0; + ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled."); + } // DEBUG // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { // Publish topics neighbours_pos_publisher(); - uavstate_publisher(); + if(statepub_active) uavstate_publisher(); grid_publisher(); send_MPpayload(); // Check updater state and step code @@ -652,88 +663,88 @@ script armstate = 0; break; - case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! - cmd_srv.request.param5 = home.latitude; - cmd_srv.request.param6 = home.longitude; - cmd_srv.request.param7 = home.altitude; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; + // case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + // cmd_srv.request.param5 = home.latitude; + // cmd_srv.request.param6 = home.longitude; + // cmd_srv.request.param7 = home.altitude; + // cmd_srv.request.command = buzzuav_closures::getcmd(); + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // break; - case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! - goto_pos = buzzuav_closures::getgoto(); - cmd_srv.request.param5 = goto_pos[0]; - cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; + // case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! + // goto_pos = buzzuav_closures::getgoto(); + // cmd_srv.request.param5 = goto_pos[0]; + // cmd_srv.request.param6 = goto_pos[1]; + // cmd_srv.request.param7 = goto_pos[2]; + // cmd_srv.request.command = buzzuav_closures::getcmd(); + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // break; - case buzzuav_closures::COMMAND_ARM: - if (!armstate) - { - SetMode("LOITER", 0); - armstate = 1; - Arm(); - } - break; + // case buzzuav_closures::COMMAND_ARM: + // if (!armstate) + // { + // SetMode("LOITER", 0); + // armstate = 1; + // Arm(); + // } + // break; - case buzzuav_closures::COMMAND_DISARM: - if (armstate) - { - armstate = 0; - SetMode("LOITER", 0); - Arm(); - } - break; + // case buzzuav_closures::COMMAND_DISARM: + // if (armstate) + // { + // armstate = 0; + // SetMode("LOITER", 0); + // Arm(); + // } + // break; - case buzzuav_closures::COMMAND_MOVETO: - goto_pos = buzzuav_closures::getgoto(); - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); - break; + // case buzzuav_closures::COMMAND_MOVETO: + // goto_pos = buzzuav_closures::getgoto(); + // roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); + // break; - case buzzuav_closures::COMMAND_GIMBAL: - gimbal = buzzuav_closures::getgimbal(); - cmd_srv.request.param1 = gimbal[0]; - cmd_srv.request.param2 = gimbal[1]; - cmd_srv.request.param3 = gimbal[2]; - cmd_srv.request.param4 = gimbal[3]; - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; + // case buzzuav_closures::COMMAND_GIMBAL: + // gimbal = buzzuav_closures::getgimbal(); + // cmd_srv.request.param1 = gimbal[0]; + // cmd_srv.request.param2 = gimbal[1]; + // cmd_srv.request.param3 = gimbal[2]; + // cmd_srv.request.param4 = gimbal[3]; + // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // break; - case buzzuav_closures::COMMAND_PICTURE: - ROS_INFO("TAKING A PICTURE HERE!! --------------"); - mavros_msgs::CommandBool capture_command; - if (capture_srv.call(capture_command)) - { - ROS_INFO("Reply: %ld", (long int)capture_command.response.success); - } - else - ROS_ERROR("Failed to call service from camera streamer"); - break; + // case buzzuav_closures::COMMAND_PICTURE: + // ROS_INFO("TAKING A PICTURE HERE!! --------------"); + // mavros_msgs::CommandBool capture_command; + // if (capture_srv.call(capture_command)) + // { + // ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + // } + // else + // ROS_ERROR("Failed to call service from camera streamer"); + // break; } } From 392531e1315f2351ebfcdc421687b09895ea7f81 Mon Sep 17 00:00:00 2001 From: Marius Klimavicius Date: Wed, 27 Jun 2018 16:52:58 -0400 Subject: [PATCH 24/77] solo sim parameters updated --- buzz_scripts/include/act/states.bzz | 3 ++- launch/launch_config/solo.yaml | 14 ++------------ launch/rosbuzz.launch | 2 +- src/roscontroller.cpp | 3 +-- 4 files changed, 6 insertions(+), 16 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9668295..0b80cea 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -127,7 +127,8 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + sqr = (r-rd)*(r-rd)+pc*pc*r*r + gamma = vd / math.sqrt(sqr) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index b8a465d..c18075b 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -6,17 +6,7 @@ topics: setpoint: mavros/setpoint_position/local armclient: mavros/cmd/arming modeclient: mavros/set_mode - localpos: /mavros/local_position/pose - stream: mavros/set_stream_rate + localpos: mavros/local_position/pose + stream: mavros/set_stream_rate altitude: mavros/global_position/rel_alt -type: - gps : sensor_msgs/NavSatFix - # for SITL Solo - # battery : mavros_msgs/BatteryState - # for solo - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/State - altitude: std_msgs/Float64 -environment : - environment : solo-simulator diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index f632f68..7f3c63b 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -3,7 +3,7 @@ - + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 413ccf9..1f25370 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1001,8 +1001,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) ROS_INFO("Set stream rate call failed!, trying again..."); ros::Duration(0.1).sleep(); } - // DEBUG - // ROS_INFO("Set stream rate call successful"); + ROS_WARN("Set stream rate call successful"); } void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) From 1bfeada346c627833264314a884ef51feb5a59d8 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 4 Jul 2018 21:17:42 -0400 Subject: [PATCH 25/77] adapted bzz files from sim. --- buzz_scripts/include/act/barrier.bzz | 36 ++++--- buzz_scripts/include/act/states.bzz | 148 +++++++++++++++++--------- buzz_scripts/include/plan/rrtstar.bzz | 2 +- buzz_scripts/include/update.bzz | 3 +- buzz_scripts/main.bzz | 13 ++- buzz_scripts/minimal.bzz | 1 - buzz_scripts/stand_by.bzz | 45 ++++---- buzz_scripts/testLJ.bzz | 72 +++++++++++++ 8 files changed, 229 insertions(+), 91 deletions(-) create mode 100755 buzz_scripts/testLJ.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 8b56f3c..a87bed7 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -7,10 +7,11 @@ # # Constants # -BARRIER_TIMEOUT = 1200 # in steps +BARRIER_TIMEOUT = 200 # in steps BARRIER_VSTIG = 80 timeW = 0 barrier = nil +bc = 0; # # Sets a barrier @@ -22,15 +23,15 @@ function barrier_create() { #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) if(barrier!=nil) { barrier=nil - BARRIER_VSTIG = BARRIER_VSTIG +1 + # BARRIER_VSTIG = BARRIER_VSTIG +1 } #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef) { +function barrier_set(threshold, transf, resumef, bc) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bc); } BVMSTATE = "BARRIERWAIT" barrier_create() @@ -39,30 +40,41 @@ function barrier_set(threshold, transf, resumef) { # # Make yourself ready # -function barrier_ready() { +function barrier_ready(bc) { #log("BARRIER READY -------") - barrier.put(id, 1) + barrier.put(id, bc) barrier.put("d", 0) } # # Executes the barrier # -function barrier_wait(threshold, transf, resumef) { - barrier.put(id, 1) - +function barrier_wait(threshold, transf, resumef, bc) { + barrier.put(id, bc) barrier.get(id) - #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + var allgood = 0 + log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + var bi = LOWEST_ROBOT_ID + allgood = 1 + while (bi= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") - barrier=nil + barrier = nil timeW = 0 BVMSTATE = resumef - } + } else if(timeW % 10 == 0 and bc > 0) + neighbors.broadcast("cmd", bc) timeW = timeW+1 } diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 0b80cea..cda775b 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -27,44 +27,60 @@ function idle() { BVMSTATE = "IDLE" } +# Custom function for the user. +function cusfun(){ + BVMSTATE="CUSFUN" + log("cusfun: yay!!!") +} + function launch() { BVMSTATE = "LAUNCH" - if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") - barrier_ready() + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) + barrier_ready(22) } else { log("Altitude: ", pose.position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } } else { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") - barrier_ready() + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) + barrier_ready(22) } } +gohomeT=100 +function goinghome() { + BVMSTATE = "GOHOME" + if(gohomeT > 0) { # TODO: Make a real check if home is reached + gohome() + gohomeT = gohomeT - 1 + } else + BVMSTATE = AUTO_LAUNCH_STATE +} + function stop() { - BVMSTATE = "STOP" + BVMSTATE = "STOP" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,"TURNEDOFF","STOP") - barrier_ready() + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) } } else { - barrier_set(ROBOTS,"TURNEDOFF","STOP") - barrier_ready() - } + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) + } } function take_picture() { BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) + setgimbal(0.0, 0.0, -90.0, 20.0) if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() + takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture BVMSTATE="IDLE" pic_time=0 @@ -117,7 +133,7 @@ function aggregate() { # circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" - rd = 15.0 + rd = 20.0 pc = 3.2 vd = 15.0 r_vec = neighbors.reduce(function(rid, data, r_vec) { @@ -127,8 +143,7 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - sqr = (r-rd)*(r-rd)+pc*pc*r*r - gamma = vd / math.sqrt(sqr) + gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) @@ -138,9 +153,9 @@ function pursuit() { # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 3.0 +EPSILON = 30.0 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) return lj } @@ -155,92 +170,119 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction -old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) - old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) - goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) + accum_lj = LimitSpeed(accum_lj, 1.0/3.0) + goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# listens to commands from the remote control (web, commandline, etc) function rc_cmd_listen() { - if(flight.rc_cmd==22) { + if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 BVMSTATE = "LAUNCH" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { - log("cmd 21") flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "TURNEDOFF" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + barrier_ready(21) BVMSTATE = "STOP" neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) + neighbors.broadcast("cmd", 20) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 BVMSTATE = "PATHPLAN" } else if(flight.rc_cmd==400) { flight.rc_cmd=0 - uav_arm() + arm() neighbors.broadcast("cmd", 400) } else if (flight.rc_cmd==401){ flight.rc_cmd=0 - uav_disarm() + disarm() neighbors.broadcast("cmd", 401) } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() } else if (flight.rc_cmd==900){ flight.rc_cmd=0 - BVMSTATE = "TASK_ALLOCATE" + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + barrier_ready(900) neighbors.broadcast("cmd", 900) } else if (flight.rc_cmd==901){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "PURSUIT" + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + barrier_ready(901) neighbors.broadcast("cmd", 901) } else if (flight.rc_cmd==902){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "AGGREGATE" + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_ready(902) neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "FORMATION" + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + barrier_ready(903) neighbors.broadcast("cmd", 903) } } +# listens to other fleet members broadcasting commands function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="BARRIERWAIT") { - BVMSTATE = "LAUNCH" - } else if(value==21 and BVMSTATE!="BARRIERWAIT") { - BVMSTATE = "STOP" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() - } else if(value==900){ # Shapes - BVMSTATE = "TASK_ALLOCATE" - } else if(value==901){ # Pursuit - destroyGraph() - BVMSTATE = "PURSUIT" - } else if(value==902){ # Agreggate - destroyGraph() - BVMSTATE = "AGGREGATE" - } else if(value==903){ # Formation - destroyGraph() - BVMSTATE = "FORMATION" - } else if(value==16 and BVMSTATE=="IDLE"){ - # neighbors.listen("gt",function(vid, value, rid) { - # print("Got (", vid, ",", value, ") from robot #", rid) - # # if(gt.id == id) statef=goto - # }) + if(BVMSTATE!="BARRIERWAIT") { + if(value==22) { + BVMSTATE = "LAUNCH" + }else if(value==20) { + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" + } else if(value==21) { + AUTO_LAUNCH_STATE = "TURNEDOFF" + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + disarm() + } else if(value==900){ # Shapes + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + #barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if(value==901){ # Pursuit + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + #barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if(value==902){ # Agreggate + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + #barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if(value==903){ # Formation + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + #barrier_ready(903) + neighbors.broadcast("cmd", 903) + } else if(value==16 and BVMSTATE=="IDLE"){ + # neighbors.listen("gt",function(vid, value, rid) { + # print("Got (", vid, ",", value, ") from robot #", rid) + # # if(gt.id == id) statef=goto + # }) + } } }) } diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 54c69ab..a55aff1 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -20,7 +20,7 @@ mapgoal = {} function navigate() { BVMSTATE="NAVIGATE" if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz - uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) } diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index f49fd7c..4a5c699 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -4,6 +4,7 @@ #################################################################################################### updated="update_ack" update_no=0 -function updated_neigh(){ +updates_active = 1 +function updated_no_bct(){ neighbors.broadcast(updated, update_no) } \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e87f015..d52fd95 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,11 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "PURSUIT" +AUTO_LAUNCH_STATE = "CUSFUN" +#Lowest robot id in the network +LOWEST_ROBOT_ID = 97 +TARGET = 9.0 +EPSILON = 30.0 ##### # Vehicule type: @@ -21,12 +25,13 @@ goal_list = { .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} } + # Executed once at init time. function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 10 + id*2.0 # m + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 # start the swarm command listener @@ -49,10 +54,14 @@ function step() { # if(BVMSTATE=="TURNEDOFF") statef=turnedoff + else if(BVMSTATE=="CUSFUN") + statef=cusfun else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE statef=launch + else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE + statef=goinghome else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="AGGREGATE") diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz index 8f0494b..a49329b 100644 --- a/buzz_scripts/minimal.bzz +++ b/buzz_scripts/minimal.bzz @@ -51,7 +51,6 @@ function step() { statef=action statef() - log("Current state: ", BVMSTATE) } diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz index 1349089..c605089 100644 --- a/buzz_scripts/stand_by.bzz +++ b/buzz_scripts/stand_by.bzz @@ -1,35 +1,38 @@ +include "act/states.bzz" +include "vstigenv.bzz" + updated="update_ack" update_no=0 -function init(){ -barrier = stigmergy.create(101) -barrier.put(id,1) -barrier_val=0 -barrier.onconflict(function (k,l,r) { -if(r. data < l. data or (r. data == l. data )) return l -else return r -}) +BVMSTATE = "UPDATESTANDBY" -s = swarm.create(1) -s.join() +# Executed once at init time. +function init(){ + barrier = stigmergy.create(101) + barrier.put(id,1) + barrier_val=0 + barrier.onconflict(function (k,l,r) { + if(r. data < l. data or (r. data == l. data )) return l + else return r + }) + init_swarm() + # start the swarm command listener + nei_cmd_listen() } function stand_by(){ + barrier.get(id) + barrier_val = barrier.size() -barrier.get(id) -barrier_val = barrier.size() - -neighbors.listen(updated, - function(vid, value, rid) { - print(" got from",vid," ", " value = ",value," ","rid"," " ) - if(value==update_no) barrier.put(rid,1) - } + neighbors.listen(updated, + function(vid, value, rid) { + if(value==update_no) barrier.put(rid,1) + } ) } - +# Executed at each time step. function step() { - -stand_by() + stand_by() } diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz new file mode 100755 index 0000000..5240618 --- /dev/null +++ b/buzz_scripts/testLJ.bzz @@ -0,0 +1,72 @@ +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 "vstigenv.bzz" + +V_TYPE = 0 + +#State launched after takeoff +AUTO_LAUNCH_STATE = "FORMATION" + + +TARGET = 8.0 +EPSILON = 3.0 +GOTO_MAXVEL = 2.5 # m/steps + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" + TAKEOFF_COUNTER = 20 +} + +# Executed at each time step. +function step() { + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="FORMATION") + statef=formation + + statef() + + log("Current state: ", BVMSTATE) + + # Auto-takeoff (delayed for simulator boot) + if(id == 0) { + if(TAKEOFF_COUNTER>0) + TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1 + else if(TAKEOFF_COUNTER == 0) { + BVMSTATE="LAUNCH" + TAKEOFF_COUNTER = -1 + } + } +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From de648255619c703e02aa92e891cdcba994608db6 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 4 Jul 2018 21:45:35 -0400 Subject: [PATCH 26/77] added data logging --- src/roscontroller.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1f25370..5e97929 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -60,6 +60,18 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } + // Create log dir and open log file + std::string path = + bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; + std::string folder_check="mkdir -p "+path; + system(folder_check.c_str()); + for(int i=5;i>0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; + log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } roscontroller::~roscontroller() @@ -69,6 +81,8 @@ roscontroller::~roscontroller() { // Destroy the BVM buzz_utility::buzz_script_destroy(); + // Close the data logging file + log.close(); } void roscontroller::GetRobotId() @@ -149,6 +163,13 @@ void roscontroller::RosControllerRun() } if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + // log data + log< Date: Wed, 4 Jul 2018 22:22:42 -0400 Subject: [PATCH 27/77] moved BVM state publisher to buzz_utility --- include/buzz_utility.h | 2 ++ include/buzzuav_closures.h | 5 +---- src/buzz_utility.cpp | 15 +++++++++++++++ src/buzzuav_closures.cpp | 13 ------------- src/roscontroller.cpp | 2 +- 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 12cf011..600b9e2 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -77,4 +77,6 @@ buzzvm_t get_vm(); void set_robot_var(int ROBOTS); int get_inmsg_size(); + +std::string getuavstate(); } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 7de79e1..dbd3110 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -98,10 +98,7 @@ double* getgoto(); * returns the current grid */ std::map> getgrid(); -/* - * returns the current Buzz state - */ -std::string getuavstate(); + /* * returns the gimbal commands */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 507e70c..0862533 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -564,4 +564,19 @@ int get_inmsg_size() { return IN_MSG.size(); } +string getuavstate() +/* +/ return current BVM state +---------------------------------------*/ +{ + std::string uav_state = "Unknown"; + if(VM && VM->strings){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + uav_state = string(obj->s.value.str); + buzzvm_pop(VM); + } + return uav_state; +} } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6631ba1..d0a9ea6 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -479,19 +479,6 @@ float* getgimbal() return rc_gimbal; } -string getuavstate() -/* -/ return current BVM state ----------------------------------------*/ -{ - static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_gload(VM); - buzzobj_t uav_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - return uav_state->s.value.str; -} - int getcmd() /* / return current mavros command to the FC diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5e97929..5f71a82 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -475,7 +475,7 @@ void roscontroller::uavstate_publisher() /----------------------------------------------------*/ { std_msgs::String uavstate_msg; - uavstate_msg.data = buzzuav_closures::getuavstate(); + uavstate_msg.data = buzz_utility::getuavstate(); uavstate_pub.publish(uavstate_msg); } From ecb2aa67fe7b359bea0c561f056947ce7c8bdaa4 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 5 Jul 2018 02:46:51 -0400 Subject: [PATCH 28/77] states.bzz syntax fix --- buzz_scripts/include/act/states.bzz | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index cda775b..f08b0e8 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -143,7 +143,8 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + var sqr = (r-rd)*(r-rd)+pc*pc*r*r + gamma = vd / math.sqrt(sqr) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) From 0836571c8e449f2f35151f66f01af9608936bc75 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 10 Jul 2018 14:03:26 -0400 Subject: [PATCH 29/77] barrier enhance fix on branch dev --- buzz_scripts/include/act/barrier.bzz | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index a87bed7..f7d5910 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -55,13 +55,7 @@ function barrier_wait(threshold, transf, resumef, bc) { var allgood = 0 log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { - var bi = LOWEST_ROBOT_ID - allgood = 1 - while (bi Date: Wed, 11 Jul 2018 18:41:50 -0400 Subject: [PATCH 30/77] Time Sync algorithm and rc cmd implementation to sync time on request --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/act/states.bzz | 15 ++++- buzz_scripts/include/timesync.bzz | 86 +++++++++++++++++++++++++++ buzz_scripts/include/utils/string.bzz | 12 ++-- buzz_scripts/main.bzz | 7 +++ include/buzz_utility.h | 2 + include/roscontroller.h | 1 + misc/cmdlinectr.sh | 3 + src/buzz_utility.cpp | 17 ++++++ src/roscontroller.cpp | 10 +++- 10 files changed, 145 insertions(+), 10 deletions(-) create mode 100644 buzz_scripts/include/timesync.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index f7d5910..d7e141c 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -87,4 +87,4 @@ function barrier_allgood(barrier, bc) { } ) return barriergood -} +} \ No newline at end of file diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f08b0e8..6f94586 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -215,7 +215,13 @@ function rc_cmd_listen() { } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() - } else if (flight.rc_cmd==900){ + } else if (flight.rc_cmd==777){ + flight.rc_cmd=0 + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ flight.rc_cmd=0 barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) barrier_ready(900) @@ -259,7 +265,12 @@ function nei_cmd_listen() { arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() - } else if(value==900){ # Shapes + } else if(value==777 and BVMSTATE=="TURNEDOFF"){ + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) neighbors.broadcast("cmd", 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..c441bf5 --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -0,0 +1,86 @@ +include "utils/string.bzz" + +# Time to be sync +logical_time = 0 +# sync algo. constants +TIME_JUMP_THR = 5 +TIME_TO_FORGET = 20 +TIME_TO_SYNC = 100 +# table to store neighbor time data +time_nei_table = {} +# Algo. global parameters +diffMaxLogical = 0 +jumped = 0 +syncError = 99999 +sync_timer = 0 + +# Function to intialize sync algorithm +function init_time_sync(){ + neighbors.listen("time_sync", + function(vid, value, rid) { + log(" TIME SYNC Got (", vid, ",", value, ") #", rid) + var msg = string.split(value,",") + var msg_time = string.toint(msg[0]) + var msg_max = string.toint(msg[1]) + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } + ) +} + +# Function to sync. algo +function step_time_sync(){ + logical_time = logical_time + 1 + sync_timer = sync_timer + 1 + if(sync_timer < TIME_TO_SYNC){ + log(" SYNC ALGO ACTIVE time:", sync_timer) + cnt = 0 + avg_offset = 0 + foreach(time_nei_table, function(key, value) { + if(value != nil){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) + time_nei_table[key] = nil + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } + } + + jumped = 0 + syncError=0 + var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + neighbors.broadcast("time_sync",mstr) + } + log("Logical time now ", logical_time) +} + +# Function to set sync timer to zero and reinitiate sync. algo + +function reinit_time_sync(){ + sync_timer = 0 +} \ No newline at end of file diff --git a/buzz_scripts/include/utils/string.bzz b/buzz_scripts/include/utils/string.bzz index 4140a1b..91ceb63 100644 --- a/buzz_scripts/include/utils/string.bzz +++ b/buzz_scripts/include/utils/string.bzz @@ -17,10 +17,10 @@ string.charat = function(s, n) { # RETURN: The position of the first match, or nil # string.indexoffirst = function(s, m) { - var ls = string.length(s) + var las = string.length(s) var lm = string.length(m) var i = 0 - while(i < ls-lm+1) { + while(i < las-lm+1) { if(string.sub(s, i, i+lm) == m) return i i = i + 1 } @@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) { # RETURN: The position of the last match, or nil # string.indexoflast = function(s, m) { - var ls = string.length(s) + var las = string.length(s) var lm = string.length(m) - var i = ls - lm + 1 + var i = las - lm + 1 while(i >= 0) { if(string.sub(s, i, i+lm) == m) return i i = i - 1 @@ -56,10 +56,10 @@ string.split = function(s, d) { var i2 = 0 # index to move along s (token end) var c = 0 # token count var t = {} # token list - var ls = string.length(s) + var las = string.length(s) var ld = string.length(d) # Go through string s - while(i2 < ls) { + while(i2 < las) { # Try every delimiter var j = 0 # index to move along d var f = nil # whether the delimiter was found or not diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d52fd95..0a3bc32 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -5,6 +5,7 @@ include "act/states.bzz" include "plan/rrtstar.bzz" include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" +include "timesync.bzz" #State launched after takeoff AUTO_LAUNCH_STATE = "CUSFUN" @@ -31,6 +32,8 @@ function init() { init_stig() init_swarm() + init_time_sync() + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -43,6 +46,10 @@ function init() { # Executed at each time step. function step() { + + # step time sync algorithm + step_time_sync() + # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 600b9e2..1c3c506 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); std::string getuavstate(); + +int getlogicaltime(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 029eca5..566b1a0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -43,6 +43,7 @@ #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 +#define CMD_SYNC_CLOCK 777 using namespace std; diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 00df45f..68b4296 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -11,6 +11,9 @@ function arm { function disarm { rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0 } +function timesync { + rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0 +} function testWP { rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10 } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 0862533..c3986d0 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -564,6 +564,7 @@ int get_inmsg_size() { return IN_MSG.size(); } + string getuavstate() /* / return current BVM state @@ -579,4 +580,20 @@ string getuavstate() } return uav_state; } + +int getlogicaltime() +/* +/ return current logical time +--------------------------------------*/ +{ + int logical_time = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + buzzvm_pop(VM); + } + return logical_time; +} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5f71a82..cb00b3e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -165,6 +165,8 @@ void roscontroller::RosControllerRun() ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data log<"); + res.success = true; + break; default: buzzuav_closures::rc_call(req.command); - ROS_INFO("----> Received unregistered command: ", req.command); + ROS_ERROR("----> Received unregistered command: ", req.command); res.success = true; break; } From d3f0299b7e86e27442adf42dcf6a760250b6dd71 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 23:18:35 -0400 Subject: [PATCH 31/77] adapted syc algo to robots --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/timesync.bzz | 85 +++++++++++++++------------- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index d7e141c..1a08ac4 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 10 == 0 and bc > 0) + } else if(timeW % 50 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index c441bf5..f190457 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -5,7 +5,8 @@ logical_time = 0 # sync algo. constants TIME_JUMP_THR = 5 TIME_TO_FORGET = 20 -TIME_TO_SYNC = 100 +TIME_TO_SYNC = 200 +COM_DELAY = 3 # table to store neighbor time data time_nei_table = {} # Algo. global parameters @@ -18,20 +19,23 @@ sync_timer = 0 function init_time_sync(){ neighbors.listen("time_sync", function(vid, value, rid) { - log(" TIME SYNC Got (", vid, ",", value, ") #", rid) - var msg = string.split(value,",") - var msg_time = string.toint(msg[0]) - var msg_max = string.toint(msg[1]) - #log("msg: 1: ", msg_time, " 2: ", msg_max ) - diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) - var time_offset = msg_time - logical_time - if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset - if(time_offset > TIME_JUMP_THR){ - logical_time = logical_time + time_offset - diffMaxLogical = math.max(diffMaxLogical-time_offset,0) - jumped = 1 + if(value != nil){ + log(" TIME SYNC Got (", vid, ",", value.time , " , ", value.max, ") #", rid) + var msg_time = value.time + var msg_max = value.max + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + if(msg_time != nil and msg_max != nil){ + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } } - time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} } ) } @@ -44,36 +48,39 @@ function step_time_sync(){ log(" SYNC ALGO ACTIVE time:", sync_timer) cnt = 0 avg_offset = 0 - foreach(time_nei_table, function(key, value) { - if(value != nil){ - #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) - var local_offset = value.time - logical_time - value.age - if(local_offset > 0){ - avg_offset = avg_offset + 1 * local_offset - cnt = cnt + 1 - } - else{ - if(math.abs(local_offset) 0){ + foreach(time_nei_table, function(key, value) { + if(value.time != 0){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) - time_nei_table[key] = nil - value.age = value.age + 1 + if(value.age > TIME_TO_FORGET) + value.time = 0 + + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } } - }) - if(cnt > 0 and jumped != 1){ - var correction = math.ceil(avg_offset / (cnt + 1) ) - if(math.abs(correction) < TIME_JUMP_THR){ - logical_time = logical_time + correction - } } - jumped = 0 syncError=0 - var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } + #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) neighbors.broadcast("time_sync",mstr) } log("Logical time now ", logical_time) From d0fa6d647f79bc2cac3d42becf70d1c57bfbe724 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 23:51:38 -0400 Subject: [PATCH 32/77] swithced debug on --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cb00b3e..e4f7f12 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -11,7 +11,7 @@ namespace rosbzz_node { const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; -const bool debug = false; +const bool debug = true; roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) /* From 81b263fa0ae95c842b57953e9547b4b9c591647f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 12 Jul 2018 01:01:34 -0400 Subject: [PATCH 33/77] adapted barrier rebroadcast time on robots --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/act/states.bzz | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 1a08ac4..27628b0 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 50 == 0 and bc > 0) + } else if(timeW % 100 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 6f94586..51624a2 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -269,7 +269,7 @@ function nei_cmd_listen() { if(logical_time != nil) reinit_time_sync() barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) barrier_ready(777) - neighbors.broadcast("cmd", 777) + #neighbors.broadcast("cmd", 777) }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) From 0587a2ab6bac35081743ea0d1abc007635030423 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 14 Jul 2018 20:30:24 -0400 Subject: [PATCH 34/77] fixed a bug within timesync algo. --- buzz_scripts/include/timesync.bzz | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index f190457..fecf9d9 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -6,7 +6,7 @@ logical_time = 0 TIME_JUMP_THR = 5 TIME_TO_FORGET = 20 TIME_TO_SYNC = 200 -COM_DELAY = 3 +COM_DELAY = 2 # table to store neighbor time data time_nei_table = {} # Algo. global parameters @@ -44,6 +44,7 @@ function init_time_sync(){ function step_time_sync(){ logical_time = logical_time + 1 sync_timer = sync_timer + 1 + log("Logical time now ", logical_time) if(sync_timer < TIME_TO_SYNC){ log(" SYNC ALGO ACTIVE time:", sync_timer) cnt = 0 @@ -51,8 +52,7 @@ function step_time_sync(){ if(size(time_nei_table) > 0){ foreach(time_nei_table, function(key, value) { if(value.time != 0){ - #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) - var local_offset = value.time - logical_time - value.age + var local_offset = value.time - logical_time + value.age if(local_offset > 0){ avg_offset = avg_offset + 1 * local_offset cnt = cnt + 1 @@ -80,10 +80,8 @@ function step_time_sync(){ jumped = 0 syncError=0 var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } - #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) neighbors.broadcast("time_sync",mstr) } - log("Logical time now ", logical_time) } # Function to set sync timer to zero and reinitiate sync. algo From 14ceab1047e29cf9de277a6eec04f2d3f45e1180 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 22 Jul 2018 14:50:30 -0400 Subject: [PATCH 35/77] added msg size + nei id + nei pos to log --- include/buzz_utility.h | 2 ++ src/buzz_utility.cpp | 4 ++++ src/roscontroller.cpp | 22 +++++++++++++++++++++- 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 1c3c506..4eda0fb 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -78,6 +78,8 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); +std::vector get_inmsg_vector(); + std::string getuavstate(); int getlogicaltime(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c3986d0..2cfcfd6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -565,6 +565,10 @@ int get_inmsg_size() return IN_MSG.size(); } +std::vector get_inmsg_vector(){ + return IN_MSG; +} + string getuavstate() /* / return current BVM state diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e4f7f12..6b48835 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -170,7 +170,27 @@ void roscontroller::RosControllerRun() log<::iterator it = + neighbours_pos_map.begin(); + log << neighbours_pos_map.size()<< ","; + for (; it != neighbours_pos_map.end(); ++it) + { + log << (double)it->second.x << "," << (double)it->second.y + << "," << (double)it->second.z << ","; + } + std::vector in_msg = buzz_utility::get_inmsg_vector(); + log <<(int)in_msg.size()<<","; + for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ + uint8_t* first_INmsg = (uint8_t*)(*it); + size_t tot = 0; + // Size is at first 2 bytes + uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t); + tot += sizeof(uint16_t); + // Decode neighbor Id + uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); + log<<(int)neigh_id<<","<<(int)msg_size<<","; + } + log < Date: Sun, 22 Jul 2018 15:10:13 -0400 Subject: [PATCH 36/77] fixed moveto command --- src/buzzuav_closures.cpp | 4 +- src/roscontroller.cpp | 158 +++++++++++++++++++-------------------- 2 files changed, 81 insertions(+), 81 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d0a9ea6..1d72cbf 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -228,8 +228,8 @@ int buzzuav_moveto(buzzvm_t vm) goto_pos[2] = height + dh; goto_pos[3] = dyaw; // DEBUG - // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], - // goto_pos[1], goto_pos[2]); + ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], + goto_pos[1], goto_pos[2]); buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6b48835..c56ac72 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -706,88 +706,88 @@ script armstate = 0; break; - // case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! - // cmd_srv.request.param5 = home.latitude; - // cmd_srv.request.param6 = home.longitude; - // cmd_srv.request.param7 = home.altitude; - // cmd_srv.request.command = buzzuav_closures::getcmd(); - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // break; + case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + cmd_srv.request.param5 = home.latitude; + cmd_srv.request.param6 = home.longitude; + cmd_srv.request.param7 = home.altitude; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - // case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! - // goto_pos = buzzuav_closures::getgoto(); - // cmd_srv.request.param5 = goto_pos[0]; - // cmd_srv.request.param6 = goto_pos[1]; - // cmd_srv.request.param7 = goto_pos[2]; - // cmd_srv.request.command = buzzuav_closures::getcmd(); - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // break; + case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! + goto_pos = buzzuav_closures::getgoto(); + cmd_srv.request.param5 = goto_pos[0]; + cmd_srv.request.param6 = goto_pos[1]; + cmd_srv.request.param7 = goto_pos[2]; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - // case buzzuav_closures::COMMAND_ARM: - // if (!armstate) - // { - // SetMode("LOITER", 0); - // armstate = 1; - // Arm(); - // } - // break; + case buzzuav_closures::COMMAND_ARM: + if (!armstate) + { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + } + break; - // case buzzuav_closures::COMMAND_DISARM: - // if (armstate) - // { - // armstate = 0; - // SetMode("LOITER", 0); - // Arm(); - // } - // break; + case buzzuav_closures::COMMAND_DISARM: + if (armstate) + { + armstate = 0; + SetMode("LOITER", 0); + Arm(); + } + break; - // case buzzuav_closures::COMMAND_MOVETO: - // goto_pos = buzzuav_closures::getgoto(); - // roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); - // break; + case buzzuav_closures::COMMAND_MOVETO: + goto_pos = buzzuav_closures::getgoto(); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); + break; - // case buzzuav_closures::COMMAND_GIMBAL: - // gimbal = buzzuav_closures::getgimbal(); - // cmd_srv.request.param1 = gimbal[0]; - // cmd_srv.request.param2 = gimbal[1]; - // cmd_srv.request.param3 = gimbal[2]; - // cmd_srv.request.param4 = gimbal[3]; - // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // break; + case buzzuav_closures::COMMAND_GIMBAL: + gimbal = buzzuav_closures::getgimbal(); + cmd_srv.request.param1 = gimbal[0]; + cmd_srv.request.param2 = gimbal[1]; + cmd_srv.request.param3 = gimbal[2]; + cmd_srv.request.param4 = gimbal[3]; + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - // case buzzuav_closures::COMMAND_PICTURE: - // ROS_INFO("TAKING A PICTURE HERE!! --------------"); - // mavros_msgs::CommandBool capture_command; - // if (capture_srv.call(capture_command)) - // { - // ROS_INFO("Reply: %ld", (long int)capture_command.response.success); - // } - // else - // ROS_ERROR("Failed to call service from camera streamer"); - // break; + case buzzuav_closures::COMMAND_PICTURE: + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + mavros_msgs::CommandBool capture_command; + if (capture_srv.call(capture_command)) + { + ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + } + else + ROS_ERROR("Failed to call service from camera streamer"); + break; } } @@ -998,9 +998,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.pose.orientation.w = q[3]; // To prevent drifting from stable position, uncomment - // if(fabs(x)>0.005 || fabs(y)>0.005) { - localsetpoint_nonraw_pub.publish(moveMsg); - // } + if(fabs(x)>0.005 || fabs(y)>0.005) { + localsetpoint_nonraw_pub.publish(moveMsg); + } } void roscontroller::SetMode(std::string mode, int delay_miliseconds) From 81f4aba155e05b56ef07c2dbd8260197e87d7b07 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 22 Jul 2018 15:42:10 -0400 Subject: [PATCH 37/77] changed autolaunch function to formation --- buzz_scripts/main.bzz | 3 ++- misc/cmdlinectr.sh | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 0a3bc32..e187ce4 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -8,7 +8,8 @@ include "vstigenv.bzz" include "timesync.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "CUSFUN" +AUTO_LAUNCH_STATE = "FORMATION" +#AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network LOWEST_ROBOT_ID = 97 TARGET = 9.0 diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 68b4296..c2794a5 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -34,7 +34,7 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch + rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } function uavstate { From 24db1dc4fc71ad8599a08db90a1dd5121e1ce783 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 22 Jul 2018 16:01:20 -0400 Subject: [PATCH 38/77] changed updaterobot function to hold heaven launch file --- misc/cmdlinectr.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index c2794a5..bea3e11 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -34,7 +34,8 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch + rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXHeavenbuzzy.launch +# rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXxbeebuzzy.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } function uavstate { From 2378a3881a98162044df994f0b8ffcf40be61045 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 24 Jul 2018 15:55:13 -0400 Subject: [PATCH 39/77] changed to old barrier and modified log for easy parsing --- buzz_scripts/include/act/old_barrier.bzz | 69 ++++++++++++++++++++++++ buzz_scripts/include/act/states.bzz | 6 +-- src/roscontroller.cpp | 11 ++-- 3 files changed, 78 insertions(+), 8 deletions(-) create mode 100644 buzz_scripts/include/act/old_barrier.bzz diff --git a/buzz_scripts/include/act/old_barrier.bzz b/buzz_scripts/include/act/old_barrier.bzz new file mode 100644 index 0000000..ef3c5fc --- /dev/null +++ b/buzz_scripts/include/act/old_barrier.bzz @@ -0,0 +1,69 @@ +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_TIMEOUT = 1200 # in steps +BARRIER_VSTIG = 80 +timeW = 0 +barrier = nil + +# +# Sets a barrier +# +function barrier_create() { + # reset + timeW = 0 + # create barrier vstig + #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) + if(barrier!=nil) { + barrier=nil + BARRIER_VSTIG = BARRIER_VSTIG +1 + } + #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) +} + +function barrier_set(threshold, transf, resumef,bc) { + statef = function() { + barrier_wait(threshold, transf, resumef,bc); + } + BVMSTATE = "BARRIERWAIT" + barrier_create() +} + +# +# Make yourself ready +# +function barrier_ready(bc) { + #log("BARRIER READY -------") + barrier.put(id, 1) + barrier.put("d", 0) +} + +# +# Executes the barrier +# +function barrier_wait(threshold, transf, resumef,bc) { + barrier.put(id, 1) + + barrier.get(id) + log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + barrier.put("d", 1) + timeW = 0 + BVMSTATE = transf + } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + barrier=nil + timeW = 0 + BVMSTATE = resumef + } + + timeW = timeW+1 +} + diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 51624a2..c379bc5 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -4,7 +4,7 @@ # ######################################## include "utils/vec2.bzz" -include "act/barrier.bzz" +include "act/old_barrier.bzz" include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. @@ -191,8 +191,8 @@ function rc_cmd_listen() { } else if(flight.rc_cmd==21) { flight.rc_cmd=0 AUTO_LAUNCH_STATE = "TURNEDOFF" - barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) - barrier_ready(21) + #barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + #barrier_ready(21) BVMSTATE = "STOP" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==20) { diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c56ac72..2c8265e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -170,16 +170,18 @@ void roscontroller::RosControllerRun() log< in_msg = buzz_utility::get_inmsg_vector(); + log <<(int)in_msg.size()<<","; + log <::iterator it = neighbours_pos_map.begin(); - log << neighbours_pos_map.size()<< ","; for (; it != neighbours_pos_map.end(); ++it) { log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z << ","; } - std::vector in_msg = buzz_utility::get_inmsg_vector(); - log <<(int)in_msg.size()<<","; + for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ uint8_t* first_INmsg = (uint8_t*)(*it); size_t tot = 0; @@ -190,8 +192,7 @@ void roscontroller::RosControllerRun() uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); log<<(int)neigh_id<<","<<(int)msg_size<<","; } - - log < Date: Fri, 27 Jul 2018 00:59:39 -0400 Subject: [PATCH 40/77] Moved time sync algo to ROS side and computes in ns, as an attempt to measure msg delays. --- buzz_scripts/include/act/states.bzz | 8 +- buzz_scripts/include/timesync.bzz | 86 ++----------- buzz_scripts/main.bzz | 8 +- include/buzz_utility.h | 20 ++- include/roscontroller.h | 18 ++- src/buzz_utility.cpp | 10 +- src/roscontroller.cpp | 187 +++++++++++++++++++++++++--- 7 files changed, 219 insertions(+), 118 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c379bc5..c50e140 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -217,9 +217,7 @@ function rc_cmd_listen() { stattab_send() } else if (flight.rc_cmd==777){ flight.rc_cmd=0 - if(logical_time != nil) reinit_time_sync() - barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) - barrier_ready(777) + reinit_time_sync() neighbors.broadcast("cmd", 777) }else if (flight.rc_cmd==900){ flight.rc_cmd=0 @@ -266,9 +264,7 @@ function nei_cmd_listen() { } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() } else if(value==777 and BVMSTATE=="TURNEDOFF"){ - if(logical_time != nil) reinit_time_sync() - barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) - barrier_ready(777) + reinit_time_sync() #neighbors.broadcast("cmd", 777) }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index fecf9d9..41f6561 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -1,87 +1,15 @@ -include "utils/string.bzz" - -# Time to be sync -logical_time = 0 -# sync algo. constants -TIME_JUMP_THR = 5 -TIME_TO_FORGET = 20 -TIME_TO_SYNC = 200 -COM_DELAY = 2 -# table to store neighbor time data -time_nei_table = {} -# Algo. global parameters -diffMaxLogical = 0 -jumped = 0 -syncError = 99999 +TIME_TO_SYNC = 100 sync_timer = 0 - -# Function to intialize sync algorithm -function init_time_sync(){ - neighbors.listen("time_sync", - function(vid, value, rid) { - if(value != nil){ - log(" TIME SYNC Got (", vid, ",", value.time , " , ", value.max, ") #", rid) - var msg_time = value.time - var msg_max = value.max - #log("msg: 1: ", msg_time, " 2: ", msg_max ) - if(msg_time != nil and msg_max != nil){ - diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) - var time_offset = msg_time - logical_time - if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset - if(time_offset > TIME_JUMP_THR){ - logical_time = logical_time + time_offset - diffMaxLogical = math.max(diffMaxLogical-time_offset,0) - jumped = 1 - } - time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} - } - } - } - ) -} +timesync_state = 0 # Function to sync. algo -function step_time_sync(){ - logical_time = logical_time + 1 +function check_time_sync(){ sync_timer = sync_timer + 1 - log("Logical time now ", logical_time) - if(sync_timer < TIME_TO_SYNC){ - log(" SYNC ALGO ACTIVE time:", sync_timer) - cnt = 0 - avg_offset = 0 - if(size(time_nei_table) > 0){ - foreach(time_nei_table, function(key, value) { - if(value.time != 0){ - var local_offset = value.time - logical_time + value.age - if(local_offset > 0){ - avg_offset = avg_offset + 1 * local_offset - cnt = cnt + 1 - } - else{ - if(math.abs(local_offset) TIME_TO_FORGET) - value.time = 0 - - value.age = value.age + 1 - } - }) - if(cnt > 0 and jumped != 1){ - var correction = math.ceil(avg_offset / (cnt + 1) ) - if(math.abs(correction) < TIME_JUMP_THR){ - logical_time = logical_time + correction - } - } - } - jumped = 0 - syncError=0 - var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } - neighbors.broadcast("time_sync",mstr) + if(sync_timer < TIME_TO_SYNC){ + log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) + timesync_state = 1 } + else timesync_state = 0 } # Function to set sync timer to zero and reinitiate sync. algo diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e187ce4..26786d2 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -11,7 +11,7 @@ include "timesync.bzz" AUTO_LAUNCH_STATE = "FORMATION" #AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network -LOWEST_ROBOT_ID = 97 +LOWEST_ROBOT_ID = 1 TARGET = 9.0 EPSILON = 30.0 @@ -33,8 +33,6 @@ function init() { init_stig() init_swarm() - init_time_sync() - TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -48,8 +46,8 @@ function init() { # Executed at each time step. function step() { - # step time sync algorithm - step_time_sync() + # check time sync algorithm state + check_time_sync() # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 4eda0fb..292f8b7 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -43,6 +43,24 @@ struct neiStatus }; typedef struct neiStatus neighbors_status; +struct neitime +{ + uint64_t nei_hardware_time; + uint64_t nei_logical_time; + uint64_t node_hardware_time; + uint64_t node_logical_time; + double nei_rate; + double relative_rate; + int age; + neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) + : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), + nei_rate(nr), relative_rate(mr) {}; + neitime() + { + } +}; +typedef struct neitime neighbor_time; + uint16_t* u64_cvt_u16(uint64_t u64); int buzz_listen(const char* type, int msg_size); @@ -82,5 +100,5 @@ std::vector get_inmsg_vector(); std::string getuavstate(); -int getlogicaltime(); +int get_timesync_state(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 566b1a0..f9c684b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -35,11 +35,17 @@ #include #include #include +#include #include "buzzuav_closures.h" #define UPDATER_MESSAGE_CONSTANT 987654321 -#define XBEE_MESSAGE_CONSTANT 586782343 -#define XBEE_STOP_TRANSMISSION 4355356352 +#define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 +#define BUZZ_MESSAGE_CONSTANT_TIME 523534343 +// Time sync algo. constants +#define COM_DELAY 100000000 // in nano seconds i.e 100 ms +#define TIME_SYNC_JUMP_THR 500000000 +#define MOVING_AVERAGE_ALPHA 0.1 + #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 @@ -90,8 +96,13 @@ private: uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; + std::map neighbours_time_map; int timer_step = 0; int robot_id = 0; + ros::Time logical_clock; + ros::Time previous_step_time; + double logical_time_rate; + bool time_sync_jumped; std::string robot_name = ""; int rc_cmd; @@ -268,6 +279,7 @@ private: bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); void get_xbee_status(); - + void time_sync_step(); + void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); }; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 2cfcfd6..fdc7277 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -585,19 +585,19 @@ string getuavstate() return uav_state; } -int getlogicaltime() +int get_timesync_state() /* / return current logical time --------------------------------------*/ { - int logical_time = 0; + int timesync_state = 0; if(VM){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1)); buzzvm_gload(VM); buzzobj_t obj = buzzvm_stack_at(VM, 1); - if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value; buzzvm_pop(VM); } - return logical_time; + return timesync_state; } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2c8265e..e6b1c44 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -13,7 +13,8 @@ namespace rosbzz_node const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const bool debug = true; -roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): +logical_clock(ros::Time()), previous_step_time(ros::Time()) /* / roscontroller class Constructor ---------------*/ @@ -60,6 +61,11 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } + // time sync algo. parameter intialization + previous_step_time.fromNSec(ros::Time::now().toNSec()); + logical_clock.fromNSec(ros::Time::now().toNSec()); + logical_time_rate = 0; + time_sync_jumped = false; // Create log dir and open log file std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; @@ -164,9 +170,10 @@ void roscontroller::RosControllerRun() if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data - log<first<<","; log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z << ","; } @@ -193,6 +201,9 @@ void roscontroller::RosControllerRun() log<<(int)neigh_id<<","<<(int)msg_size<<","; } log<payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT) + else if (msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME || + msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_WTO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1106,8 +1134,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; gps_rb(nei_pos, cvt_neighbours_pos_payload); + int index = 3; + if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME) index = 5; // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1118,8 +1148,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - delete[] out; - buzz_utility::in_msg_append((message_obt + 3)); + if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME){ + // update time struct for sync algo + double nr = 0; + push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } + else{ + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } } } @@ -1382,4 +1421,114 @@ void roscontroller::get_xbee_status() * TriggerAPIRssi(all_ids); */ } + +void roscontroller::time_sync_step() +/* + * Steps the time syncronization algorithm + ------------------------------------------------------------------ */ +{ + // ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); + double avgRate = logical_time_rate; + double avgOffset = 0; + int offsetCount = 0; + map::iterator it; + for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ++it) + { + avgRate += (it->second).relative_rate; + // estimate current offset + int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; + // ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); + if(offset > 0){ + // neighbor is ahead in time + avgOffset = avgOffset + (1 * offset); + offsetCount++; + } + else{ + if(std::abs(offset)second).age < BUZZRATE) (it->second).age++; + else neighbours_time_map.erase(it); + } + if(offsetCount>0 && !time_sync_jumped){ + int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); + if(std::abs(correction) < TIME_SYNC_JUMP_THR){ + // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); + // correct time with the diff within this step + differnce in nei + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + // ROS_INFO(" time sync loop : avgOffset %f, offsetCount : %d , correction %" PRId64 "time diff %" PRIu64 " old_time %" PRIu64 " rate %f", + // avgOffset, offsetCount, correction, time_diff, old_time, avgRate); + uint64_t final_time = old_time + time_diff + (uint64_t)correction;// + (time_diff * avgRate); + + // ROS_INFO(" Final time: %" PRIu64, final_time); + logical_clock.fromNSec(final_time); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + + // ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); + } + else{ + // correct time with the diff within this step + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + //ROS_INFO("inside non jump without coorection time sync : %f ", logical_clock.toSec()); + } + } + else{ + // correct time with the diff within this step + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + + //ROS_INFO("without correction time sync : %f ", logical_clock.toSec()); + } + time_sync_jumped = false; + //neighbours_time_map.clear(); + logical_time_rate = avgRate; + +} + +void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr) +/* + * pushes a time syncronization msg into its data slot + ------------------------------------------------------------------ */ +{ + map::iterator it = neighbours_time_map.find(nid); + double relativeRate =0; + if (it != neighbours_time_map.end()){ + uint64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; + uint64_t delatNei = round((nh - (it->second).nei_hardware_time ) + + (nh - (it->second).nei_hardware_time ) * nr); + double currentRate = 0; + if(deltaLocal !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; + relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + + (1- MOVING_AVERAGE_ALPHA)*currentRate; + + // ROS_INFO("deltaLocal %" PRIu64 ", delatNei %" PRIu64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, + // (it->second).relative_rate, relativeRate); + neighbours_time_map.erase(it); + } + int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); + // ROS_INFO("neigh msg push from %i "", my clock %" PRIu64 ", nei clock %" PRIu64 ", offset: %" PRId64", relative rate %f", nid, logical_clock.toNSec(), nl, offset, relativeRate); + if(offset > TIME_SYNC_JUMP_THR){ + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + logical_clock.fromNSec(old_time + time_diff + offset );// + time_diff * logical_time_rate ); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + time_sync_jumped = true; + } + buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), + logical_clock.toNSec(), nr, relativeRate); + neighbours_time_map.insert(make_pair(nid, nt)); + + // ROS_INFO("neigh time sync : %f , nei logical %i", logical_clock.toSec(), nl); +} } From 145598f18c4e087da1dae623539a5307d9277c32 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 27 Jul 2018 20:30:51 -0400 Subject: [PATCH 41/77] com delay parameter tunning for time sync algo. --- include/roscontroller.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index f9c684b..7807601 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -42,7 +42,7 @@ #define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 #define BUZZ_MESSAGE_CONSTANT_TIME 523534343 // Time sync algo. constants -#define COM_DELAY 100000000 // in nano seconds i.e 100 ms +#define COM_DELAY 33000000 // in nano seconds i.e 33 ms #define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 From 3853479d639eb6e0e6d4344e6f99a1708d8307f2 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 21:03:39 -0400 Subject: [PATCH 42/77] improved sync algo. and packet delay estimation log. --- include/buzz_utility.h | 14 ++- include/roscontroller.h | 53 +++++++- src/roscontroller.cpp | 260 ++++++++++++++++++++++------------------ 3 files changed, 206 insertions(+), 121 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 292f8b7..951207e 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -34,6 +34,18 @@ struct rb_struct }; typedef struct rb_struct RB_struct; +struct neiMsg +{ + uint8_t* msg; + uint16_t msgid; + double time_received; + double time_sent; + neiMsg(uint8_t* m, uint16_t mid, double tr, double ts) + : msg(m), msgid(mid), time_received(tr), time_sent(ts) {}; + neiMsg(); +}; +typedef struct pos_struct nei_msg_struct; + struct neiStatus { uint gps_strenght = 0; @@ -54,7 +66,7 @@ struct neitime int age; neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), - nei_rate(nr), relative_rate(mr) {}; + nei_rate(nr), relative_rate(mr),age(0) {}; neitime() { } diff --git a/include/roscontroller.h b/include/roscontroller.h index 7807601..3e5739d 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -38,9 +38,16 @@ #include #include "buzzuav_closures.h" -#define UPDATER_MESSAGE_CONSTANT 987654321 -#define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 -#define BUZZ_MESSAGE_CONSTANT_TIME 523534343 +/* +* ROSBuzz message types +*/ +typedef enum { + ROS_BUZZ_MSG_NIL = 0, // dummy msg + UPDATER_MESSAGE, // Update msg + BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info + BUZZ_MESSAGE_TIME, // Broadcast message with time info +} rosbuzz_msgtype; + // Time sync algo. constants #define COM_DELAY 33000000 // in nano seconds i.e 33 ms #define TIME_SYNC_JUMP_THR 500000000 @@ -55,6 +62,25 @@ using namespace std; namespace rosbzz_node { ++ +template +class circularBuffer { +private: + vector data; + unsigned int lastEntryPos; + int size; + +public: + circularBuffer(uint8_t s): data(s), lastEntryPos(0), size(s){}; + ~circularBuffer(){}; + void push(T d){ + data[lastEntryPos] = d; + if(lastEntryPos > size-1) lastEntryPos = 0; + else lastEntryPos++; + } + vector get_data(){ return data;}; +}; + class roscontroller { public: @@ -91,6 +117,20 @@ private: }; typedef struct POSE ros_pose; + struct MsgData + { + int msgid; + uint16_t nid; + uint16_t size; + double sent_time; + double received_time; + MsgData(int mi, uint16_t ni, uint16_t s, double st, double rt): + msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; + MsgData(){}; + }; + typedef struct MsgData msg_data; + + ros_pose target, home, cur_pos; uint64_t payload; @@ -103,6 +143,9 @@ private: ros::Time previous_step_time; double logical_time_rate; bool time_sync_jumped; + double com_delay; + std::vector inmsgdata; + double out_msg_time; std::string robot_name = ""; int rc_cmd; @@ -111,7 +154,7 @@ private: int barrier; int update; int statepub_active; - int message_number = 0; + int16_t msg_id = 0; uint8_t no_of_robots = 0; bool rcclient; bool xbeeplugged = false; @@ -281,5 +324,7 @@ private: void get_xbee_status(); void time_sync_step(); void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); + uint64_t get_logical_time(); + void set_logical_time_correction(uint64_t cor); }; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e6b1c44..6d273e2 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -63,9 +63,12 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) } // time sync algo. parameter intialization previous_step_time.fromNSec(ros::Time::now().toNSec()); - logical_clock.fromNSec(ros::Time::now().toNSec()); - logical_time_rate = 0; + if(robot_id==10)logical_clock.fromSec(101.0); + else logical_clock.fromNSec(0); + logical_time_rate = 1; time_sync_jumped = false; + com_delay = 0; + out_msg_time=0; // Create log dir and open log file std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; @@ -172,38 +175,38 @@ void roscontroller::RosControllerRun() // log data // hardware time now log< in_msg = buzz_utility::get_inmsg_vector(); - log <<(int)in_msg.size()<<","; - log < 0)log<<","; map::iterator it = neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { log<< it->first<<","; log << (double)it->second.x << "," << (double)it->second.y - << "," << (double)it->second.z << ","; + << "," << (double)it->second.z; } - - for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ - uint8_t* first_INmsg = (uint8_t*)(*it); - size_t tot = 0; - // Size is at first 2 bytes - uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t); - tot += sizeof(uint16_t); - // Decode neighbor Id - uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); - log<<(int)neigh_id<<","<<(int)msg_size<<","; + for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ + log<<","<<(int)it->nid <<","<<(int)it->msgid<<","<<(int)it->size<<","<sent_time + <<","<received_time; } + inmsgdata.clear(); log<payload64[0]; + uint8_t r8header[4]; + uint8_t mtype; + uint16_t mid; + float stime; + memcpy(r8header,&rheader, 7*sizeof(uint8_t)); + memcpy(&mtype, r8header, sizeof(uint8_t)); + memcpy(&mid, r8header+1, sizeof(uint16_t)); + memcpy(&stime, r8header+3, sizeof(float)); + + ROS_INFO("Received Header : mtype %u mid %u stime %f", mtype,mid,stime); // Check for Updater message, if updater message push it into updater FIFO - if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) + if ((int)mtype == (int)UPDATER_MESSAGE) { uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint64_t message_obt[obt_msg_size]; @@ -1105,7 +1156,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // Copy packet into temporary buffer neglecting update constant memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); uint16_t unMsgSize = *(uint16_t*)(pl); - fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); + if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize); if (unMsgSize > 0) { buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); @@ -1114,8 +1165,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) free(pl); } // BVM FIFO message - else if (msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME || - msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_WTO_TIME) + else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || + (int)mtype == (int)BUZZ_MESSAGE_WTO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1123,6 +1174,15 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { message_obt[i - 1] = (uint64_t)msg->payload64[i]; } + // determine buzz msg index based on msg type + int index = 3; + if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; + // Extract robot id of the neighbour + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); + get_logical_time(); + // store in msg data for data log + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + inmsgdata.push_back(inm); // Extract neighbours position from payload double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); @@ -1133,11 +1193,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; + // Compute RB in robot body ref. frame gps_rb(nei_pos, cvt_neighbours_pos_payload); - int index = 3; - if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME) index = 5; - // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1148,17 +1205,16 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME){ + if((int)mtype == (int)BUZZ_MESSAGE_TIME){ // update time struct for sync algo double nr = 0; push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); - delete[] out; buzz_utility::in_msg_append((message_obt + index)); } else{ - delete[] out; buzz_utility::in_msg_append((message_obt + index)); } + delete[] out; } } @@ -1427,7 +1483,7 @@ void roscontroller::time_sync_step() * Steps the time syncronization algorithm ------------------------------------------------------------------ */ { - // ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); + //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); double avgRate = logical_time_rate; double avgOffset = 0; int offsetCount = 0; @@ -1437,98 +1493,70 @@ void roscontroller::time_sync_step() avgRate += (it->second).relative_rate; // estimate current offset int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; - // ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); - if(offset > 0){ - // neighbor is ahead in time - avgOffset = avgOffset + (1 * offset); - offsetCount++; - } - else{ - if(std::abs(offset)second).nei_logical_time, (int64_t)(it->second).node_logical_time); if((it->second).age < BUZZRATE) (it->second).age++; else neighbours_time_map.erase(it); } + avgRate = avgRate/(neighbours_time_map.size()+1); if(offsetCount>0 && !time_sync_jumped){ int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); if(std::abs(correction) < TIME_SYNC_JUMP_THR){ // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); - // correct time with the diff within this step + differnce in nei + rate change - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - avgRate = avgRate/(neighbours_time_map.size()+1); - // ROS_INFO(" time sync loop : avgOffset %f, offsetCount : %d , correction %" PRId64 "time diff %" PRIu64 " old_time %" PRIu64 " rate %f", - // avgOffset, offsetCount, correction, time_diff, old_time, avgRate); - uint64_t final_time = old_time + time_diff + (uint64_t)correction;// + (time_diff * avgRate); - - // ROS_INFO(" Final time: %" PRIu64, final_time); - logical_clock.fromNSec(final_time); - previous_step_time.fromNSec(ros::Time::now().toNSec()); - - // ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); - } - else{ - // correct time with the diff within this step + rate change - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - avgRate = avgRate/(neighbours_time_map.size()+1); - logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); - previous_step_time.fromNSec(ros::Time::now().toNSec()); - //ROS_INFO("inside non jump without coorection time sync : %f ", logical_clock.toSec()); + set_logical_time_correction(correction); + ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); } } - else{ - // correct time with the diff within this step + rate change - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - avgRate = avgRate/(neighbours_time_map.size()+1); - logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); - previous_step_time.fromNSec(ros::Time::now().toNSec()); - - //ROS_INFO("without correction time sync : %f ", logical_clock.toSec()); - } + get_logical_time(); // just to update clock time_sync_jumped = false; //neighbours_time_map.clear(); logical_time_rate = avgRate; } - void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr) /* * pushes a time syncronization msg into its data slot ------------------------------------------------------------------ */ { + ROS_INFO("Received from [%i] L %"PRIu64" , H: %"PRIu64" , nei rate %f",nid, nl,nh,nr); map::iterator it = neighbours_time_map.find(nid); - double relativeRate =0; + double relativeRate =1; if (it != neighbours_time_map.end()){ - uint64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; - uint64_t delatNei = round((nh - (it->second).nei_hardware_time ) - + (nh - (it->second).nei_hardware_time ) * nr); + int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; + int64_t delatNei = round(nh - (it->second).nei_hardware_time); double currentRate = 0; - if(deltaLocal !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; + ROS_INFO("current in hand for nei : me H %"PRIu64" , nei H %"PRIu64" ", (it->second).node_hardware_time, + (it->second).nei_hardware_time ); + if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + (1- MOVING_AVERAGE_ALPHA)*currentRate; - // ROS_INFO("deltaLocal %" PRIu64 ", delatNei %" PRIu64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, - // (it->second).relative_rate, relativeRate); + ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", + deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); neighbours_time_map.erase(it); } int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); - // ROS_INFO("neigh msg push from %i "", my clock %" PRIu64 ", nei clock %" PRIu64 ", offset: %" PRId64", relative rate %f", nid, logical_clock.toNSec(), nl, offset, relativeRate); if(offset > TIME_SYNC_JUMP_THR){ - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - logical_clock.fromNSec(old_time + time_diff + offset );// + time_diff * logical_time_rate ); - previous_step_time.fromNSec(ros::Time::now().toNSec()); + set_logical_time_correction(offset);// + time_diff * logical_time_rate ); time_sync_jumped = true; + ROS_WARN("CLock Jumped %"PRIu64, logical_clock.toNSec()); } buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); neighbours_time_map.insert(make_pair(nid, nt)); - // ROS_INFO("neigh time sync : %f , nei logical %i", logical_clock.toSec(), nl); } + + uint64_t roscontroller::get_logical_time(){ + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + logical_clock.fromNSec(old_time + time_diff);// + time_diff * logical_time_rate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + return logical_clock.toNSec(); + } + void roscontroller::set_logical_time_correction(uint64_t cor){ + uint64_t l_time_now = get_logical_time(); + logical_clock.fromNSec(l_time_now + cor); + } } From 28a31e42a1208a10e84787ace172a71b441f6e25 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 21:18:10 -0400 Subject: [PATCH 43/77] typo fix --- include/roscontroller.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 3e5739d..1ab67af 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -62,7 +62,7 @@ using namespace std; namespace rosbzz_node { -+ + template class circularBuffer { private: From ef17789c5a3145727e07d7767900a9c038ce5009 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 22:08:52 -0400 Subject: [PATCH 44/77] stack buffer overflow bug fix --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6d273e2..393eed9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -616,7 +616,7 @@ with size......... | / uint16_t mid = (uint16_t)msg_id; get_logical_time(); float stime = (float)logical_clock.toSec(); - uint8_t r8header[4]; + uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); From 3602b4c2d6667ef234447dd1bb4070b8c3843ae5 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 22:24:44 -0400 Subject: [PATCH 45/77] stack fix --- src/roscontroller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 393eed9..4d3e0b7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -642,7 +642,7 @@ with size......... | / uint16_t mid = (uint16_t)msg_id; get_logical_time(); float stime = (float)logical_clock.toSec(); - uint8_t r8header[4]; + uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); @@ -703,7 +703,7 @@ with size......... | / uint16_t mid = (uint16_t)msg_id; get_logical_time(); float stime = (float)logical_clock.toSec(); - uint8_t r8header[4]; + uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); @@ -1130,7 +1130,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { // decode msg header uint64_t rheader = msg->payload64[0]; - uint8_t r8header[4]; + uint8_t r8header[8]; uint8_t mtype; uint16_t mid; float stime; From 323004feceaa7db0ec3a9c87a0943dcabf8c881e Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 22:52:33 -0400 Subject: [PATCH 46/77] removed sync algo. debug prints --- src/roscontroller.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 4d3e0b7..c0e0cac 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -622,7 +622,6 @@ with size......... | / memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(float)); memcpy(&rheader, r8header, 7*sizeof(uint8_t)); - ROS_INFO("SENT Header : mtype %u mid %u stime %f", mtype,mid,stime); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -632,9 +631,6 @@ with size......... | / // add time sync algo data payload_out.payload64.push_back(ros::Time::now().toNSec()); payload_out.payload64.push_back(logical_clock.toNSec()); - // ROS_INFO("[%i] SENT_MSG NO : %i",robot_id, msg_id); - ROS_INFO("SENT L CLock : %"PRIu64" , H clock : %"PRIu64" , Rate %f", - ros::Time::now().toNSec(),logical_clock.toNSec(), logical_time_rate ); } else{ // prepare rosbuzz msg header @@ -648,7 +644,6 @@ with size......... | / memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(float)); memcpy(&rheader, r8header, 7*sizeof(uint8_t)); - ROS_INFO("SENT Header : mtype %u mid %u stime %f", mtype,mid,stime); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -1139,7 +1134,6 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) memcpy(&mid, r8header+1, sizeof(uint16_t)); memcpy(&stime, r8header+3, sizeof(float)); - ROS_INFO("Received Header : mtype %u mid %u stime %f", mtype,mid,stime); // Check for Updater message, if updater message push it into updater FIFO if ((int)mtype == (int)UPDATER_MESSAGE) { @@ -1495,7 +1489,6 @@ void roscontroller::time_sync_step() int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; avgOffset = avgOffset + offset; offsetCount++; - ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); if((it->second).age < BUZZRATE) (it->second).age++; else neighbours_time_map.erase(it); } @@ -1503,9 +1496,7 @@ void roscontroller::time_sync_step() if(offsetCount>0 && !time_sync_jumped){ int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); if(std::abs(correction) < TIME_SYNC_JUMP_THR){ - // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); set_logical_time_correction(correction); - ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); } } get_logical_time(); // just to update clock @@ -1519,20 +1510,17 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou * pushes a time syncronization msg into its data slot ------------------------------------------------------------------ */ { - ROS_INFO("Received from [%i] L %"PRIu64" , H: %"PRIu64" , nei rate %f",nid, nl,nh,nr); map::iterator it = neighbours_time_map.find(nid); double relativeRate =1; if (it != neighbours_time_map.end()){ int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; int64_t delatNei = round(nh - (it->second).nei_hardware_time); double currentRate = 0; - ROS_INFO("current in hand for nei : me H %"PRIu64" , nei H %"PRIu64" ", (it->second).node_hardware_time, - (it->second).nei_hardware_time ); if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + (1- MOVING_AVERAGE_ALPHA)*currentRate; - ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", + ROS_INFO("SYNC MSG RECEIVED deltaLocal %"PRIu64", delatNei %"PRId64" , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); neighbours_time_map.erase(it); } @@ -1540,7 +1528,6 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou if(offset > TIME_SYNC_JUMP_THR){ set_logical_time_correction(offset);// + time_diff * logical_time_rate ); time_sync_jumped = true; - ROS_WARN("CLock Jumped %"PRIu64, logical_clock.toNSec()); } buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); From efb43f0543868b4b08e8e2de8a2b6d26df8e6b85 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 23:55:38 -0400 Subject: [PATCH 47/77] endieness floting point possible fix --- src/roscontroller.cpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c0e0cac..3d69680 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -615,13 +615,14 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - float stime = (float)logical_clock.toSec(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; + ROS_INFO("Sent stime %f for %d",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); - memcpy(r8header+3, &stime, sizeof(float)); - memcpy(&rheader, r8header, 7*sizeof(uint8_t)); + memcpy(r8header+3, &stime, sizeof(uint32_t)); + memcpy(&rheader, r8header, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -637,13 +638,14 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - float stime = (float)logical_clock.toSec(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; + ROS_INFO("Sent stime %f for %d",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); - memcpy(r8header+3, &stime, sizeof(float)); - memcpy(&rheader, r8header, 7*sizeof(uint8_t)); + memcpy(r8header+3, &stime, sizeof(uint32_t)); + memcpy(&rheader, r8header, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -697,13 +699,13 @@ with size......... | / uint8_t mtype = (uint8_t)UPDATER_MESSAGE; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - float stime = (float)logical_clock.toSec(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); - memcpy(r8header+3, &stime, sizeof(float)); - memcpy(&rheader, r8header, 7*sizeof(uint8_t)); + memcpy(r8header+3, &stime, sizeof(uint32_t)); + memcpy(&rheader, r8header, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader); for (int i = 0; i < total_size; i++) @@ -1128,12 +1130,13 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint8_t r8header[8]; uint8_t mtype; uint16_t mid; - float stime; - memcpy(r8header,&rheader, 7*sizeof(uint8_t)); + uint32_t temptime; + memcpy(r8header,&rheader, sizeof(uint64_t)); memcpy(&mtype, r8header, sizeof(uint8_t)); memcpy(&mid, r8header+1, sizeof(uint16_t)); - memcpy(&stime, r8header+3, sizeof(float)); - + memcpy(&temptime, r8header+3, sizeof(uint32_t)); + float stime = (float)temptime/(float)100000; + ROS_INFO("received stime %f for %u",stime, mid); // Check for Updater message, if updater message push it into updater FIFO if ((int)mtype == (int)UPDATER_MESSAGE) { From c84a2110d41a4e6ec197184a736d3b3dc32dc298 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Sun, 29 Jul 2018 04:08:25 +0000 Subject: [PATCH 48/77] Debug flotting point value serialization --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3d69680..9e782b3 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -615,8 +615,8 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); + uint32_t stime = (uint32_t) (logical_clock.toSec() * 100000); + ROS_INFO("Sent stime %u for %u",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); @@ -638,8 +638,8 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); + uint32_t stime = (uint32_t) (logical_clock.toSec() * 100000); + ROS_INFO("Sent stime %u for %u",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); From 522e07f5a3d5ece04edd00b346d5ced051a55ee1 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jul 2018 00:21:35 -0400 Subject: [PATCH 49/77] debug serialization --- src/roscontroller.cpp | 32 +++++++++++++------------------- 1 file changed, 13 insertions(+), 19 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3d69680..6e10840 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -610,15 +610,17 @@ with size......... | / msg_id++; payload_out.sysid = (uint8_t)robot_id; payload_out.msgid = (uint32_t)msg_id; + uint8_t mtype; + uint16_t mid = (uint16_t)msg_id; + get_logical_time(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; + ROS_INFO("Sent stime %u for %u",stime,mid); + uint8_t r8header[8]; + uint64_t rheader = 0; + if(buzz_utility::get_timesync_state()){ // prepare rosbuzz msg header - uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME; - uint16_t mid = (uint16_t)msg_id; - get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); - uint8_t r8header[8]; - uint64_t rheader = 0; + mtype = (uint8_t)BUZZ_MESSAGE_TIME; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(uint32_t)); @@ -635,13 +637,7 @@ with size......... | / } else{ // prepare rosbuzz msg header - uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; - uint16_t mid = (uint16_t)msg_id; - get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); - uint8_t r8header[8]; - uint64_t rheader = 0; + mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(uint32_t)); @@ -696,12 +692,10 @@ with size......... | / memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); free(buff_send); // prepare rosbuzz msg header - uint8_t mtype = (uint8_t)UPDATER_MESSAGE; - uint16_t mid = (uint16_t)msg_id; + mtype = (uint8_t)UPDATER_MESSAGE; + mid = (uint16_t)msg_id; get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - uint8_t r8header[8]; - uint64_t rheader = 0; + stime = (uint32_t)logical_clock.toSec() * 100000; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(uint32_t)); From f86da0d2e197da259a468a78e18d0e3fce2dc529 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jul 2018 18:28:08 -0400 Subject: [PATCH 50/77] fixed the issues with serialization and time sync algo. --- buzz_scripts/include/timesync.bzz | 13 ++- include/buzz_utility.h | 16 +--- include/roscontroller.h | 29 +----- src/buzz_utility.cpp | 18 +++- src/roscontroller.cpp | 151 ++++++++++++++---------------- 5 files changed, 107 insertions(+), 120 deletions(-) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index 41f6561..8843795 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -1,5 +1,7 @@ TIME_TO_SYNC = 100 sync_timer = 0 +timesync_old_state = 0 +timesync_itr = 0 timesync_state = 0 # Function to sync. algo @@ -9,11 +11,18 @@ function check_time_sync(){ log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) timesync_state = 1 } - else timesync_state = 0 + else{ + timesync_state = 0 + } + if(timesync_old_state == 0 and timesync_state == 1){ + timesync_itr = timesync_itr + 1 + timesync_old_state = 0 + } + timesync_old_state = timesync_state } # Function to set sync timer to zero and reinitiate sync. algo function reinit_time_sync(){ sync_timer = 0 -} \ No newline at end of file +} diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 951207e..047b575 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -34,18 +34,6 @@ struct rb_struct }; typedef struct rb_struct RB_struct; -struct neiMsg -{ - uint8_t* msg; - uint16_t msgid; - double time_received; - double time_sent; - neiMsg(uint8_t* m, uint16_t mid, double tr, double ts) - : msg(m), msgid(mid), time_received(tr), time_sent(ts) {}; - neiMsg(); -}; -typedef struct pos_struct nei_msg_struct; - struct neiStatus { uint gps_strenght = 0; @@ -66,7 +54,7 @@ struct neitime int age; neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), - nei_rate(nr), relative_rate(mr),age(0) {}; + nei_rate(nr), relative_rate(mr) {}; neitime() { } @@ -113,4 +101,6 @@ std::vector get_inmsg_vector(); std::string getuavstate(); int get_timesync_state(); + +int get_timesync_itr(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 1ab67af..9713a9d 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -49,7 +49,7 @@ typedef enum { } rosbuzz_msgtype; // Time sync algo. constants -#define COM_DELAY 33000000 // in nano seconds i.e 33 ms +#define COM_DELAY 100000000 // in nano seconds i.e 100 ms #define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 @@ -62,25 +62,6 @@ using namespace std; namespace rosbzz_node { - -template -class circularBuffer { -private: - vector data; - unsigned int lastEntryPos; - int size; - -public: - circularBuffer(uint8_t s): data(s), lastEntryPos(0), size(s){}; - ~circularBuffer(){}; - void push(T d){ - data[lastEntryPos] = d; - if(lastEntryPos > size-1) lastEntryPos = 0; - else lastEntryPos++; - } - vector get_data(){ return data;}; -}; - class roscontroller { public: @@ -130,7 +111,6 @@ private: }; typedef struct MsgData msg_data; - ros_pose target, home, cur_pos; uint64_t payload; @@ -141,11 +121,10 @@ private: int robot_id = 0; ros::Time logical_clock; ros::Time previous_step_time; + std::vector inmsgdata; + double out_msg_time; double logical_time_rate; bool time_sync_jumped; - double com_delay; - std::vector inmsgdata; - double out_msg_time; std::string robot_name = ""; int rc_cmd; @@ -154,7 +133,7 @@ private: int barrier; int update; int statepub_active; - int16_t msg_id = 0; + int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; bool xbeeplugged = false; diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index fdc7277..ebedb52 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -587,7 +587,7 @@ string getuavstate() int get_timesync_state() /* -/ return current logical time +/ return time sync state from bzz script --------------------------------------*/ { int timesync_state = 0; @@ -600,4 +600,20 @@ int get_timesync_state() } return timesync_state; } +int get_timesync_itr() +/* +/ return time sync iteration from bzz script +--------------------------------------*/ +{ + int timesync_itr = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value; + buzzvm_pop(VM); + } + return timesync_itr; +} + } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6e10840..1e42ebe 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -63,11 +63,9 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) } // time sync algo. parameter intialization previous_step_time.fromNSec(ros::Time::now().toNSec()); - if(robot_id==10)logical_clock.fromSec(101.0); - else logical_clock.fromNSec(0); - logical_time_rate = 1; + logical_clock.fromSec(0); + logical_time_rate = 0; time_sync_jumped = false; - com_delay = 0; out_msg_time=0; // Create log dir and open log file std::string path = @@ -172,12 +170,14 @@ void roscontroller::RosControllerRun() } if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + // log data // hardware time now log< 0)log<<","; @@ -203,10 +203,10 @@ void roscontroller::RosControllerRun() } inmsgdata.clear(); log<payload64[0]; - uint8_t r8header[8]; - uint8_t mtype; - uint16_t mid; - uint32_t temptime; - memcpy(r8header,&rheader, sizeof(uint64_t)); - memcpy(&mtype, r8header, sizeof(uint8_t)); - memcpy(&mid, r8header+1, sizeof(uint16_t)); - memcpy(&temptime, r8header+3, sizeof(uint32_t)); + uint64_t rhead = msg->payload64[0]; + uint16_t r16head[4]; + memcpy(r16head,&rhead, sizeof(uint64_t)); + uint16_t mtype = r16head[0]; + uint16_t mid = r16head[1]; + uint32_t temptime=0; + memcpy(&temptime, r16head+2, sizeof(uint32_t)); float stime = (float)temptime/(float)100000; ROS_INFO("received stime %f for %u",stime, mid); + ROS_INFO("Received header : type %u mid %u time %u",r16head[0],r16head[1],r16head[2]); // Check for Updater message, if updater message push it into updater FIFO - if ((int)mtype == (int)UPDATER_MESSAGE) + if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) { uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint64_t message_obt[obt_msg_size]; @@ -1147,7 +1140,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // Copy packet into temporary buffer neglecting update constant memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); uint16_t unMsgSize = *(uint16_t*)(pl); - if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize); + fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); @@ -1165,15 +1158,6 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { message_obt[i - 1] = (uint64_t)msg->payload64[i]; } - // determine buzz msg index based on msg type - int index = 3; - if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; - // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); - get_logical_time(); - // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); - inmsgdata.push_back(inm); // Extract neighbours position from payload double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); @@ -1184,8 +1168,16 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - // Compute RB in robot body ref. frame gps_rb(nei_pos, cvt_neighbours_pos_payload); + int index = 3; + if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; + // Extract robot id of the neighbour + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); + get_logical_time(); + // store in msg data for data log + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + inmsgdata.push_back(inm); + if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1196,16 +1188,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - if((int)mtype == (int)BUZZ_MESSAGE_TIME){ + if((int)mtype == (int)BUZZ_MESSAGE_TIME){ // update time struct for sync algo double nr = 0; push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); + delete[] out; buzz_utility::in_msg_append((message_obt + index)); } else{ + delete[] out; buzz_utility::in_msg_append((message_obt + index)); } - delete[] out; } } @@ -1529,7 +1522,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); neighbours_time_map.insert(make_pair(nid, nt)); - + ROS_INFO("SYNC MSG RECEIVED deltaLocal from %i",nid); } uint64_t roscontroller::get_logical_time(){ From 92cf7c4412917271af88c8eeb34bc71ca7482e8e Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jul 2018 19:05:08 -0400 Subject: [PATCH 51/77] Adapted graph script for tests. --- .../include/taskallocate/graphformGPS.bzz | 31 +++++++++---------- buzz_scripts/main.bzz | 3 ++ 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 61e77fd..cb82810 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -10,7 +10,7 @@ include "taskallocate/graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER -ROOT_ID = 2 +ROOT_ID = 5 # # Global variables @@ -389,7 +389,7 @@ function TransitionToJoined(){ #write statues #v_tag.put(m_nLabel, m_lockstig) barrier_create() - barrier_ready() + barrier_ready(940) m_navigation.x=0.0 m_navigation.y=0.0 @@ -568,7 +568,7 @@ function set_rc_goto() { goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) print("Saving GPS goal: ",goal.latitude, goal.longitude) - uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude) + storegoal(goal.latitude, goal.longitude, pose.position.altitude) m_gotjoinedparent = 1 } } @@ -655,8 +655,7 @@ function DoJoined(){ return } } - - barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED") + barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941) BroadcastGraph() } # @@ -681,17 +680,17 @@ function DoLock() { goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() - if(loop) { - if(timeout_graph==0) { - if(graphid < 3) - graphid = graphid + 1 - else - graphid = 0 - timeout_graph = 40 - BVMSTATE="TASK_ALLOCATE" - } - timeout_graph = timeout_graph - 1 - } +# if(loop) { +# if(timeout_graph==0) { +# if(graphid < 3) +# graphid = graphid + 1 +# else +# graphid = 0 +# timeout_graph = 40 +# BVMSTATE="TASK_ALLOCATE" +# } +# timeout_graph = timeout_graph - 1 +# } } # diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 26786d2..97ecc9b 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -32,6 +32,7 @@ goal_list = { function init() { init_stig() init_swarm() + initGraph() TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -108,8 +109,10 @@ function step() { # Executed once when the robot (or the simulator) is reset. function reset() { + resetGraph() } # Executed once at the end of experiment. function destroy() { + destroyGraph() } From 0dd435e80eba3e8235d23921813afcf3ad97d2e8 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Sun, 29 Jul 2018 23:44:43 +0000 Subject: [PATCH 52/77] state change to task allocation --- buzz_scripts/main.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 97ecc9b..1249829 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -8,7 +8,7 @@ include "vstigenv.bzz" include "timesync.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "FORMATION" +AUTO_LAUNCH_STATE = "TASK_ALLOCATE" #AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network LOWEST_ROBOT_ID = 1 From 5665ac6ef2bb418a4725d2e75f6df2a1ff01b2cc Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 29 Jul 2018 19:58:19 -0400 Subject: [PATCH 53/77] fixed hook naming issue for storegoal --- buzz_scripts/include/taskallocate/graphformGPS.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index cb82810..7eccc4f 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -568,7 +568,7 @@ function set_rc_goto() { goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) print("Saving GPS goal: ",goal.latitude, goal.longitude) - storegoal(goal.latitude, goal.longitude, pose.position.altitude) + uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude) m_gotjoinedparent = 1 } } From 5d06389f5ecca779feb3ce9e34699b5c389e3203 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Mon, 30 Jul 2018 03:10:54 +0000 Subject: [PATCH 54/77] changes to pos tolarence value in navigation --- buzz_scripts/include/act/states.bzz | 9 +++++---- src/roscontroller.cpp | 7 +------ 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c50e140..065614f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,10 +10,10 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2.5 # m/steps +GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.5 # m. -GOTOANG_TOL = 0.1 # rad. +GOTODIST_TOL = 0.8 # m. +GOTOANG_TOL = 0.2 # rad. path_it = 0 graphid = 0 pic_time = 0 @@ -96,7 +96,8 @@ function goto_gps(transf) { else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else { - LimitSpeed(m_navigation, 1.0) + #LimitSpeed(m_navigation, 1.0) + log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1e42ebe..0d94cf4 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -614,7 +614,6 @@ with size......... | / tmphead[1] = (uint16_t)message_number; get_logical_time(); uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000); - ROS_INFO("Sent stime %u for %u",stime,message_number); memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); uint64_t rheader[1]; payload_out.sysid = (uint8_t)robot_id; @@ -623,7 +622,6 @@ with size......... | / if(buzz_utility::get_timesync_state()){ // prepare rosbuzz msg header tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME; - ROS_INFO("sent header : type %u mid %u time %u",tmphead[0],tmphead[1],tmphead[2]); memcpy(rheader, tmphead, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader[0]); @@ -641,7 +639,6 @@ with size......... | / else{ // prepare rosbuzz msg header tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; - ROS_INFO("sent header : type %u mid %u time %u",tmphead[0],tmphead[1],tmphead[2]); memcpy(rheader, tmphead, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader[0]); @@ -1122,8 +1119,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint32_t temptime=0; memcpy(&temptime, r16head+2, sizeof(uint32_t)); float stime = (float)temptime/(float)100000; - ROS_INFO("received stime %f for %u",stime, mid); - ROS_INFO("Received header : type %u mid %u time %u",r16head[0],r16head[1],r16head[2]); + if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid); // Check for Updater message, if updater message push it into updater FIFO if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) { @@ -1522,7 +1518,6 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); neighbours_time_map.insert(make_pair(nid, nt)); - ROS_INFO("SYNC MSG RECEIVED deltaLocal from %i",nid); } uint64_t roscontroller::get_logical_time(){ From c5f0a3ddd40f70d2fed94919561bf3f85730684e Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Mon, 30 Jul 2018 03:39:49 +0000 Subject: [PATCH 55/77] time sync out of bound fix --- src/roscontroller.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0d94cf4..ae70aee 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1477,11 +1477,12 @@ void roscontroller::time_sync_step() offsetCount++; if((it->second).age < BUZZRATE) (it->second).age++; else neighbours_time_map.erase(it); + ROS_INFO("Size of nei time %i",neighbours_time_map.size()); } avgRate = avgRate/(neighbours_time_map.size()+1); if(offsetCount>0 && !time_sync_jumped){ int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); - if(std::abs(correction) < TIME_SYNC_JUMP_THR){ + if(correction < TIME_SYNC_JUMP_THR && correction > 0 ){ set_logical_time_correction(correction); } } @@ -1506,7 +1507,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + (1- MOVING_AVERAGE_ALPHA)*currentRate; - ROS_INFO("SYNC MSG RECEIVED deltaLocal %"PRIu64", delatNei %"PRId64" , currentrate %f , this relative rate %f, final relativeRate %f", + ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); neighbours_time_map.erase(it); } @@ -1517,6 +1518,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou } buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); + nt.age=0; neighbours_time_map.insert(make_pair(nid, nt)); } From 00fdc56642dab32fee34a98d579c9c5e26b28f35 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 00:09:29 -0400 Subject: [PATCH 56/77] iterator of neighbours time map bug fix --- src/roscontroller.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ae70aee..8a19bfa 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1468,15 +1468,20 @@ void roscontroller::time_sync_step() double avgOffset = 0; int offsetCount = 0; map::iterator it; - for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ++it) + for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ) { avgRate += (it->second).relative_rate; // estimate current offset int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; avgOffset = avgOffset + offset; offsetCount++; - if((it->second).age < BUZZRATE) (it->second).age++; - else neighbours_time_map.erase(it); + if((it->second).age > BUZZRATE){ + neighbours_time_map.erase(it); + } + else{ + (it->second).age++; + ++it; + } ROS_INFO("Size of nei time %i",neighbours_time_map.size()); } avgRate = avgRate/(neighbours_time_map.size()+1); From 5195fbf3293d3f274e118e20750db40ae3faae87 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 00:24:24 -0400 Subject: [PATCH 57/77] iterator post increment fail fix --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 8a19bfa..ba440e6 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1476,7 +1476,7 @@ void roscontroller::time_sync_step() avgOffset = avgOffset + offset; offsetCount++; if((it->second).age > BUZZRATE){ - neighbours_time_map.erase(it); + neighbours_time_map.erase(it++); } else{ (it->second).age++; From 79fea108a2f6e4069cb41d9472bc029c9511a246 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 00:54:56 -0400 Subject: [PATCH 58/77] nav parameter adaptation --- buzz_scripts/include/act/states.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 065614f..9fa5df6 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -12,7 +12,7 @@ BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.8 # m. +GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.2 # rad. path_it = 0 graphid = 0 @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x/2.0, m_navigation.y/2.0, rc_goto.altitude - pose.position.altitude, 0.0) } } From fbc5bd8e72a70c500e493a66b4f66b829faa4889 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:07:50 -0400 Subject: [PATCH 59/77] disabled movement at lock state --- buzz_scripts/include/taskallocate/graphformGPS.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 7eccc4f..5f387e4 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -677,7 +677,7 @@ function DoLock() { m_navigation=motion_vector() } # #move - goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) +# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() # if(loop) { From 7048c28142c28ca7610a0bfc09f9d9434a916c39 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:23:05 -0400 Subject: [PATCH 60/77] nav param removed scaling --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9fa5df6..5e1db50 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x/2.0, m_navigation.y/2.0, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } From a2a19d558363620943d3682bc17605e6bb8df821 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:32:05 -0400 Subject: [PATCH 61/77] nav param tunning to avoid ossilations --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 5e1db50..f486291 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x/5, m_navigation.y/5, rc_goto.altitude - pose.position.altitude, 0.0) } } From 861305bfcd70790afad51153deb5692a3a0f97be Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:40:13 -0400 Subject: [PATCH 62/77] nav parameter --- buzz_scripts/include/act/states.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f486291..9e8fd78 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -12,7 +12,7 @@ BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 1.0 # m. +GOTODIST_TOL = 2.0 # m. GOTOANG_TOL = 0.2 # rad. path_it = 0 graphid = 0 @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x/5, m_navigation.y/5, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } From 25968cfe6d6f6c2f3cbf78d24d119d80d81e1290 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Tue, 31 Jul 2018 09:25:02 +0000 Subject: [PATCH 63/77] Changes on speed limit, navigation and local pos control. --- buzz_scripts/include/act/states.bzz | 8 +++++--- buzz_scripts/include/taskallocate/graphformGPS.bzz | 2 +- buzz_scripts/include/utils/conversions.bzz | 8 ++++---- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9e8fd78..2ec6c4f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 0.5 # m/steps +GOTO_MAXVEL = 5.0 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 2.0 # m. GOTOANG_TOL = 0.2 # rad. @@ -89,6 +89,7 @@ function take_picture() { } function goto_gps(transf) { + log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude) m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) @@ -96,8 +97,9 @@ function goto_gps(transf) { else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else { - #LimitSpeed(m_navigation, 1.0) - log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) + log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y ) + m_navigation = LimitSpeed(m_navigation, 1.0) + log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y ) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 5f387e4..11d2984 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -537,7 +537,7 @@ function DoJoining(){ if(m_gotjoinedparent!=1) set_rc_goto() - else + else goto_gps(TransitionToJoined) #pack the communication package m_selfMessage.State=s2i(BVMSTATE) diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 50a58bc..92dc4e1 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) { } ned_x = d_lat/180*math.pi * 6371000.0; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); - #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); - #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) + m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing) } function gps_from_vec(vec) { @@ -66,4 +66,4 @@ function gps_from_vec(vec) { #goal.longitude = d_lon + pose.position.longitude; return Lgoal -} \ No newline at end of file +} From 79857a3f5582e5854ef3a48abf70772086f010a3 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Tue, 31 Jul 2018 18:32:58 +0000 Subject: [PATCH 64/77] lowered the velocity and target threshold. increased altitude --- buzz_scripts/include/act/states.bzz | 6 +++--- buzz_scripts/main.bzz | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 2ec6c4f..a4746af 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,10 +10,10 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 5.0 # m/steps +GOTO_MAXVEL = 2.0 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 2.0 # m. -GOTOANG_TOL = 0.2 # rad. +GOTODIST_TOL = 1.0 # m. +GOTOANG_TOL = 0.1 # rad. path_it = 0 graphid = 0 pic_time = 0 diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1249829..e0340d9 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -34,7 +34,7 @@ function init() { init_swarm() initGraph() - TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m loop = 1 # start the swarm command listener From bf4f23bc48b8fe0e7affcfba6d4f8746ecf9b4cd Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Mon, 6 Aug 2018 14:26:17 -0400 Subject: [PATCH 65/77] data logging fixes. --- buzz_scripts/include/act/states.bzz | 2 +- include/roscontroller.h | 6 +++--- src/roscontroller.cpp | 14 +++++++------- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index a4746af..c708df9 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2.0 # m/steps +GOTO_MAXVEL = 0.2 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. diff --git a/include/roscontroller.h b/include/roscontroller.h index 9713a9d..f1604f0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -104,8 +104,8 @@ private: uint16_t nid; uint16_t size; double sent_time; - double received_time; - MsgData(int mi, uint16_t ni, uint16_t s, double st, double rt): + uint64_t received_time; + MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; MsgData(){}; }; @@ -122,7 +122,7 @@ private: ros::Time logical_clock; ros::Time previous_step_time; std::vector inmsgdata; - double out_msg_time; + uint64_t out_msg_time; double logical_time_rate; bool time_sync_jumped; std::string robot_name = ""; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ba440e6..2441720 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -176,25 +176,25 @@ void roscontroller::RosControllerRun() log< 0)log<<","; + // if(neighbours_pos_map.size() > 0)log<<","; map::iterator it = neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { log<< it->first<<","; - log << (double)it->second.x << "," << (double)it->second.y + log <<","<< (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ @@ -654,7 +654,7 @@ with size......... | / } //Store out msg time get_logical_time(); - out_msg_time = logical_clock.toSec(); + out_msg_time = logical_clock.toNSec(); // publish prepared messages in respective topic payload_pub.publish(payload_out); delete[] out; @@ -1171,7 +1171,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); get_logical_time(); // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toNSec()); inmsgdata.push_back(inm); if (debug) From 670d70601b1a59ef67ecf54a98032370136f5531 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 6 Aug 2018 15:00:38 -0400 Subject: [PATCH 66/77] data logging fix --- src/roscontroller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2441720..6e85238 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -193,8 +193,8 @@ void roscontroller::RosControllerRun() neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { - log<< it->first<<","; - log <<","<< (double)it->second.x << "," << (double)it->second.y + log<<","<< it->first<<","; + log<< (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ From d05dd25bf64aab628adb7a95d056dcc9b06867b6 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 6 Aug 2018 15:16:47 -0400 Subject: [PATCH 67/77] data logging fixes --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6e85238..3f704de 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -173,7 +173,7 @@ void roscontroller::RosControllerRun() // log data // hardware time now - log< Date: Wed, 8 Aug 2018 01:09:13 +0000 Subject: [PATCH 68/77] velocity change --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c708df9..7fe503d 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 0.2 # m/steps +GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. From f58373e2b5c721d524852f090e606b5a6917bc0a Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Wed, 8 Aug 2018 01:54:48 +0000 Subject: [PATCH 69/77] diabled barrier for stop state --- buzz_scripts/include/act/states.bzz | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 7fe503d..f0d2af2 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -67,12 +67,14 @@ function stop() { neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) - barrier_ready(21) + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) } } else { - barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) - barrier_ready(21) + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) } } From f2c662dfa9b0f7fa218fd9d510b695330574d0de Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Tue, 14 Aug 2018 16:27:21 -0400 Subject: [PATCH 70/77] changed logging time to rostime, to test NTP. --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3f704de..f809a6d 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -613,7 +613,7 @@ with size......... | / uint16_t tmphead[4]; tmphead[1] = (uint16_t)message_number; get_logical_time(); - uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000); + uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000); memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); uint64_t rheader[1]; payload_out.sysid = (uint8_t)robot_id; @@ -654,7 +654,7 @@ with size......... | / } //Store out msg time get_logical_time(); - out_msg_time = logical_clock.toNSec(); + out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec(); // publish prepared messages in respective topic payload_pub.publish(payload_out); delete[] out; @@ -1169,9 +1169,9 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; // Extract robot id of the neighbour uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); - get_logical_time(); + //get_logical_time(); // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toNSec()); + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,ros::Time::now().toNSec()); inmsgdata.push_back(inm); if (debug) From b129d26a029580b4c59d0e8cdb7121e51fc9e40f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 16 Aug 2018 15:28:52 -0400 Subject: [PATCH 71/77] made NED pos available in buzz --- include/buzzuav_closures.h | 2 ++ src/buzzuav_closures.cpp | 18 ++++++++++++++++++ src/roscontroller.cpp | 1 + 3 files changed, 21 insertions(+) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index dbd3110..f43da11 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -89,6 +89,8 @@ void set_filtered_packet_loss(float value); /* * sets current position */ +void set_currentNEDpos(double x, double y); + void set_currentpos(double latitude, double longitude, float altitude, float yaw); /* * returns the current go to position diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 1d72cbf..e011c61 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -19,6 +19,7 @@ static float rc_gimbal[4]; static float batt[3]; static float obst[5] = { 0, 0, 0, 0, 0 }; static double cur_pos[4]; +static double cur_NEDpos[2]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd = 0; @@ -629,6 +630,15 @@ int buzzuav_update_xbee_status(buzzvm_t vm) return vm->state; } +void set_currentNEDpos(double x, double y) +/* +/ update interface position array +-----------------------------------*/ +{ + cur_NEDpos[0] = x; + cur_NEDpos[1] = y; +} + void set_currentpos(double latitude, double longitude, float altitude, float yaw) /* / update interface position array @@ -697,6 +707,14 @@ int buzzuav_update_currentpos(buzzvm_t vm) buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0)); buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 0)); + buzzvm_pushf(vm, cur_NEDpos[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 0)); + buzzvm_pushf(vm, cur_NEDpos[1]); + buzzvm_tput(vm); // Store read table in the proximity table buzzvm_push(vm, tPoseTable); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index f809a6d..0d87452 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -987,6 +987,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt { cur_pos.x = msg->pose.position.x; cur_pos.y = msg->pose.position.y; + set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead tf::Quaternion q( msg->pose.orientation.x, From 2d6d5d0d285213007a2a47ed0bb1d43375f649d4 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Thu, 16 Aug 2018 16:24:04 -0400 Subject: [PATCH 72/77] namespace fix --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0d87452..d2da99a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -987,7 +987,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt { cur_pos.x = msg->pose.position.x; cur_pos.y = msg->pose.position.y; - set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); + buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead tf::Quaternion q( msg->pose.orientation.x, From ffbe4569cc4045a30c15fb9638e8c45dbcb8f1a1 Mon Sep 17 00:00:00 2001 From: mistTX1 Date: Tue, 21 Aug 2018 01:40:49 +0000 Subject: [PATCH 73/77] mavros version selection as cmake flag --- CMakeLists.txt | 4 ++-- src/buzzuav_closures.cpp | 8 ++++++++ src/roscontroller.cpp | 21 +++++++++++++++++++++ 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 672d68d..2b48f35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,9 +6,9 @@ if(UNIX) endif() if(SIM) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1") else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1") endif() ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index e011c61..1e368c5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -401,7 +401,11 @@ int buzzuav_arm(buzzvm_t vm) / Buzz closure to arm /---------------------------------------*/ { +#ifdef MAVROSKINETIC + cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; +#else cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; +#endif printf(" Buzz requested Arm \n"); buzz_cmd = COMMAND_ARM; return buzzvm_ret0(vm); @@ -412,7 +416,11 @@ int buzzuav_disarm(buzzvm_t vm) / Buzz closure to disarm /---------------------------------------*/ { +#ifdef MAVROSKINETIC + cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; +#else cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; +#endif printf(" Buzz requested Disarm \n"); buzz_cmd = COMMAND_DISARM; return buzzvm_ret0(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d2da99a..5b4df3a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -780,7 +780,11 @@ script } else ROS_ERROR("Failed to call service from flight controller"); +#ifdef MAVROSKINETIC + cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START; +#else cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; +#endif if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -818,7 +822,11 @@ script cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param4 = gimbal[3]; +#ifdef MAVROSKINETIC + cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +#else cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +#endif if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -1220,8 +1228,13 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m buzzuav_closures::rc_call(rc_cmd); res.success = true; break; +#ifdef MAVROSKINETIC + case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM: + rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; +#else case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; +#endif armstate = req.param1; if (armstate) { @@ -1249,10 +1262,18 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m buzzuav_closures::rc_call(rc_cmd); res.success = true; break; +#ifdef MAVROSKINETIC + case mavros_msgs::CommandCode::DO_MOUNT_CONTROL: +#else case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: +#endif ROS_INFO("RC_Call: Gimbal!!!! "); buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); +#ifdef MAVROSKINETIC + rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +#else rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +#endif buzzuav_closures::rc_call(rc_cmd); res.success = true; break; From fff1c3398d0a051a6413ba6e4f18458cf281b1bc Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 28 Aug 2018 16:18:29 -0400 Subject: [PATCH 74/77] disabled onboard time sync, hoping NTP would sync. --- buzz_scripts/include/timesync.bzz | 26 +++++++++++++------------- src/roscontroller.cpp | 9 +++++---- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index 8843795..8b933ee 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -6,19 +6,19 @@ timesync_state = 0 # Function to sync. algo function check_time_sync(){ - sync_timer = sync_timer + 1 - if(sync_timer < TIME_TO_SYNC){ - log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) - timesync_state = 1 - } - else{ - timesync_state = 0 - } - if(timesync_old_state == 0 and timesync_state == 1){ - timesync_itr = timesync_itr + 1 - timesync_old_state = 0 - } - timesync_old_state = timesync_state + # sync_timer = sync_timer + 1 + # if(sync_timer < TIME_TO_SYNC){ + # log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) + # timesync_state = 1 + # } + # else{ + # timesync_state = 0 + # } + # if(timesync_old_state == 0 and timesync_state == 1){ + # timesync_itr = timesync_itr + 1 + # timesync_old_state = 0 + # } + # timesync_old_state = timesync_state } # Function to set sync timer to zero and reinitiate sync. algo diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5b4df3a..220522b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -204,9 +204,9 @@ void roscontroller::RosControllerRun() inmsgdata.clear(); log<payload64[0] == (uint64_t)UPDATER_MESSAGE) { From c41bb81763584223c188d47bd56b31887263aaf1 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 30 Aug 2018 11:22:37 -0400 Subject: [PATCH 75/77] Fixed the limited buffer size bug in inmsgs and changed to y formation --- buzz_scripts/include/act/states.bzz | 2 +- include/roscontroller.h | 1 + src/roscontroller.cpp | 4 ++-- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f0d2af2..c00c78b 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -15,7 +15,7 @@ GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. path_it = 0 -graphid = 0 +graphid = 3 pic_time = 0 g_it = 0 diff --git a/include/roscontroller.h b/include/roscontroller.h index f1604f0..0ed4756 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -52,6 +52,7 @@ typedef enum { #define COM_DELAY 100000000 // in nano seconds i.e 100 ms #define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 +#define MAX_NUMBER_OF_ROBOTS 10 #define TIMEOUT 60 #define BUZZRATE 10 diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 220522b..c03a33e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -392,12 +392,12 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) Subscribe(n_c); - payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + payload_sub = n_c.subscribe(in_payload, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, this); // publishers payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); - neigh_pos_pub = n_c.advertise("neighbours_pos", 5); + neigh_pos_pub = n_c.advertise("neighbours_pos", MAX_NUMBER_OF_ROBOTS); uavstate_pub = n_c.advertise("uavstate", 5); grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); From 064760108611591426d86c679c7789b1a95cebee Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 31 Aug 2018 19:59:14 -0400 Subject: [PATCH 76/77] Custom function to test waypoints. --- buzz_scripts/include/act/states.bzz | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c00c78b..09b9eb7 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -31,6 +31,15 @@ function idle() { function cusfun(){ BVMSTATE="CUSFUN" log("cusfun: yay!!!") + LOCAL_TARGET = math.vec2.new(5 ,0 ) + local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location + if(math.vec2.length(local_set_point) > GOTODIST_TOL){ + local_set_point = LimitSpeed(local_set_point, 1.0) + goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0) + } + else{ + log("TARGET REACHED") + } } function launch() { From 8cdd5eb10546bdb0d1b5eb22b332b929c4742489 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 7 Sep 2018 00:17:30 -0400 Subject: [PATCH 77/77] force push from ros_drones_ws --- CMakeLists.txt | 12 +- buzz_scripts/include/act/states.bzz | 25 +-- .../include/taskallocate/graphformGPS.bzz | 31 ++-- buzz_scripts/include/utils/conversions.bzz | 6 +- buzz_scripts/main.bzz | 7 +- include/buzz_utility.h | 4 +- include/buzzuav_closures.h | 16 +- include/rosbuzz/mavrosCC.h | 24 +++ include/roscontroller.h | 27 ++- src/buzz_utility.cpp | 55 +++--- src/buzzuav_closures.cpp | 50 +++-- src/rosbuzz.cpp | 2 +- src/roscontroller.cpp | 171 +++++++----------- 13 files changed, 200 insertions(+), 230 deletions(-) create mode 100644 include/rosbuzz/mavrosCC.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b48f35..4d868f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,11 +5,8 @@ if(UNIX) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") endif() -if(SIM) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1") -else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1") -endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}") + ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp @@ -70,6 +67,11 @@ install(TARGETS rosbuzz_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +## Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) find_package(catkin REQUIRED COMPONENTS roslaunch) roslaunch_add_file_check(launch) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 09b9eb7..d657797 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -4,12 +4,14 @@ # ######################################## include "utils/vec2.bzz" -include "act/old_barrier.bzz" +include "act/barrier.bzz" +#include "act/old_barrier.bzz" include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps + GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. @@ -27,6 +29,7 @@ function idle() { BVMSTATE = "IDLE" } + # Custom function for the user. function cusfun(){ BVMSTATE="CUSFUN" @@ -94,7 +97,7 @@ function take_picture() { takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture BVMSTATE="IDLE" - pic_time=0 + pic_time=0 } pic_time=pic_time+1 } @@ -108,9 +111,7 @@ function goto_gps(transf) { else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else { - log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y ) - m_navigation = LimitSpeed(m_navigation, 1.0) - log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y ) + m_navigation = LimitSpeed(m_navigation, 1.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } @@ -141,7 +142,7 @@ function aggregate() { centroid = math.vec2.scale(centroid, 1.0 / neighbors.count()) cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS)) cmdbin = LimitSpeed(cmdbin, 1.0/2.0) - goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # circle all together around centroid @@ -163,7 +164,7 @@ function pursuit() { dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) vfg = LimitSpeed(vfg, 2.0) - goto_abs(vfg.x, vfg.y, 0.0, 0.0) + goto_abs(vfg.x, vfg.y, 0.0, 0.0) } # Lennard-Jones interaction magnitude @@ -173,17 +174,17 @@ function lj_magnitude(dist, target, epsilon) { lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) return lj } - + # Neighbor data to LJ interaction vector function lj_vector(rid, data) { return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) } - + # Accumulator of neighbor LJ interactions function lj_sum(rid, data, accum) { return math.vec2.add(data, accum) } - + # Calculates and actuates the flocking interaction function formation() { BVMSTATE="FORMATION" @@ -200,7 +201,7 @@ function rc_cmd_listen() { if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 - BVMSTATE = "LAUNCH" + BVMSTATE = "LAUNCH" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { flight.rc_cmd=0 @@ -303,7 +304,7 @@ function nei_cmd_listen() { # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) # # if(gt.id == id) statef=goto - # }) + # }) } } }) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 11d2984..c2de02c 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -190,7 +190,7 @@ function pack_guide_msg(send_table){ else{ pon=1 } - var b=math.abs(send_table.Bearing) + var b=math.abs(send_table.Bearing) send_value=r_id*1000+pon*100+b return send_value } @@ -257,10 +257,10 @@ function LJ_vec(i){ var target=target4label(nei_id) var cDir={.x=0.0,.y=0.0} if(target!="miss"){ - cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) + cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) } #log(id,"dis=",dis,"target=",target,"label=",nei_id) - #log("x=",cDir.x,"y=",cDir.y) + #log("x=",cDir.x,"y=",cDir.y) return cDir } # @@ -272,11 +272,11 @@ function motion_vector(){ while(i get_inmsg_vector(); -std::string getuavstate(); +std::string get_bvmstate(); int get_timesync_state(); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f43da11..71b99f1 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -6,6 +6,7 @@ #include "mavros_msgs/Mavlink.h" #include "ros/ros.h" #include "buzz_utility.h" +#include "rosbuzz/mavrosCC.h" #define EARTH_RADIUS (double)6371000.0 #define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0))) @@ -13,19 +14,6 @@ namespace buzzuav_closures { -typedef enum { - COMMAND_NIL = 0, // Dummy command - COMMAND_TAKEOFF, // Take off - COMMAND_LAND, - COMMAND_GOHOME, - COMMAND_ARM, - COMMAND_DISARM, - COMMAND_GOTO, - COMMAND_MOVETO, - COMMAND_PICTURE, - COMMAND_GIMBAL, -} Custom_CommandCode; - /* * prextern int() function in Buzz * This function is used to print data from buzz @@ -89,6 +77,7 @@ void set_filtered_packet_loss(float value); /* * sets current position */ + void set_currentNEDpos(double x, double y); void set_currentpos(double latitude, double longitude, float altitude, float yaw); @@ -101,6 +90,7 @@ double* getgoto(); */ std::map> getgrid(); + /* * returns the gimbal commands */ diff --git a/include/rosbuzz/mavrosCC.h b/include/rosbuzz/mavrosCC.h new file mode 100644 index 0000000..7917bac --- /dev/null +++ b/include/rosbuzz/mavrosCC.h @@ -0,0 +1,24 @@ +#pragma once + +#if MAVROSKINETIC + +const short MISSION_START = mavros_msgs::CommandCode::MISSION_START; +const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; + +#else + +const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START; +const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM; + +#endif + +const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF; +const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND; +const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; +const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT; +const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT; +const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE; +const short CMD_REQUEST_UPDATE = 666; +const short CMD_SYNC_CLOCK = 777; diff --git a/include/roscontroller.h b/include/roscontroller.h index 0ed4756..d7bbbc5 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -35,8 +35,8 @@ #include #include #include -#include #include "buzzuav_closures.h" +#include "rosbuzz/mavrosCC.h" /* * ROSBuzz message types @@ -44,24 +44,22 @@ typedef enum { ROS_BUZZ_MSG_NIL = 0, // dummy msg UPDATER_MESSAGE, // Update msg - BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info + BUZZ_MESSAGE_NO_TIME, // Broadcast message wihout time info BUZZ_MESSAGE_TIME, // Broadcast message with time info } rosbuzz_msgtype; // Time sync algo. constants #define COM_DELAY 100000000 // in nano seconds i.e 100 ms -#define TIME_SYNC_JUMP_THR 500000000 +#define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 #define MAX_NUMBER_OF_ROBOTS 10 #define TIMEOUT 60 #define BUZZRATE 10 -#define CMD_REQUEST_UPDATE 666 -#define CMD_SYNC_CLOCK 777 using namespace std; -namespace rosbzz_node +namespace rosbuzz_node { class roscontroller { @@ -99,6 +97,8 @@ private: }; typedef struct POSE ros_pose; + ros_pose target, home, cur_pos; + struct MsgData { int msgid; @@ -106,14 +106,12 @@ private: uint16_t size; double sent_time; uint64_t received_time; - MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): + MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; MsgData(){}; }; typedef struct MsgData msg_data; - ros_pose target, home, cur_pos; - uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; @@ -122,8 +120,8 @@ private: int robot_id = 0; ros::Time logical_clock; ros::Time previous_step_time; - std::vector inmsgdata; - uint64_t out_msg_time; + std::vector inmsgdata; + uint64_t out_msg_time; double logical_time_rate; bool time_sync_jumped; std::string robot_name = ""; @@ -133,7 +131,6 @@ private: int armstate; int barrier; int update; - int statepub_active; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; @@ -141,6 +138,7 @@ private: bool multi_msg; uint8_t no_cnt = 0; uint8_t old_val = 0; + bool debug = false; std::string bzzfile_name; std::string fcclient_name; std::string armclient; @@ -164,7 +162,7 @@ private: ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; - ros::Publisher uavstate_pub; + ros::Publisher bvmstate_pub; ros::Publisher grid_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; @@ -212,7 +210,7 @@ private: void neighbours_pos_publisher(); /*UAVState publisher*/ - void uavstate_publisher(); + void state_publisher(); /*Grid publisher*/ void grid_publisher(); @@ -302,6 +300,7 @@ private: bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); void get_xbee_status(); + void time_sync_step(); void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); uint64_t get_logical_time(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index ebedb52..42b5f57 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -204,25 +204,25 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); @@ -258,25 +258,25 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); @@ -366,7 +366,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_ } // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_gstore(VM); @@ -426,7 +426,7 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t } // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_gstore(VM); @@ -539,14 +539,6 @@ int update_step_test() return a == BUZZVM_STATE_READY; } -buzzvm_t get_vm() -/* -/ return the BVM -----------------*/ -{ - return VM; -} - void set_robot_var(int ROBOTS) /* / set swarm size in the BVM @@ -569,7 +561,15 @@ std::vector get_inmsg_vector(){ return IN_MSG; } -string getuavstate() +buzzvm_t get_vm() +/* +/ return the BVM +----------------*/ +{ + return VM; +} + +string get_bvmstate() /* / return current BVM state ---------------------------------------*/ @@ -579,12 +579,19 @@ string getuavstate() buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_gload(VM); buzzobj_t obj = buzzvm_stack_at(VM, 1); - uav_state = string(obj->s.value.str); + if(obj->o.type == BUZZTYPE_STRING) + uav_state = string(obj->s.value.str); + else + uav_state = "Not Available"; buzzvm_pop(VM); } return uav_state; } +int get_swarmsize() { + return (int)buzzdict_size(VM->swarmmembers) + 1; +} + int get_timesync_state() /* / return time sync state from bzz script diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 1e368c5..4c8b2a5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -105,10 +105,10 @@ float constrainAngle(float x) / Wrap the angle between -pi, pi ----------------------------------------------------------- */ { - x = fmod(x, 2 * M_PI); + x = fmod(x + M_PI, 2 * M_PI); if (x < 0.0) x += 2 * M_PI; - return x; + return x - M_PI; } void rb_from_gps(double nei[], double out[], double cur[]) @@ -229,9 +229,9 @@ int buzzuav_moveto(buzzvm_t vm) goto_pos[2] = height + dh; goto_pos[3] = dyaw; // DEBUG - ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], - goto_pos[1], goto_pos[2]); - buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? + // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], + // goto_pos[1], goto_pos[2]); + buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? return buzzvm_ret0(vm); } @@ -332,7 +332,7 @@ int buzzuav_takepicture(buzzvm_t vm) / Buzz closure to take a picture here. /----------------------------------------*/ { - buzz_cmd = COMMAND_PICTURE; + buzz_cmd = IMAGE_START_CAPTURE; return buzzvm_ret0(vm); } @@ -356,7 +356,7 @@ int buzzuav_setgimbal(buzzvm_t vm) rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); - buzz_cmd = COMMAND_GIMBAL; + buzz_cmd = DO_MOUNT_CONTROL; return buzzvm_ret0(vm); } @@ -389,10 +389,10 @@ int buzzuav_storegoal(buzzvm_t vm) double rb[3]; rb_from_gps(goal, rb, cur_pos); - if (fabs(rb[0]) < 150.0) + if (fabs(rb[0]) < 150.0) { rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - - ROS_INFO("Set RC_GOTO ---- %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("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + } return buzzvm_ret0(vm); } @@ -401,13 +401,9 @@ int buzzuav_arm(buzzvm_t vm) / Buzz closure to arm /---------------------------------------*/ { -#ifdef MAVROSKINETIC - cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; -#else - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; -#endif + cur_cmd = COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); - buzz_cmd = COMMAND_ARM; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -416,13 +412,9 @@ int buzzuav_disarm(buzzvm_t vm) / Buzz closure to disarm /---------------------------------------*/ { -#ifdef MAVROSKINETIC - cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; -#else - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; -#endif + cur_cmd = COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Disarm \n"); - buzz_cmd = COMMAND_DISARM; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -436,9 +428,9 @@ int buzzuav_takeoff(buzzvm_t vm) buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value; height = goto_pos[2]; - cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; + cur_cmd = NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); - buzz_cmd = COMMAND_TAKEOFF; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -447,9 +439,9 @@ int buzzuav_land(buzzvm_t vm) / Buzz closure to land /-------------------------------------------------------------*/ { - cur_cmd = mavros_msgs::CommandCode::NAV_LAND; + cur_cmd = NAV_LAND; printf(" Buzz requested Land !!! \n"); - buzz_cmd = COMMAND_LAND; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -458,9 +450,9 @@ int buzzuav_gohome(buzzvm_t vm) / Buzz closure to return Home /-------------------------------------------------------------*/ { - cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + cur_cmd = NAV_RETURN_TO_LAUNCH; printf(" Buzz requested gohome !!! \n"); - buzz_cmd = COMMAND_GOHOME; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -683,7 +675,7 @@ void update_neighbors(buzzvm_t vm) } } -// Clear neighbours pos +// Clear neighbours pos void clear_neighbours_pos(){ neighbors_map.clear(); } diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index b849cbd..3816f55 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -17,7 +17,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "rosBuzz"); ros::NodeHandle nh_priv("~"); ros::NodeHandle nh; - rosbzz_node::roscontroller RosController(nh, nh_priv); + rosbuzz_node::roscontroller RosController(nh, nh_priv); RosController.RosControllerRun(); return 0; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c03a33e..4e1f2b8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -8,12 +8,11 @@ #include "roscontroller.h" #include -namespace rosbzz_node +namespace rosbuzz_node { const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; -const bool debug = true; -roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): logical_clock(ros::Time()), previous_step_time(ros::Time()) /* / roscontroller class Constructor @@ -132,23 +131,15 @@ void roscontroller::RosControllerRun() // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // check for BVMSTATE variable - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_gload(VM); - buzzobj_t obj = buzzvm_stack_at(VM, 1); - if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1; - else - { - statepub_active = 0; - ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled."); - } + if(buzz_utility::get_bvmstate()=="Not Available") + ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE."); // DEBUG // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { // Publish topics neighbours_pos_publisher(); - if(statepub_active) uavstate_publisher(); + state_publisher(); grid_publisher(); send_MPpayload(); // Check updater state and step code @@ -184,10 +175,6 @@ void roscontroller::RosControllerRun() << cur_pos.altitude * 100000 << ","; log << (int)no_of_robots<<","; log << neighbours_pos_map.size()<< ","; - log<<(int)inmsgdata.size()<<","; - log<< message_number<<","; - log<< out_msg_time<<","; - log < 0)log<<","; map::iterator it = neighbours_pos_map.begin(); @@ -202,7 +189,9 @@ void roscontroller::RosControllerRun() <<","<received_time; } inmsgdata.clear(); - log<(topic, "sensor_msgs/LaserScan")); node_handle.getParam("topics/battery", topic); - m_smTopic_infos.insert(pair(topic, "mavros_msgs/BatteryStatus")); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/BatteryState")); node_handle.getParam("topics/status", topic); m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); @@ -398,7 +395,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", MAX_NUMBER_OF_ROBOTS); - uavstate_pub = n_c.advertise("uavstate", 5); + bvmstate_pub = n_c.advertise("bvmstate", 5); grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); @@ -431,7 +428,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c) { flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); } - else if (it->second == "mavros_msgs/BatteryStatus") + else if (it->second == "sensor_msgs/BatteryState") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } @@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher() neigh_pos_pub.publish(neigh_pos_array); } -void roscontroller::uavstate_publisher() +void roscontroller::state_publisher() /* / Publish current UAVState from Buzz script /----------------------------------------------------*/ { - std_msgs::String uavstate_msg; - uavstate_msg.data = buzz_utility::getuavstate(); - uavstate_pub.publish(uavstate_msg); + std_msgs::String state_msg; + state_msg.data = buzz_utility::get_bvmstate(); + bvmstate_pub.publish(state_msg); } void roscontroller::grid_publisher() @@ -619,7 +616,7 @@ with size......... | / rheader[0]=0; payload_out.sysid = (uint8_t)robot_id; payload_out.msgid = (uint32_t)message_number; - + if(buzz_utility::get_timesync_state()){ // prepare rosbuzz msg header tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME; @@ -630,7 +627,7 @@ with size......... | / payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[2]); //payload_out.payload64.push_back((uint64_t)message_number); - // add time sync algo data + // add time sync algo data payload_out.payload64.push_back(ros::Time::now().toNSec()); payload_out.payload64.push_back(logical_clock.toNSec()); //uint64_t ltrate64 = 0; @@ -639,7 +636,7 @@ with size......... | / } else{ // prepare rosbuzz msg header - tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; + tmphead[0] = (uint8_t)BUZZ_MESSAGE_NO_TIME; memcpy(rheader, tmphead, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader[0]); @@ -714,7 +711,7 @@ script float* gimbal; switch (buzzuav_closures::bzz_cmd()) { - case buzzuav_closures::COMMAND_TAKEOFF: + case NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); @@ -737,7 +734,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_LAND: + case NAV_LAND: cmd_srv.request.command = buzzuav_closures::getcmd(); if (current_mode != "LAND") { @@ -756,7 +753,7 @@ script armstate = 0; break; - case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + case NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! cmd_srv.request.param5 = home.latitude; cmd_srv.request.param6 = home.longitude; cmd_srv.request.param7 = home.altitude; @@ -769,32 +766,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! - goto_pos = buzzuav_closures::getgoto(); - cmd_srv.request.param5 = goto_pos[0]; - cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); -#ifdef MAVROSKINETIC - cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START; -#else - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; -#endif - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; - - case buzzuav_closures::COMMAND_ARM: + case COMPONENT_ARM_DISARM: if (!armstate) { SetMode("LOITER", 0); @@ -803,7 +775,7 @@ script } break; - case buzzuav_closures::COMMAND_DISARM: + case COMPONENT_ARM_DISARM+1: if (armstate) { armstate = 0; @@ -812,22 +784,18 @@ script } break; - case buzzuav_closures::COMMAND_MOVETO: + case NAV_SPLINE_WAYPOINT: goto_pos = buzzuav_closures::getgoto(); roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; - case buzzuav_closures::COMMAND_GIMBAL: + case DO_MOUNT_CONTROL: gimbal = buzzuav_closures::getgimbal(); cmd_srv.request.param1 = gimbal[0]; cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param4 = gimbal[3]; -#ifdef MAVROSKINETIC - cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; -#else - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; -#endif + cmd_srv.request.command = DO_MOUNT_CONTROL; if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -836,7 +804,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_PICTURE: + case IMAGE_START_CAPTURE: ROS_INFO("TAKING A PICTURE HERE!! --------------"); mavros_msgs::CommandBool capture_command; if (capture_srv.call(capture_command)) @@ -902,10 +870,10 @@ float roscontroller::constrainAngle(float x) / Wrap the angle between -pi, pi ----------------------------------------------------------- */ { - x = fmod(x, 2 * M_PI); + x = fmod(x + M_PI, 2 * M_PI); if (x < 0.0) x += 2 * M_PI; - return x; + return x - M_PI; } void roscontroller::gps_rb(POSE nei_pos, double out[]) @@ -917,10 +885,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[]) float ned_x = 0.0, ned_y = 0.0; gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); - // out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y, ned_x); - out[1] = constrainAngle(atan2(ned_y, ned_x)); - // out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; } @@ -996,6 +961,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt { cur_pos.x = msg->pose.position.x; cur_pos.y = msg->pose.position.y; + buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead tf::Quaternion q( @@ -1127,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint16_t mtype = r16head[0]; uint16_t mid = r16head[1]; uint32_t temptime=0; - memcpy(&temptime, r16head+2, sizeof(uint32_t)); + memcpy(&temptime, r16head+2, sizeof(uint32_t)); float stime = (float)temptime/(float)100000; // if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid); // Check for Updater message, if updater message push it into updater FIFO @@ -1156,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) } // BVM FIFO message else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || - (int)mtype == (int)BUZZ_MESSAGE_WTO_TIME) + (int)mtype == (int)BUZZ_MESSAGE_NO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1217,25 +1183,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m int rc_cmd; switch (req.command) { - case mavros_msgs::CommandCode::NAV_TAKEOFF: + case NAV_TAKEOFF: ROS_INFO("RC_call: TAKE OFF!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; + rc_cmd = NAV_TAKEOFF; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case mavros_msgs::CommandCode::NAV_LAND: + case NAV_LAND: ROS_INFO("RC_Call: LAND!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_LAND; + rc_cmd = NAV_LAND; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; -#ifdef MAVROSKINETIC - case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; -#else - case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; -#endif + case COMPONENT_ARM_DISARM: + rc_cmd = COMPONENT_ARM_DISARM; armstate = req.param1; if (armstate) { @@ -1250,31 +1211,23 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m res.success = true; } break; - case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: + case NAV_RETURN_TO_LAUNCH: ROS_INFO("RC_Call: GO HOME!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + rc_cmd = NAV_RETURN_TO_LAUNCH; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case mavros_msgs::CommandCode::NAV_WAYPOINT: + case NAV_WAYPOINT: ROS_INFO("RC_Call: GO TO!!!! "); buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); - rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; + rc_cmd = NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; -#ifdef MAVROSKINETIC - case mavros_msgs::CommandCode::DO_MOUNT_CONTROL: -#else - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: -#endif + case DO_MOUNT_CONTROL: ROS_INFO("RC_Call: Gimbal!!!! "); buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); -#ifdef MAVROSKINETIC - rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; -#else - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; -#endif + rc_cmd = DO_MOUNT_CONTROL; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; @@ -1303,7 +1256,7 @@ void roscontroller::get_number_of_robots() / Garbage collector for the number of robots in the swarm --------------------------------------------------------------------------*/ { - int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; + int cur_robots = buzz_utility::get_swarmsize(); if (no_of_robots == 0) { no_of_robots = cur_robots; @@ -1483,7 +1436,7 @@ void roscontroller::get_xbee_status() void roscontroller::time_sync_step() /* - * Steps the time syncronization algorithm + * Steps the time syncronization algorithm ------------------------------------------------------------------ */ { //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); @@ -1495,13 +1448,13 @@ void roscontroller::time_sync_step() { avgRate += (it->second).relative_rate; // estimate current offset - int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; + int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; avgOffset = avgOffset + offset; offsetCount++; if((it->second).age > BUZZRATE){ neighbours_time_map.erase(it++); } - else{ + else{ (it->second).age++; ++it; } @@ -1519,10 +1472,10 @@ void roscontroller::time_sync_step() //neighbours_time_map.clear(); logical_time_rate = avgRate; -} +} void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr) /* - * pushes a time syncronization msg into its data slot + * pushes a time syncronization msg into its data slot ------------------------------------------------------------------ */ { map::iterator it = neighbours_time_map.find(nid); @@ -1532,10 +1485,10 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou int64_t delatNei = round(nh - (it->second).nei_hardware_time); double currentRate = 0; if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; - relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + (1- MOVING_AVERAGE_ALPHA)*currentRate; - - ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", + + ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); neighbours_time_map.erase(it); }