diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..4d868f8 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rosbuzz) + +if(UNIX) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + mavros_msgs + sensor_msgs + nav_msgs + message_generation +) + +############################## +############################## + +add_message_files( + FILES + neigh_pos.msg +) + +generate_messages( +DEPENDENCIES +sensor_msgs +) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS include +# LIBRARIES xbee_ros_node + CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +include_directories( + include ${rosbuzz_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) +# set the path to the library folder +link_directories(/usr/local/lib) + +# Executables +add_executable(rosbuzz_node src/rosbuzz.cpp + src/roscontroller.cpp + src/buzz_utility.cpp + src/buzzuav_closures.cpp + src/buzz_update.cpp) +target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread) +add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp) + +# Executables and libraries for installation to do +install(TARGETS rosbuzz_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + 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/Update.log b/buzz_scripts/Update.log new file mode 100644 index 0000000..e69de29 diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz new file mode 100644 index 0000000..27628b0 --- /dev/null +++ b/buzz_scripts/include/act/barrier.bzz @@ -0,0 +1,90 @@ +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_TIMEOUT = 200 # in steps +BARRIER_VSTIG = 80 +timeW = 0 +barrier = nil +bc = 0; + +# +# 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, bc) + barrier.put("d", 0) +} + +# +# Executes the barrier +# +function barrier_wait(threshold, transf, resumef, bc) { + barrier.put(id, bc) + barrier.get(id) + 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) { + allgood = barrier_allgood(barrier,bc) + } + + if(allgood) { + barrier.put("d", 1) + timeW = 0 + BVMSTATE = transf + } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + barrier = nil + timeW = 0 + BVMSTATE = resumef + } else if(timeW % 100 == 0 and bc > 0) + neighbors.broadcast("cmd", bc) + + timeW = timeW+1 +} + +barriergood = 1 + +# Barrer check all entries +function barrier_allgood(barrier, bc) { + barriergood = 1 + barrier.foreach( + function(key, value, robot){ + #log("VS entry : ", key, " ", value, " ", robot) + if(value != bc and key != "d"){ + barriergood = 0 + } + } + ) + return barriergood +} \ No newline at end of file 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 new file mode 100644 index 0000000..d657797 --- /dev/null +++ b/buzz_scripts/include/act/states.bzz @@ -0,0 +1,311 @@ +######################################## +# +# FLIGHT-RELATED FUNCTIONS +# +######################################## +include "utils/vec2.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. +GOTOANG_TOL = 0.1 # rad. +path_it = 0 +graphid = 3 +pic_time = 0 +g_it = 0 + +function turnedoff() { + BVMSTATE = "TURNEDOFF" +} + +function idle() { + BVMSTATE = "IDLE" +} + + +# Custom function for the user. +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() { + BVMSTATE = "LAUNCH" + 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, "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, "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" + 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) { + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) + } + } else { + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) + } +} + +function take_picture() { + BVMSTATE="PICTURE" + setgimbal(0.0, 0.0, -90.0, 20.0) + if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize + takepicture() + } else if(pic_time>=PICTURE_WAIT) { # wait for the picture + BVMSTATE="IDLE" + pic_time=0 + } + pic_time=pic_time+1 +} + +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) + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + transf() + else { + m_navigation = LimitSpeed(m_navigation, 1.0) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + } +} + +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)) + }) + 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) + 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) +} + +# circle all together around centroid +function pursuit() { + BVMSTATE="PURSUIT" + rd = 20.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) + r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) + r = math.vec2.length(r_vec) + 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) + vfg = LimitSpeed(vfg, 2.0) + goto_abs(vfg.x, vfg.y, 0.0, 0.0) +} + +# Lennard-Jones interaction magnitude +TARGET = 8.0 +EPSILON = 30.0 +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" + # 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()) + 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) { + log("cmd 22") + flight.rc_cmd=0 + BVMSTATE = "LAUNCH" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_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 + arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + disarm() + neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==777){ + flight.rc_cmd=0 + reinit_time_sync() + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + 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() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + destroyGraph() + 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(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==777 and BVMSTATE=="TURNEDOFF"){ + reinit_time_sync() + #neighbors.broadcast("cmd", 777) + }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/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz new file mode 100644 index 0000000..c3d3393 --- /dev/null +++ b/buzz_scripts/include/plan/mapmatrix.bzz @@ -0,0 +1,145 @@ + +# Write to matrix +robot_marker = "X" + +# copy a full matrix row +function mat_copyrow(out,ro,in,ri){ + out[ro] = {} + var icr = 1 + while(icr <= size(in[ri])) { + out[ro][icr]=in[ri][icr] + icr = icr + 1 + } +} + +function getvec(t,row){ + return math.vec2.new(t[row][1],t[row][2]) +} + +function init_test_map(len){ + map = {} + var indexR = 1 + while(indexR <= len) { + map[indexR] = {} + var indexC = 1 + while(indexC <= len) { + map[indexR][indexC] = 1.0 + indexC = indexC + 1 + } + indexR = indexR + 1 + } + # DEBUG\TEST: puts an obstacle right in the middle + map[5][5] = 0.0 + map[6][5] = 0.0 + map[4][5] = 0.0 + + log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.") +} + +function init_map(len){ + map = {} + var indexR = 1 + while(indexR <= len) { + map[indexR] = {} + var indexC = 1 + while(indexC <= len) { + map[indexR][indexC] = 1.0 + indexC = indexC + 1 + } + indexR = indexR + 1 + } + log("Occupancy grid initialized (",size(map),"x",size(map[1]),").") +} + +function add_obstacle(pos, off, inc_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("Add obstacle in cell: ", xi, " ", yi) + var old=map[xi][yi] + if(old-inc_trust > 0.0) + map[xi][yi] = old-inc_trust + else + map[xi][yi] = 0.0 + } +} + +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) + var old=map[xi][yi] + if(old + dec_trust < 1.0) #x,y + map[xi][yi] = old+dec_trust + else + map[xi][yi] = 1.0 + } +} + + +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) + }) +} + +function table_copy(t) { + var t2 = {} + foreach(t, function(key, value) { + t2[key] = value + }) + return t2 +} + +function print_pos(t) { + var ir=1 + while(ir <= size(t)) { + log(ir, ": ", t[ir][1], " ", t[ir][2]) + ir = ir + 1 + } +} + +function print_map(t) { + var ir=size(t) + log("Printing a ", size(t), " by ", size(t[1]), " map") + while(ir > 0) { + logst=string.concat("\t", string.tostring(ir), "\t:") + ic = size(t[ir]) + while(ic > 0) { + if(ir==cur_cell.x and ic==cur_cell.y) + logst = string.concat(logst, " XXXXXXXX") + else + logst = string.concat(logst, " ", string.tostring(t[ir][ic])) + ic = ic - 1 + } + log(logst) + ir = ir - 1 + } + export_map(map) +} diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz new file mode 100644 index 0000000..a55aff1 --- /dev/null +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -0,0 +1,525 @@ +##### +# RRT* Path Planing +# +# map table-based matrix +##### +include "plan/mapmatrix.bzz" + +RRT_TIMEOUT = 500 +RRT_RUNSTEP = 3 +PROX_SENSOR_THRESHOLD_M = 8.0 +GSCALE = {.x=1, .y=1} +map = nil +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 + 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, 0.0) + 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, 0.0) + } + } 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) { + # create a map big enough for the goal + 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)) +} + +# 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) + # create the goal in the map grid + mapgoal = getcell(math.vec2.add(m_goal, m_pos)) + + #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=0,.4=1,.5=0} + goalReached = 0 + timeout = 0 + + # search for a path + old_statef = statef + rrtstar() +} + +# 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) + 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])) + cell.y = size(map[1]) + if(cell.x < 1) + cell.x = 1 + if(cell.y < 1) + cell.y = 1 + + return cell +} + +function populateGrid(m_pos) { + # getproxobs(m_pos) + getneiobs (m_pos) + export_map(map) +} + +# 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 + neighbors.foreach(function(rid, data) { + add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) + }) +} + +# 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) { +# 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 +# TODO: adapt to M100 stereo +function getproxobs (m_curpos) { + var updated_obstacle = 0 + cur_cell = getcell(m_curpos) + + reduce(proximity, + function(key, value, acc) { + 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; + } + } + } + return acc + }, math.vec2.new(0, 0)) + + return updated_obstacle +} + +# 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 + 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) + } + if(doesItIntersect(Cvec, Nvec)) { + log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") + pathisblocked = 1 + } + Cvec = Nvec + nb = nb + 1 + } + + return pathisblocked +} + +## +# main search loop as a state +## +function rrtstar() { + # update state machine variables + BVMSTATE = "PATHPLAN" + + + 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)) + #log("Point generated: ", pt.x, " ", pt.y) + + var pointList = findPointsInRadius(pt,Q,RADIUS); + + # Find connection that provides the least cost to come + nbCount = 0; + minCounted = 999; + minCounter = 0; + + if(size(pointList) != 0) { + #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) + ipt=1 + while(ipt <= size(pointList)) { + pointNumber = {} + mat_copyrow(pointNumber,1,pointList,ipt) + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(pointNumber,1)); + #log("intersects1: ", intersects) + + # If there is no intersection we need consider its connection + 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][4]][5] + + if(distance < minCounted) { + minCounted = distance; + minCounter = nbCount; + } + } + ipt = ipt + 1 + } + 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] + Q[numberOfPoints][4] = numberOfPoints + Q[numberOfPoints][5] = minCounted + + #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y) + + # Now check to see if any of the other points can be redirected + nbCount = 0; + ipt = 1 + 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)); + #log("intersects2: ", intersects) + + # If there is no intersection we need consider its connection + nbCount = nbCount + 1; + 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 + } + } + ipt = ipt + 1 + } + + # Check to see if this new point is within the goal + if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } else { + # Associate with the closest point + pointNum = findClosestPoint(pt,arrayOfPoints); + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum)); + #log("intersects3 (", pointNum, "): ", intersects) + + # 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 + Q[numberOfPoints][4] = numberOfPoints + Q[numberOfPoints][5] = Q[pointNum][5]+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt)) + + #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y) + + # Check to see if this new point is within the goal + if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } + if(numberOfPoints % 100 == 0) { + log(numberOfPoints, " points processed. Still looking for goal."); + } + timeout = timeout + 1 + step_timeout = step_timeout + 1 + } + + if(goalReached){ + log("Goal found(",numberOfPoints,")!") + Path = convert_path(getPath(Q,numberOfPoints)) + # done. resume the state machine + BVMSTATE = "NAVIGATE" + } else if(timeout >= RRT_TIMEOUT) { + log("FAILED TO FIND A PATH!!!") + Path = nil + # done. resume the state machine + BVMSTATE = "NAVIGATE" + } +} + +# Go through each points and find the distances between them and the +# target point +function findClosestPoint(point,aPt) { + var distance = 999 + var pointNb = -1 + var ifcp=1 + while(ifcp <= size(aPt)) { + var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + + if(range < distance) { + distance = range; + pointNb = ifcp; + } + ifcp = ifcp + 1 + } + return pointNb +} + +# Find the closest point in the tree +function findPointsInRadius(point,q,r) { + var ptList = {} + var counted = 0; + var iir = 1 + while(iir <= size(q)) { + var tmpvv = getvec(q,iir) + #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) + var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + + if(distance < r) { + counted = counted+1; + mat_copyrow(ptList,counted,q,iir) + } + + iir = iir + 1 + } + 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) + 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 >= 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 + return 0 + } else + return 0 + } + var vec = math.vec2.scale(dif,1.0/distance) + var pathstep = size(map) - 2 + + idii = 1.0 + while(idii <= pathstep) { + 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, "(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) { + if(map[xi][yi] < 0.5) { # because obstacle use trust values + #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!") + return 1; + } + } else { + #log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y) + return 1; + } + } + idii = idii + 1.0 + } + #log("No intersect!") + return 0 +} + +# create a table with only the path's points in order +function getPath(Q,nb){ + var pathL={} + var npt=0 + # get the path from the point list + while(nb > 0) { + npt=npt+1 + pathL[npt] = {} + pathL[npt][1]=Q[nb][1] + pathL[npt][2]=Q[nb][2] + 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. + var pathR={} + var totpt = npt + 1 + while(npt > 0){ + pathR[totpt-npt] = {} + 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)) + return pathR +} diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz new file mode 100644 index 0000000..dc6527e --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -0,0 +1,798 @@ +# +# Include files +# + +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 +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 + +# +# Global variables +# + +# +# Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLabel={}#store received neighbour message +m_MessageReqLabel={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +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("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("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + +#navigation vector +m_navigation={.x=0,.y=0} + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 +m_messageID={} +repeat_assign=0 +assign_label=-1 +assign_id=-1 +m_gotjoinedparent = 0 +#neighbor distance to lock the current pattern +lock_neighbor_id={} +lock_neighbor_dis={} + +#Label request id +m_unRequestId=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +# virtual stigmergy for the LOCK barrier. +m_lockstig = 1 + +# Lennard-Jones parameters, may need change +EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i=0){ + pon=0 + } + else{ + pon=1 + } + var b=math.abs(send_table.Bearing) + send_value=r_id*1000+pon*100+b + return send_value +} +# +#unpack message +# +function unpackmessage(recv_value){ + var wan=(recv_value-recv_value%100000)/100000 + recv_value=recv_value-wan*100000 + var qian=(recv_value-recv_value%10000)/10000 + recv_value=recv_value-qian*10000 + var bai=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-bai*1000 + var shi=(recv_value-recv_value%10)/10 + recv_value=recv_value-shi*10 + var ge=recv_value + var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0} + return_table.State=wan + return_table.Label=qian + return_table.ReqLabel=bai + return_table.ReqID=shi + return_table.Response=ge + return return_table +} +# +#unpack guide message +# +function unpack_guide_msg(recv_value){ +#log(id,"I pass value=",recv_value) + var qian=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-qian*1000 + var bai=(recv_value-recv_value%100)/100 + recv_value=recv_value-bai*100 + var b=recv_value + var return_table={.Label=0.0,.Bearing=0.0} + return_table.Label=qian + if(bai==1){ + b=b*-1.0 + } + return_table.Bearing=b + return return_table +} +# +#get the target distance to neighbr nei_id +# +function target4label(nei_id){ + var return_val="miss" + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} +# +#Transistion to state free +# +function TransitionToFree(){ + BVMSTATE="GRAPH_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=s2i(BVMSTATE) +} +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + BVMSTATE="GRAPH_ASKING" + m_nLabel=un_label + m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.ReqLabel=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} +# +#Transistion to state joining +# +function TransitionToJoining(){ + BVMSTATE="GRAPH_JOINING" + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod +} +# +#Transistion to state joined +# +function TransitionToJoined(){ + BVMSTATE="GRAPH_JOINED" + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + + #write statues + #v_tag.put(m_nLabel, m_lockstig) + barrier_create() + barrier_ready(900+50) + + m_navigation.x=0.0 + m_navigation.y=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 +# +function TransitionToLock(){ + BVMSTATE="GRAPH_LOCK" + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + #record neighbor distance + lock_neighbor_id={} + lock_neighbor_dis={} + var i=0 + while(i0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLabels={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + TransitionToAsking(unFoundLabel) + return + } + + #set message + m_selfMessage.State=s2i(BVMSTATE) + + BroadcastGraph() +} +# +#Do asking +# +function DoAsking(){ + UpdateGraph() + #look for response from predecessor + var i=0 + var psResponse=-1 + while(im_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + if(repeat_assign==0){ + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + assign_label=ReqLabel + assign_id=ReqID + repeat_assign=1 + } + m_selfMessage.ReqLabel=assign_label + m_selfMessage.ReqID=assign_id + m_selfMessage.Response=r2i("REQ_GRANTED") + #m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",assign_label) + log("ID=",assign_id) + m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod + } + + #lost pred, wait for some time and transit to free + if(seenPred==0){ + m_unWaitCount=m_unWaitCount-1 + if(m_unWaitCount==0){ + TransitionToFree() + return + } + } + barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941) + BroadcastGraph() +} +# +#Do Lock +# +timeout_graph = 40 +function DoLock() { + UpdateGraph() + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_navigation.x=0.0 + m_navigation.y=0.0 + #calculate motion vection + if(m_nLabel==0){ + m_navigation.x=0.0 #change value so that robot 0 will move + m_navigation.y=0.0 + } + if(m_nLabel!=0){ + m_navigation=motion_vector() + } +# #move + +# 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 +# } +} + +# +# Executed at init +# +function initGraph() { + # + # Global parameters for graph formation + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=100 + m_fTargetAngleTolerance=0.1 + m_unJoiningLostPeriod=100 +} +# +# Executed every step (main loop) +# +function UpdateGraph() { + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} +} + +function BroadcastGraph() { + #navigation + #broadcast message + neighbors.broadcast("m",packmessage(m_selfMessage)) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLabel={}#store received neighbour message + m_MessageReqLabel={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCount=0 + +} +# +# 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} + m_nLabel=-1 + m_messageID={} + lock_neighbor_id={} + lock_neighbor_dis={} + m_unRequestId=0 + m_bias=0 + m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + m_unWaitCount=0 + repeat_assign=0 + assign_label=-1 + assign_id=-1 + m_gotjoinedparent = 0 + + if(graphid==0){ + log("Loading P graph") + Read_GraphP() + } else if(graphid==1) { + log("Loading O graph") + Read_GraphO() + } else if(graphid==2) { + log("Loading L graph") + Read_GraphL() + } else if(graphid==3) { + log("Loading Y graph") + Read_GraphY() + } + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==ROOT_ID){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} +# +# Executed upon destroy +# +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, 0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("m") +} diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph new file mode 100644 index 0000000..6747976 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 0.0 5000.0 +2 0 200.0 1.57 7000.0 +3 1 200.0 0.0 9000.0 +4 2 200.0 1.57 11000.0 +5 4 200.0 1.57 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph new file mode 100644 index 0000000..3724ba7 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 0.0 5000.0 +2 0 200.0 1.57 7000.0 +3 1 200.0 1.57 9000.0 +4 1 141.0 3.93 11000.0 +5 2 141.0 0.785 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph new file mode 100644 index 0000000..52a8d6a --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 0.0 5000.0 +2 0 200.0 1.57 7000.0 +3 0 200.0 4.71 9000.0 +4 1 141.0 0.79 11000.0 +5 2 200.0 0.0 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph new file mode 100644 index 0000000..0aff949 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 4.71 5000.0 +2 0 141.0 2.36 7000.0 +3 0 141.0 0.79 9000.0 +4 2 141.0 2.36 11000.0 +5 3 141.0 0.79 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png new file mode 100644 index 0000000..3e01a88 Binary files /dev/null and b/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png differ diff --git a/buzz_scripts/include/taskallocate/graphs/images/frame_01207.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01207.png new file mode 100644 index 0000000..6be4f0e Binary files /dev/null and b/buzz_scripts/include/taskallocate/graphs/images/frame_01207.png differ diff --git a/buzz_scripts/include/taskallocate/graphs/images/frame_01394.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01394.png new file mode 100644 index 0000000..b967a62 Binary files /dev/null and b/buzz_scripts/include/taskallocate/graphs/images/frame_01394.png differ diff --git a/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png new file mode 100644 index 0000000..b0531d5 Binary files /dev/null and b/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png differ diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz new file mode 100644 index 0000000..2988d81 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphL(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label 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] = { + .Label = 1, + .Pred = 0, + .distance = 800, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 800, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 1, + .distance = 800, + .bearing = 0.0, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 2, + .distance = 800, + .bearing = 1.57, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 4, + .distance = 800, + .bearing = 1.57, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz new file mode 100644 index 0000000..068e5a2 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphO(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label 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] = { + .Label = 1, + .Pred = 0, + .distance = 2000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 2000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 1, + .distance = 2000, + .bearing = 1.57, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 1, + .distance = 1414, + .bearing = 3.93, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 2, + .distance = 1414, + .bearing = 0.785, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz new file mode 100644 index 0000000..14e183b --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphP(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label 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] = { + .Label = 1, + .Pred = 0, + .distance = 1000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 1000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 1, + .distance = 707, + .bearing = 0.79, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 2, + .distance = 1000, + .bearing = 0.0, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz new file mode 100644 index 0000000..171c693 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphY(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label 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] = { + .Label = 1, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 707, + .bearing = 2.36, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 0, + .distance = 707, + .bearing = 0.79, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 2, + .distance = 707, + .bearing = 2.36, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 3, + .distance = 707, + .bearing = 0.79, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz new file mode 100644 index 0000000..a924696 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz @@ -0,0 +1,60 @@ +#Table of the nodes in the graph +m_vecNodes={} +m_vecNodes_fixed={} +function Read_Graph(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label 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] = { + .Label = 1, + .Pred = 0, + .distance = 1000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 1000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 0, + .distance = 1000, + .bearing = 3.14, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 0, + .distance = 707, + .bearing = 3.93, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} + +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv new file mode 100644 index 0000000..e0a2c42 --- /dev/null +++ b/buzz_scripts/include/taskallocate/waypointlist.csv @@ -0,0 +1,89 @@ +0,-73.1531935978886,45.5084960903092,15,15 +1,-73.1530989420915,45.5085624255498,15,15 +2,-73.1530042862771,45.5086287608025,15,15 +3,-73.1529096304626,45.5086950960552,15,15 +4,-73.1529458247399,45.5087204611798,15,15 +5,-73.1530404805543,45.5086541259271,15,15 +6,-73.1531351363515,45.5085877906865,15,15 +7,-73.1532297921486,45.508521455446,15,15 +8,-73.1533244479457,45.5084551202054,15,15 +9,-73.1533606422273,45.508480485333,15,15 +10,-73.1532659864302,45.5085468205736,15,15 +11,-73.153171330633,45.5086131558142,15,15 +12,-73.1530766748359,45.5086794910548,15,15 +13,-73.1529820190215,45.5087458263075,15,15 +14,-73.1530182132901,45.5087711914261,15,15 +15,-73.1531128691045,45.5087048561733,15,15 +16,-73.1532075249016,45.5086385209327,15,15 +17,-73.1533021806988,45.5085721856922,15,15 +18,-73.1533968364959,45.5085058504516,15,15 +19,-73.1534330307645,45.5085312155701,15,15 +20,-73.1533383749674,45.5085975508107,15,15 +21,-73.1532437191702,45.5086638860513,15,15 +22,-73.1531490633731,45.5087302212919,15,15 +23,-73.1530544075587,45.5087965565446,15,15 +24,-73.1530906018273,45.5088219216632,15,15 +25,-73.1531852576417,45.5087555864105,15,15 +26,-73.1532799134389,45.5086892511699,15,15 +27,-73.153374569236,45.5086229159293,15,15 +28,-73.1534692250331,45.5085565806887,15,15 +29,-73.1535054193017,45.5085819458072,15,15 +30,-73.1534107635046,45.5086482810478,15,15 +31,-73.1533161077075,45.5087146162884,15,15 +32,-73.1532214519103,45.508780951529,15,15 +33,-73.1531267960959,45.5088472867817,15,15 +34,-73.1531629903645,45.5088726519003,15,15 +35,-73.1532576461789,45.5088063166476,15,15 +36,-73.1533523019761,45.508739981407,15,15 +37,-73.1534469577732,45.5086736461664,15,15 +38,-73.1535416135703,45.5086073109258,15,15 +39,-73.1535778078389,45.5086326760444,15,15 +40,-73.1534831520418,45.5086990112849,15,15 +41,-73.1533884962447,45.5087653465255,15,15 +42,-73.1532938404476,45.5088316817661,15,15 +43,-73.1531991846331,45.5088980170188,15,15 +44,-73.1532353789017,45.5089233821374,15,15 +45,-73.1533300347162,45.5088570468847,15,15 +46,-73.1534246905133,45.5087907116441,15,15 +47,-73.1535193463104,45.5087243764035,15,15 +48,-73.1536140021075,45.5086580411629,15,15 +49,-73.1536501963762,45.5086834062815,15,15 +50,-73.153555540579,45.508749741522,15,15 +51,-73.1534608847819,45.5088160767626,15,15 +52,-73.1533662289848,45.5088824120032,15,15 +53,-73.1532715731703,45.508948747256,15,15 +54,-73.1533077674389,45.5089741123745,15,15 +55,-73.1534024232534,45.5089077771218,15,15 +56,-73.1534970790505,45.5088414418812,15,15 +57,-73.1535917348476,45.5087751066406,15,15 +58,-73.1536863906448,45.5087087714,15,15 +59,-73.1537225849134,45.5087341365186,15,15 +60,-73.1536279291163,45.5088004717592,15,15 +61,-73.1535332733191,45.5088668069997,15,15 +62,-73.153438617522,45.5089331422403,15,15 +63,-73.1533439617076,45.5089994774931,15,15 +64,-73.1533801559762,45.5090248426116,15,15 +65,-73.1534748117906,45.5089585073589,15,15 +66,-73.1535694675877,45.5088921721183,15,15 +67,-73.1536641233849,45.5088258368777,15,15 +68,-73.153758779182,45.5087595016371,15,15 +69,-73.1537949734506,45.5087848667557,15,15 +70,-73.1537003176535,45.5088512019963,15,15 +71,-73.1536056618563,45.5089175372369,15,15 +72,-73.1535110060592,45.5089838724775,15,15 +73,-73.1534163502448,45.5090502077302,15,15 +74,-73.1534525445134,45.5090755728487,15,15 +75,-73.1535472003278,45.509009237596,15,15 +76,-73.1536418561249,45.5089429023554,15,15 +77,-73.1537365119221,45.5088765671148,15,15 +78,-73.1538311677192,45.5088102318742,15,15 +79,-73.1538673619878,45.5088355969928,15,15 +80,-73.1537727061907,45.5089019322334,15,15 +81,-73.1536780503936,45.508968267474,15,15 +82,-73.1535833945964,45.5090346027146,15,15 +83,-73.153488738782,45.5091009379673,15,15 +84,-73.1535249330852,45.5091263031101,15,15 +85,-73.1536195888996,45.5090599678574,15,15 +86,-73.1537142446968,45.5089936326168,15,15 +87,-73.1538089004939,45.5089272973762,15,15 +88,-73.153903556291,45.5088609621356,15,15 diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..8b933ee --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -0,0 +1,28 @@ +TIME_TO_SYNC = 100 +sync_timer = 0 +timesync_old_state = 0 +timesync_itr = 0 +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 +} + +# Function to set sync timer to zero and reinitiate sync. algo + +function reinit_time_sync(){ + sync_timer = 0 +} diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz new file mode 100644 index 0000000..4a5c699 --- /dev/null +++ b/buzz_scripts/include/update.bzz @@ -0,0 +1,10 @@ +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +updates_active = 1 +function updated_no_bct(){ + neighbors.broadcast(updated, update_no) +} \ No newline at end of file diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz new file mode 100644 index 0000000..8cef004 --- /dev/null +++ b/buzz_scripts/include/utils/conversions.bzz @@ -0,0 +1,69 @@ +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 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={} + 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 +} diff --git a/buzz_scripts/include/utils/string.bzz b/buzz_scripts/include/utils/string.bzz new file mode 100644 index 0000000..91ceb63 --- /dev/null +++ b/buzz_scripts/include/utils/string.bzz @@ -0,0 +1,92 @@ +# +# Returns the string character at the given position. +# PARAM s: The string +# PARAM n: The position of the wanted character +# RETURN The character at the wanted position, or nil +# +string.charat = function(s, n) { + return string.sub(s, n, n+1) +} + +# +# Returns the index of the first occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the first match, or nil +# +string.indexoffirst = function(s, m) { + var las = string.length(s) + var lm = string.length(m) + var i = 0 + while(i < las-lm+1) { + if(string.sub(s, i, i+lm) == m) return i + i = i + 1 + } + return nil +} + +# +# Returns the index of the last occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the last match, or nil +# +string.indexoflast = function(s, m) { + var las = string.length(s) + var lm = string.length(m) + var i = las - lm + 1 + while(i >= 0) { + if(string.sub(s, i, i+lm) == m) return i + i = i - 1 + } + return nil +} + +# Splits a string s using the delimiters in d. The string list is +# returned in a table indexed by value (starting at 0). +# PARAM s: The string +# PARAM d: A string containing the delimiters +# RETURN: A table containing the tokens +string.split = function(s, d) { + var i1 = 0 # index to move along s (token start) + var i2 = 0 # index to move along s (token end) + var c = 0 # token count + var t = {} # token list + var las = string.length(s) + var ld = string.length(d) + # Go through string s + while(i2 < las) { + # Try every delimiter + var j = 0 # index to move along d + var f = nil # whether the delimiter was found or not + while(j < ld and (not f)) { + if(string.charat(s, i2) == string.charat(d, j)) { + # Delimiter found + f = 1 + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + c = c + 1 + } + # Start new token + i1 = i2 + 1 + } + else { + # Next delimiter + j = j + 1 + } + } + # Next string character + i2 = i2 + 1 + } + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + } + # Return token list + return t; +} diff --git a/buzz_scripts/include/utils/vec2.bzz b/buzz_scripts/include/utils/vec2.bzz new file mode 100644 index 0000000..ebfce96 --- /dev/null +++ b/buzz_scripts/include/utils/vec2.bzz @@ -0,0 +1,107 @@ +# +# Create a new namespace for vector2 functions +# +math.vec2 = {} + +# +# Creates a new vector2. +# PARAM x: The x coordinate. +# PARAM y: The y coordinate. +# RETURN: A new vector2. +# +math.vec2.new = function(x, y) { + return { .x = x, .y = y } +} + +# +# Creates a new vector2 from polar coordinates. +# PARAM l: The length of the vector2. +# PARAM a: The angle of the vector2. +# RETURN: A new vector2. +# +math.vec2.newp = function(l, a) { + return { + .x = l * math.cos(a), + .y = l * math.sin(a) + } +} + +# +# Calculates the length of the given vector2. +# PARAM v: The vector2. +# RETURN: The length of the vector. +# +math.vec2.length = function(v) { + return math.sqrt(v.x * v.x + v.y * v.y) +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +math.vec2.angle = function(v) { + return math.atan(v.y, v.x) +} + +# +# Returns the normalized form of a vector2. +# PARAM v: The vector2. +# RETURN: The normalized form. +# +math.vec2.norm = function(v) { + var l = math.length(v) + return { + .x = v.x / l, + .y = v.y / l + } +} + +# +# Calculates v1 + v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.add = function(v1, v2) { + return { + .x = v1.x + v2.x, + .y = v1.y + v2.y + } +} + +# +# Calculates v1 - v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.sub = function(v1, v2) { + return { + .x = v1.x - v2.x, + .y = v1.y - v2.y + } +} + +# +# Scales a vector by a numeric constant. +# PARAM v: A vector2. +# PARAM s: A number (float or int). +# RETURN: s * v +# +math.vec2.scale = function(v, s) { + return { + .x = v.x * s, + .y = v.y * s + } +} + +# +# Calculates v1 . v2 (the dot product) +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 . v2 +# +math.vec2.dot = function(v1, v2) { + return v1.x * v2.x + v1.y * v2.y +} diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz new file mode 100644 index 0000000..4c0e06b --- /dev/null +++ b/buzz_scripts/include/vstigenv.bzz @@ -0,0 +1,131 @@ +######################################## +# +# FLEET V.STIGMERGY-RELATED FUNCTIONS +# +######################################## +# +# Constants +# +STATUS_VSTIG = 20 +GROUND_VSTIG = 21 +HIGHEST_ROBOTID = 14 +WAIT4STEP = 10 + +# +# Init var +# +var v_status = {} +var v_ground = {} +b_updating = 0 +vstig_counter = WAIT4STEP + + +function init_swarm() { + s = swarm.create(1) + s.join() +} + +function init_stig() { + v_status = stigmergy.create(STATUS_VSTIG) + v_ground = stigmergy.create(GROUND_VSTIG) +} + +function uav_updatestig() { + # TODO: Push values on update only. + if(vstig_counter<=0) { + b_updating=1 + #var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status} + ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status + log("Pushing ", ls, "on vstig with id:", id) + v_status.put(id, ls) + vstig_counter=WAIT4STEP + } else { + b_updating=0 + vstig_counter=vstig_counter-1 + } +} + +function unpackstatus(recv_value){ + gps=(recv_value-recv_value%1000000)/1000000 + recv_value=recv_value-gps*1000000 + batt=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-batt*1000 + xbee=(recv_value-recv_value%10)/10 + recv_value=recv_value-xbee*10 + fc=recv_value + log("- GPS ", gps) + log("- Battery ", batt) + log("- Xbee ", xbee) + log("- Status ", fc) +} + +function checkusers() { + # Read a value from the structure + if(size(users)>0) + log("Got a user!") + +# log(users) +# users_print(users.dataG) +# if(size(users.dataG)>0) +# vt.put("p", users.dataG) + + # Get the number of keys in the structure +# log("The vstig has ", vt.size(), " elements") +# users_save(vt.get("p")) +# table_print(users.dataL) +} + +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +# printing the contents of a table: a custom function +function usertab_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + +function stattab_print() { + if(v_status.size()>0) { + if(b_updating==0) { + u=0 + while(u0) { + if(b_updating==0) { + u=0 + while(u 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() + +# initGraph() + + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m + + loop = 1 + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +# Executed at each time step. +function step() { + + # check time sync algorithm state + check_time_sync() + + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + 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") + statef=aggregate + else if(BVMSTATE=="FORMATION") + statef=formation + else if(BVMSTATE=="PURSUIT") + statef=pursuit + else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ? + statef=resetGraph + else if(BVMSTATE=="GRAPH_FREE") + statef=DoFree + else if(BVMSTATE=="GRAPH_ASKING") + statef=DoAsking + else if(BVMSTATE=="GRAPH_JOINING") + statef=DoJoining + else if(BVMSTATE=="GRAPH_JOINED") + statef=DoJoined + else if(BVMSTATE=="GRAPH_TRANSTOLOCK") + statef=TransitionToLock + 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 + 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() { +} diff --git a/buzz_scripts/mainRRT.bzz b/buzz_scripts/mainRRT.bzz new file mode 100644 index 0000000..2563c3e --- /dev/null +++ b/buzz_scripts/mainRRT.bzz @@ -0,0 +1,80 @@ +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 +# 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() { +} diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz new file mode 100644 index 0000000..a49329b --- /dev/null +++ b/buzz_scripts/minimal.bzz @@ -0,0 +1,63 @@ +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() { +} diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz new file mode 100644 index 0000000..c605089 --- /dev/null +++ b/buzz_scripts/stand_by.bzz @@ -0,0 +1,38 @@ +include "act/states.bzz" +include "vstigenv.bzz" + +updated="update_ack" +update_no=0 +BVMSTATE = "UPDATESTANDBY" + +# 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() + + neighbors.listen(updated, + function(vid, value, rid) { + if(value==update_no) barrier.put(rid,1) + } +) + +} + +# Executed at each time step. +function step() { + 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() { +} diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz new file mode 100644 index 0000000..70531f4 --- /dev/null +++ b/buzz_scripts/testaloneWP.bzz @@ -0,0 +1,57 @@ +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" + +#State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" + +function action() { + BVMSTATE = "ACTION" + uav_storegoal(-1.0,-1.0,-1.0) + goto_gps(picture) +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +# 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=="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() { +} diff --git a/include/buzz_update.h b/include/buzz_update.h new file mode 100644 index 0000000..4b43e86 --- /dev/null +++ b/include/buzz_update.h @@ -0,0 +1,171 @@ +#ifndef BUZZ_UPDATE_H +#define BUZZ_UPDATE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define delete_p(p) \ + do \ + { \ + free(p); \ + p = NULL; \ + } while (0) +namespace buzz_update +{ + 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 + { + uint8_t* queue; + uint8_t* size; + }; + typedef struct updater_msgqueue_s* updater_msgqueue_t; + + struct updater_code_s + { + uint8_t* bcode; + uint8_t* bcode_size; + }; + typedef struct updater_code_s* updater_code_t; + + /**************************/ + /*Updater data*/ + /**************************/ + + 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; + + /**************************************************************************/ + /*Updater routine from msg processing to file checks to be called from main*/ + /**************************************************************************/ + void update_routine(); + + /************************************************/ + /*Initalizes the updater */ + /************************************************/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + + /*********************************************************/ + /*Appends buffer of given size to in msg queue of updater*/ + /*********************************************************/ + + void code_message_inqueue_append(uint8_t* msg, uint16_t size); + + /*********************************************************/ + /*Processes messages inside the queue of the updater*/ + /*********************************************************/ + + void code_message_inqueue_process(); + + /*****************************************************/ + /*Obtains messages from out msgs queue of the updater*/ + /*******************************************************/ + uint8_t* getupdater_out_msg(); + + /******************************************************/ + /*Obtains out msg queue size*/ + /*****************************************************/ + uint8_t* getupdate_out_msg_size(); + + /**************************************************/ + /*Destroys the out msg queue*/ + /*************************************************/ + void destroy_out_msg_queue(); + + // buzz_updater_elem_t get_updater(); + /***************************************************/ + /*Sets bzz file name*/ + /***************************************************/ + void set_bzz_file(const char* in_bzz_file, bool dbg); + + /****************************************************/ + /*Tests the code from a buffer*/ + /***************************************************/ + + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); + + /****************************************************/ + /*Destroys the updater*/ + /***************************************************/ + + void destroy_updater(); + + /****************************************************/ + /*Checks for updater message*/ + /***************************************************/ + + 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/buzz_utility.h b/include/buzz_utility.h new file mode 100644 index 0000000..50132fe --- /dev/null +++ b/include/buzz_utility.h @@ -0,0 +1,108 @@ +#pragma once +#include +#include "buzz_utility.h" +#include "buzzuav_closures.h" +#include "buzz_update.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +namespace buzz_utility +{ +struct pos_struct +{ + double x, y, z; + pos_struct(double x, double y, double z) : x(x), y(y), z(z){}; + pos_struct() + { + } +}; +typedef struct pos_struct Pos_struct; +struct rb_struct +{ + double r, b, latitude, longitude, altitude; + rb_struct(double la, double lo, double al, double r, double b) + : latitude(la), longitude(lo), altitude(al), r(r), b(b){}; + rb_struct() + { + } +}; +typedef struct rb_struct RB_struct; + +struct neiStatus +{ + uint gps_strenght = 0; + uint batt_lvl = 0; + uint xbee = 0; + uint flight_status = 0; +}; +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); +int make_table(buzzobj_t* t); +int buzzusers_reset(); +int create_stig_tables(); + +void in_msg_append(uint64_t* payload); + +uint64_t* obt_out_msg(); + +void update_sensors(); + +int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); + +int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size); + +int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size); + +void buzz_script_step(); + +void buzz_script_destroy(); + +int buzz_script_done(); + +int update_step_test(); + +int get_robotid(); + +int get_swarmsize(); + +buzzvm_t get_vm(); + +void set_robot_var(int ROBOTS); + +int get_inmsg_size(); + +std::vector get_inmsg_vector(); + +std::string get_bvmstate(); + +int get_timesync_state(); + +int get_timesync_itr(); +} diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h new file mode 100644 index 0000000..71b99f1 --- /dev/null +++ b/include/buzzuav_closures.h @@ -0,0 +1,183 @@ +#pragma once + +#include +#include +#include "mavros_msgs/CommandCode.h" +#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))) +#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI))) + +namespace buzzuav_closures +{ +/* + * prextern int() function in Buzz + * This function is used to print data from buzz + * The command to use in Buzz is buzzros_print takes any available datatype in Buzz + */ +int buzzros_print(buzzvm_t vm); +void setWPlist(std::string path); + +/* + * closure to move following a vector + */ +int buzzuav_moveto(buzzvm_t vm); +/* + * closure to store a new GPS goal + */ +int buzzuav_storegoal(buzzvm_t vm); +/* + * closure to control the gimbal + */ +int buzzuav_setgimbal(buzzvm_t vm); +/* + * parse a csv list of waypoints + */ +void parse_gpslist(); +/* + * closure to export a 2D map + */ +int buzz_exportmap(buzzvm_t vm); +/* + * closure to take a picture + */ +int buzzuav_takepicture(buzzvm_t vm); +/* + * Returns the current command from local variable + */ +int getcmd(); +/* + * Sets goto position from rc client + */ +void rc_set_goto(int id, double latitude, double longitude, double altitude); +/* + *Sets gimbal orientation from rc client + */ +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); +/* + * sets rc requested command + */ +void rc_call(int rc_cmd); +/* + * sets the battery state + */ +void set_battery(float voltage, float current, float remaining); +/* + * sets the xbee network status + */ +void set_deque_full(bool state); +void set_rssi(float value); +void set_raw_packet_loss(float value); +void set_filtered_packet_loss(float value); +// void set_api_rssi(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 + */ +double* getgoto(); +/* + * returns the current grid + */ +std::map> getgrid(); + + +/* + * returns the gimbal commands + */ +float* getgimbal(); +/* + *updates flight status + */ +void flight_status_update(uint8_t state); +/* + *Update neighbors table + */ +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 + */ +int buzzuav_addNeiStatus(buzzvm_t vm); +/* + * returns the current array of neighbors status + */ +mavros_msgs::Mavlink get_status(); + +/* + *Flight status + */ +void set_obstacle_dist(float dist[]); + +/* + * Commands the UAV to takeoff + */ +int buzzuav_takeoff(buzzvm_t vm); +/* + * Arm command from Buzz + */ +int buzzuav_arm(buzzvm_t vm); +/* + * Disarm from buzz + */ +int buzzuav_disarm(buzzvm_t vm); +/* Commands the UAV to land + */ +int buzzuav_land(buzzvm_t vm); + +/* + * Command the UAV to go to home location + */ +int buzzuav_gohome(buzzvm_t vm); + +/* + * Updates battery information in Buzz + */ +int buzzuav_update_battery(buzzvm_t vm); +/* + * Updates xbee_status information in Buzz + */ +int buzzuav_update_xbee_status(buzzvm_t vm); +/* + * Updates current position in Buzz + */ +int buzzuav_update_currentpos(buzzvm_t vm); +/* + * add new target in the BVM + */ +int buzzuav_addtargetRB(buzzvm_t vm); +/* + * Updates flight status and rc command in Buzz, put it in a tabel to acess it + * use flight.status for flight status + * use flight.rc_cmd for current rc cmd + */ +int buzzuav_update_flight_status(buzzvm_t vm); + +/* + * Updates IR information in Buzz + * Proximity and ground sensors to do !!!! + */ +int buzzuav_update_prox(buzzvm_t vm); +/* + * returns the current FC command + */ +int bzz_cmd(); + +int dummy_closure(buzzvm_t vm); +} 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 new file mode 100644 index 0000000..d7bbbc5 --- /dev/null +++ b/include/roscontroller.h @@ -0,0 +1,309 @@ +#pragma once +#include +#include +#include +#include +#include +#include "mavros_msgs/GlobalPositionTarget.h" +#include "mavros_msgs/CommandCode.h" +#include "mavros_msgs/CommandLong.h" +#include "mavros_msgs/CommandBool.h" +#include "mavros_msgs/ExtendedState.h" +#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" +#include "mavros_msgs/WaypointPush.h" +#include "mavros_msgs/Waypoint.h" +#include "mavros_msgs/PositionTarget.h" +#include "mavros_msgs/StreamRate.h" +#include "mavros_msgs/ParamGet.h" +#include "geometry_msgs/PoseStamped.h" +#include "std_msgs/Float64.h" +#include "std_msgs/String.h" +#include +#include +#include +#include +#include "buzz_utility.h" +#include +#include +#include +#include +#include +#include +#include "buzzuav_closures.h" +#include "rosbuzz/mavrosCC.h" + +/* +* ROSBuzz message types +*/ +typedef enum { + ROS_BUZZ_MSG_NIL = 0, // dummy msg + UPDATER_MESSAGE, // Update msg + 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 MOVING_AVERAGE_ALPHA 0.1 +#define MAX_NUMBER_OF_ROBOTS 10 + +#define TIMEOUT 60 +#define BUZZRATE 10 + +using namespace std; + +namespace rosbuzz_node +{ +class roscontroller +{ +public: + roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); + ~roscontroller(); + void RosControllerRun(); + + static const string CAPTURE_SRV_DEFAULT_NAME; + +private: + struct num_robot_count + { + uint8_t history[10]; + uint8_t index = 0; + uint8_t current = 0; + num_robot_count() + { + } + }; + typedef struct num_robot_count Num_robot_count; + + Num_robot_count count_robots; + + 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 POSE ros_pose; + + ros_pose target, home, cur_pos; + + struct MsgData + { + int msgid; + uint16_t nid; + uint16_t size; + double sent_time; + 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(){}; + }; + typedef struct MsgData msg_data; + + 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; + std::vector inmsgdata; + uint64_t out_msg_time; + double logical_time_rate; + bool time_sync_jumped; + std::string robot_name = ""; + + int rc_cmd; + float fcu_timeout; + int armstate; + int barrier; + int update; + int message_number = 0; + uint8_t no_of_robots = 0; + bool rcclient; + bool xbeeplugged = false; + 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; + std::string modeclient; + std::string rcservice_name; + std::string bcfname, dbgfname; + std::string out_payload; + std::string in_payload; + std::string stand_by; + std::string xbeesrv_name; + std::string capture_srv_name; + std::string setpoint_name; + std::string stream_client_name; + std::string setpoint_nonraw; + + // 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; + ros::Publisher bvmstate_pub; + ros::Publisher grid_pub; + ros::Publisher localsetpoint_nonraw_pub; + ros::ServiceServer service; + ros::Subscriber current_position_sub; + ros::Subscriber users_sub; + ros::Subscriber battery_sub; + ros::Subscriber payload_sub; + ros::Subscriber flight_estatus_sub; + ros::Subscriber flight_status_sub; + ros::Subscriber obstacle_sub; + ros::Subscriber Robot_id_sub; + ros::Subscriber relative_altitude_sub; + ros::Subscriber local_pos_sub; + + std::map m_smTopic_infos; + + int setpoint_counter; + + std::ofstream log; + + // Commands for flight controller + mavros_msgs::CommandLong cmd_srv; + + mavros_msgs::CommandBool m_cmdBool; + ros::ServiceClient arm_client; + + mavros_msgs::SetMode m_cmdSetMode; + ros::ServiceClient mode_client; + + // Initialize publisher and subscriber, done in the constructor + void Initialize_pub_sub(ros::NodeHandle& n_c); + + std::string current_mode; + + /*Obtain data from ros parameter server*/ + void Rosparameters_get(ros::NodeHandle& n_c_priv); + + /*compiles buzz script from the specified .bzz file*/ + std::string Compile_bzz(std::string bzzfile_name); + + /*Flight controller service call*/ + void flight_controller_service_call(); + + /*Neighbours pos publisher*/ + void neighbours_pos_publisher(); + + /*UAVState publisher*/ + void state_publisher(); + + /*Grid publisher*/ + void grid_publisher(); + + /*BVM message payload publisher*/ + void send_MPpayload(); + + /*Prepare messages and publish*/ + void prepare_msg_and_publish(); + + /*Refresh neighbours Position for every ten step*/ + void maintain_pos(int tim_step); + + /*Puts neighbours position inside neigbours_pos_map*/ + void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr); + + /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ + void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr); + + /*Set the current position of the robot callback*/ + void set_cur_pos(double latitude, double longitude, double altitude); + + /*convert from spherical to cartesian coordinate system callback */ + float constrainAngle(float x); + 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); + + /*battery status callback */ + void battery(const sensor_msgs::BatteryState::ConstPtr& msg); + + /*flight extended status callback*/ + void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); + + /*flight status callback*/ + void flight_status_update(const mavros_msgs::State::ConstPtr& msg); + + /*current position callback*/ + void global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg); + + /*current relative altitude callback*/ + void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg); + + /*payload callback callback*/ + void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); + + /* RC commands service */ + bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res); + + /*robot id sub callback*/ + void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); + + /*Obstacle distance table callback*/ + void obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg); + + /*Get publisher and subscriber from YML file*/ + void GetSubscriptionParameters(ros::NodeHandle& node_handle); + + /*Arm/disarm method that can be called from buzz*/ + void Arm(); + + /*set mode like guided for solo*/ + void SetMode(std::string mode, int delay_miliseconds); + + /*Robot independent subscribers*/ + void Subscribe(ros::NodeHandle& n_c); + + void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); + + void fc_command_setup(); + + void SetLocalPosition(float x, float y, float z, float yaw); + + void SetLocalPositionNonRaw(float x, float y, float z, float yaw); + + void SetStreamRate(int id, int rate, int on_off); + + void get_number_of_robots(); + + // functions related to Xbee modules information + void GetRobotId(); + bool GetDequeFull(bool& result); + bool GetRssi(float& result); + bool TriggerAPIRssi(const uint8_t short_id); + 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(); + + 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/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml new file mode 100644 index 0000000..c18075b --- /dev/null +++ b/launch/launch_config/solo.yaml @@ -0,0 +1,12 @@ +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 + diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml new file mode 100644 index 0000000..9e33d37 --- /dev/null +++ b/launch/launch_config/topics.yaml @@ -0,0 +1,13 @@ +topics: + gps : global_position/global + battery : battery + status : state + estatus : extended_state + fcclient: cmd/command + setpoint: setpoint_position/local + armclient: cmd/arming + modeclient: set_mode + localpos: local_position/pose + stream: set_stream_rate + altitude: global_position/rel_alt + obstacles: obstacles diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch new file mode 100644 index 0000000..63c7906 --- /dev/null +++ b/launch/rosbuzz.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch new file mode 100644 index 0000000..36fa979 --- /dev/null +++ b/launch/rosbuzzd.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh new file mode 100644 index 0000000..bea3e11 --- /dev/null +++ b/misc/cmdlinectr.sh @@ -0,0 +1,44 @@ +#! /bin/bash +function takeoff { + rosservice call $1/buzzcmd 0 22 0 0 0 0 0 0 0 0 +} +function land { + rosservice call $1/buzzcmd 0 21 0 0 0 0 0 0 0 0 +} +function arm { + rosservice call $1/buzzcmd 0 400 0 1 0 0 0 0 0 0 +} +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 +} +function record { + rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info + +} +function clean { + sudo rm /var/log/upstart/robot* + sudo rm /var/log/upstart/dji* + sudo rm /var/log/upstart/x3s* +} +function startrobot { + sudo service dji start +} +function stoprobot { + sudo service dji stop +} +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/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 { + let "a = $1 + 900" + rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0 +} diff --git a/msg/neigh_pos.msg b/msg/neigh_pos.msg new file mode 100644 index 0000000..8db4257 --- /dev/null +++ b/msg/neigh_pos.msg @@ -0,0 +1,2 @@ +Header header +sensor_msgs/NavSatFix[] pos_neigh diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..0fa6866 --- /dev/null +++ b/package.xml @@ -0,0 +1,61 @@ + + + rosbuzz + 0.1.0 + The rosbuzz package + + + + + vivek + + + + + + MIT + + + + + + + + + + + + vivek + + + + + + + + + + + + + + catkin + roscpp + roscpp + std_msgs + std_msgs + mavros_msgs + mavros_msgs + sensor_msgs + sensor_msgs + nav_msgs + nav_msgs + message_generation + message_runtime + + + + + + + diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..d61cdb4 --- /dev/null +++ b/readme.md @@ -0,0 +1,111 @@ +ROS Implemenation of Buzz +========================= + +What is Buzz? +============= + +Buzz is a novel programming language for heterogeneous robots swarms. + +Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion. + +Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations. + +Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm. + +Self-organization results from the fact that the Buzz run-time platform is purely distributed. + +The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS. + +More information is available at http://the.swarming.buzz/wiki/doku.php?id=start. + +Description: +============ + +Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS. + + +Downloading ROS Package +======================= + + $ git clone https://github.com/MISTLab/ROSBuzz.git rosbuzz + +Requirements +============ + +* Buzz : + +You can download the development sources through git: + + $ git clone https://github.com/MISTLab/Buzz.git buzz + +* ROS binary distribution (Indigo or higher) with catkin (could be used with older versions of ROS with catkin but not tested) + + +You need the following package: + +* mavros_msgs : + +You can install using apt-get: + + $ sudo apt-get install ros--mavros ros--mavros-extras + +Compilation +=========== + +To compile the ros package, execute the following: + + $ cd catkin_ws + $ catkin_make + $ source devel/setup.bash + +Run +=== +To run the ROSBuzz package using the launch file, execute the following: + + $ roslaunch rosbuzz rosbuzz.launch + +Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch). + +* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script. + +Publisher +========= + +* Messages from Buzz (BVM): +The package publishes mavros_msgs/Mavlink message "outMavlink". + +* Command to the flight controller: +The package publishes geometry_msgs/PoseStamped message "setpoint_position/local". + +Subscribers +----------- + +* Current position of the Robot: +The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose". + +* Messages to Buzz (BVM): +The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink". + +* Status: +The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state". + +Service +------- + +* Remote Controller: +The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line. + +References +------ +* 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. + +Visual Studio Code +-------------------- +To activate highlights of the code in Visual Studio Code or Roboware add the following to settings.json: +``` + "files.associations": { + "*.bzz":"python" + } +``` diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp new file mode 100644 index 0000000..8624c3a --- /dev/null +++ b/src/buzz_update.cpp @@ -0,0 +1,724 @@ +/** @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" + +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 const uint16_t MAX_UPDATE_TRY = 5; + static size_t old_byte_code_size = 0; + static bool debug = false; + + /*Initialize updater*/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) + { + 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); + if (fd < 0) + { + 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); + } + } + } + } + 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)) + { + size += sizeof(uint16_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + { + 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); + } + + 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"; + + /*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 + { + 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); + } + } + } + + 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 ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) + { + 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; + } + } + } + + 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)) + { + if(debug) ROS_WARN("Initializtion test passed"); + if (buzz_utility::update_step_test()) + { + /*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 + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + } + else + { + /*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); + } + return 0; + } + } + else + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + 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("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(); + 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 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) + { + 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); + } + + 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; + } + + /*-------------------------------------------------------- + / Create Buzz bytecode from the bzz script input + /-------------------------------------------------------*/ + 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_; + }*/ +} \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp new file mode 100644 index 0000000..42b5f57 --- /dev/null +++ b/src/buzz_utility.cpp @@ -0,0 +1,626 @@ +/** @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_utility.h" + +namespace buzz_utility +{ +/****************************************/ +/****************************************/ + +static buzzvm_t VM = 0; +static char* BO_FNAME = 0; +static uint8_t* BO_BUF = 0; +static buzzdebug_t DBG_INFO = 0; +static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets +static uint8_t Robot_id = 0; +static std::vector IN_MSG; +std::map users_map; + +/****************************************/ + +uint16_t* u64_cvt_u16(uint64_t u64) +/* +/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user +------------------------------------------------------------------------*/ +{ + uint16_t* out = new uint16_t[4]; + uint32_t int32_1 = u64 & 0xFFFFFFFF; + uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32; + out[0] = int32_1 & 0xFFFF; + out[1] = (int32_1 & (0xFFFF0000)) >> 16; + out[2] = int32_2 & 0xFFFF; + out[3] = (int32_2 & (0xFFFF0000)) >> 16; + // DEBUG + // cout << " values " < 0 && unMsgSize <= size - tot) + { + buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize)); + tot += unMsgSize; + } + } while (size - tot > sizeof(uint16_t) && unMsgSize > 0); + free(first_INmsg); + IN_MSG.erase(IN_MSG.begin()); + } + // Process messages VM call* + buzzvm_process_inmsgs(VM); +} + +uint64_t* obt_out_msg() +/* +/ Obtains messages from buzz out message Queue +-------------------------------------------------*/ +{ + // Process out messages + buzzvm_process_outmsgs(VM); + uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE); + memset(buff_send, 0, MAX_MSG_SIZE); + // Taking into consideration the sizes included at the end + ssize_t tot = sizeof(uint16_t); + // Send robot id + *(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot; + tot += sizeof(uint16_t); + // Send messages from FIFO + do + { + // Are there more messages? + if (buzzoutmsg_queue_isempty(VM)) + break; + // Get first message + buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); + // Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes + // DEBUG + // ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); + if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) + { + buzzmsg_payload_destroy(&m); + break; + } + + // Add message length to data buffer + *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); + tot += sizeof(uint16_t); + + // Add payload to data buffer + memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); + tot += buzzmsg_payload_size(m); + + // Get rid of message + buzzoutmsg_queue_next(VM); + buzzmsg_payload_destroy(&m); + } while (1); + + uint16_t total_size = (ceil((float)tot / (float)sizeof(uint64_t))); + *(uint16_t*)buff_send = (uint16_t)total_size; + + uint64_t* payload_64 = new uint64_t[total_size]; + + memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); + free(buff_send); + // DEBUG + // for(int i=0;ipc); + char* msg; + if (dbg != NULL) + { + asprintf(&msg, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", BO_FNAME, dbg->fname, + dbg->line, dbg->col, VM->errormsg); + } + else + { + asprintf(&msg, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", BO_FNAME, VM->pc, VM->errormsg); + } + return msg; +} + +static int buzz_register_hooks() +/* +/ Buzz hooks that can be used inside .bzz file +------------------------------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + 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, "goto_abs", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); + buzzvm_gstore(VM); + 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, "setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); + buzzvm_gstore(VM); + 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, "arm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); + buzzvm_gstore(VM); + 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, "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)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap)); + buzzvm_gstore(VM); + + return VM->state; +} + +static int testing_buzz_register_hooks() +/* +/ Register dummy Buzz hooks for test during update +---------------------------------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + 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, "goto_abs", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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, "setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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, "arm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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, "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)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + + return VM->state; +} + +int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) +/* +/ Sets the .bzz and .bdbg file +---------------------------------*/ +{ + ROS_INFO(" Robot ID: %i", robot_id); + Robot_id = robot_id; + // Read bytecode from file and fill the bo buffer + FILE* fd = fopen(bo_filename, "rb"); + if (!fd) + { + perror(bo_filename); + return 0; + } + fseek(fd, 0, SEEK_END); + size_t bcode_size = ftell(fd); + rewind(fd); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fd) < bcode_size) + { + perror(bo_filename); + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + fclose(fd); + return 0; + } + fclose(fd); + + // Save bytecode file name + BO_FNAME = strdup(bo_filename); + + return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); +} + +int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +/* +/ Sets a new update +-----------------------*/ +{ + // Reset the Buzz VM + if (VM) + buzzvm_destroy(&VM); + VM = buzzvm_new(Robot_id); + // Get rid of debug info + if (DBG_INFO) + buzzdebug_destroy(&DBG_INFO); + DBG_INFO = buzzdebug_new(); + + // Read debug information + if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + perror(bdbg_filename); + return 0; + } + // Set byte code + if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); + return 0; + } + // Register hook functions + if (buzz_register_hooks() != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); + return 0; + } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + + // Execute the global part of the script + if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE) + { + ROS_ERROR("Error executing global part, VM state : %i", VM->state); + return 0; + } + // Call the Init() function + if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } + // All OK + return 1; +} + +int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +/* +/ Performs a initialization test +-----------------------------------*/ +{ + // Reset the Buzz VM + if (VM) + buzzvm_destroy(&VM); + VM = buzzvm_new(Robot_id); + // Get rid of debug info + if (DBG_INFO) + buzzdebug_destroy(&DBG_INFO); + DBG_INFO = buzzdebug_new(); + + // Read debug information + if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + perror(bdbg_filename); + return 0; + } + // Set byte code + if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); + return 0; + } + // Register hook functions + if (testing_buzz_register_hooks() != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); + return 0; + } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + + // Execute the global part of the script + if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE) + { + ROS_ERROR("Error executing global part, VM state : %i", VM->state); + return 0; + } + // Call the Init() function + if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } + // All OK + return 1; +} + +struct buzzswarm_elem_s +/* +/ Swarm struct +----------------*/ +{ + buzzdarray_t swarms; + uint16_t age; +}; +typedef struct buzzswarm_elem_s* buzzswarm_elem_t; + +void update_sensors() +/* +/ Update from all external inputs +-------------------------------*/ +{ + // Update sensors + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_xbee_status(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); +} + +void buzz_script_step() +/* +/ Step through the buzz script +-------------------------------*/ +{ + // Process available messages + in_message_process(); + // Update sensors + update_sensors(); + // Call Buzz step() function + if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); + buzzvm_dump(VM); + } +} + +void buzz_script_destroy() +/* +/ Destroy the bvm and other related ressources +-------------------------------------*/ +{ + if (VM) + { + if (VM->state != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); + buzzvm_dump(VM); + } + buzzvm_function_call(VM, "destroy", 0); + buzzvm_destroy(&VM); + free(BO_FNAME); + buzzdebug_destroy(&DBG_INFO); + } + ROS_INFO("Script execution stopped."); +} + +int buzz_script_done() +/* +/ Check if the BVM execution terminated +---------------------------------------*/ +{ + return VM->state != BUZZVM_STATE_READY; +} + +int update_step_test() +/* +/ Step test for the update mechanism +------------------------------------*/ +{ + // Process available messages + in_message_process(); + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); + + int a = buzzvm_function_call(VM, "step", 0); + + if (a != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info()); + fprintf(stdout, "step test VM state %i\n", a); + } + + return a == BUZZVM_STATE_READY; +} + +void set_robot_var(int ROBOTS) +/* +/ set swarm size in the BVM +-----------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, ROBOTS); + buzzvm_gstore(VM); +} + +int get_inmsg_size() +/* +/ get the incoming msgs size +------------------------------*/ +{ + return IN_MSG.size(); +} + +std::vector get_inmsg_vector(){ + return IN_MSG; +} + +buzzvm_t get_vm() +/* +/ return the BVM +----------------*/ +{ + return VM; +} + +string get_bvmstate() +/* +/ 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); + 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 +--------------------------------------*/ +{ + int timesync_state = 0; + if(VM){ + 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) timesync_state = obj->i.value; + buzzvm_pop(VM); + } + 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/buzzuav_closures.cpp b/src/buzzuav_closures.cpp new file mode 100644 index 0000000..4c8b2a5 --- /dev/null +++ b/src/buzzuav_closures.cpp @@ -0,0 +1,859 @@ +/** @file buzzuav_closures.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 "buzzuav_closures.h" +#include "math.h" + +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[4]; +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[4]; +static double cur_NEDpos[2]; +static uint8_t status; +static int cur_cmd = 0; +static int rc_cmd = 0; +static int rc_id = -1; +static int buzz_cmd = 0; +static float height = 0; +static bool deque_full = false; +static int rssi = 0; +static float raw_packet_loss = 0.0; +static int filtered_packet_loss = 0; +static float api_rssi = 0.0; +string WPlistname = ""; + +std::map targets_map; +std::map wplist_map; +std::map neighbors_map; +std::map neighbors_status_map; +std::map> grid; + +/****************************************/ +/****************************************/ + +int buzzros_print(buzzvm_t vm) +/* +/ Buzz closure to print out +----------------------------------------------------------- */ +{ + std::ostringstream buffer(std::ostringstream::ate); + buffer << "[" << buzz_utility::get_robotid() << "] "; + for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) + { + buzzvm_lload(vm, index); + buzzobj_t o = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + switch (o->o.type) + { + case BUZZTYPE_NIL: + buffer << " BUZZ - [nil]"; + break; + case BUZZTYPE_INT: + buffer << " " << o->i.value; + break; + case BUZZTYPE_FLOAT: + buffer << " " << o->f.value; + break; + case BUZZTYPE_TABLE: + buffer << " [table with " << buzzdict_size(o->t.value) << " elems]"; + break; + case BUZZTYPE_CLOSURE: + if (o->c.value.isnative) + { + buffer << " [n-closure @" << o->c.value.ref << "]"; + } + else + { + buffer << " [c-closure @" << o->c.value.ref << "]"; + } + break; + case BUZZTYPE_STRING: + buffer << " " << o->s.value.str; + break; + case BUZZTYPE_USERDATA: + buffer << " [userdata @" << o->u.value << "]"; + break; + default: + break; + } + } + ROS_INFO("%s", buffer.str().c_str()); + return buzzvm_ret0(vm); +} + +void setWPlist(string path) +/* +/ set the absolute path for a csv list of waypoints +----------------------------------------------------------- */ +{ + WPlistname = path + "include/taskallocate/waypointlist.csv"; +} + +float constrainAngle(float x) +/* +/ Wrap the angle between -pi, pi +----------------------------------------------------------- */ +{ + x = fmod(x + M_PI, 2 * M_PI); + if (x < 0.0) + x += 2 * M_PI; + return x - M_PI; +} + +void rb_from_gps(double nei[], double out[], double cur[]) +/* +/ Compute Range and Bearing from 2 GPS set of coordinates +/----------------------------------------*/ +{ + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); + out[1] = constrainAngle(atan2(ned_y, ned_x)); + out[2] = 0.0; +} + +void parse_gpslist() +/* +/ parse a csv of GPS targets +/----------------------------------------*/ +{ + // Open file: + ROS_INFO("WP list file: %s", WPlistname.c_str()); + std::ifstream fin(WPlistname.c_str()); // Open in text-mode. + + // Opening may fail, always check. + if (!fin) + { + ROS_ERROR("GPS list parser, could not open file."); + return; + } + + // Prepare a C-string buffer to be used when reading lines. + const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need. + char buffer[MAX_LINE_LENGTH] = {}; + const char* DELIMS = "\t ,"; // Tab, space or comma. + + // Read the file and load the data: + buzz_utility::RB_struct RB_arr; + // Read one line at a time. + while (fin.getline(buffer, MAX_LINE_LENGTH)) + { + // Extract the tokens: + int tid = atoi(strtok(buffer, DELIMS)); + double lon = atof(strtok(NULL, DELIMS)); + double lat = atof(strtok(NULL, DELIMS)); + int alt = atoi(strtok(NULL, DELIMS)); + // int tilt = atoi(strtok(NULL, DELIMS)); + // DEBUG + // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); + RB_arr.latitude = lat; + RB_arr.longitude = lon; + RB_arr.altitude = alt; + // Insert elements. + map::iterator it = wplist_map.find(tid); + if (it != wplist_map.end()) + wplist_map.erase(it); + wplist_map.insert(make_pair(tid, RB_arr)); + } + + ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); + + // Close the file: + fin.close(); +} + +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); + 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) { + 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) { + buzzvm_dup(vm); + buzzvm_pushi(vm, j); + buzzvm_tget(vm); + 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)); + buzzvm_pop(vm); + } + // DEBUG + // ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value)); + return buzzvm_ret0(vm); +} + +int buzzuav_moveto(buzzvm_t vm) +/* +/ Buzz closure to move following a 3D vector + Yaw +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // dx + buzzvm_lload(vm, 2); // dy + 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 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]); + buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? + return buzzvm_ret0(vm); +} + +int buzzuav_addtargetRB(buzzvm_t vm) +/* +/ Buzz closure to add a target (goal) GPS +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // longitude + buzzvm_lload(vm, 2); // latitude + buzzvm_lload(vm, 3); // id + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + double tmp[3]; + tmp[0] = buzzvm_stack_at(vm, 2)->f.value; + tmp[1] = buzzvm_stack_at(vm, 1)->f.value; + tmp[2] = 0.0; + int uid = buzzvm_stack_at(vm, 3)->i.value; + double rb[3]; + + rb_from_gps(tmp, rb, cur_pos); + if (fabs(rb[0]) < 100.0) + { + buzz_utility::RB_struct RB_arr; + RB_arr.latitude = tmp[0]; + RB_arr.longitude = tmp[1]; + RB_arr.altitude = tmp[2]; + RB_arr.r = rb[0]; + RB_arr.b = rb[1]; + map::iterator it = targets_map.find(uid); + if (it != targets_map.end()) + targets_map.erase(it); + targets_map.insert(make_pair(uid, RB_arr)); + // DEBUG + // ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + return vm->state; + } + else + ROS_WARN(" ---------- Target too far %f", rb[0]); + + return 0; +} + +int buzzuav_addNeiStatus(buzzvm_t vm) +/* +/ closure to add neighbors status to the BVM +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 5); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + buzzvm_type_assert(vm, 4, BUZZTYPE_INT); + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_INT); + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); + buzz_utility::neighbors_status newRS; + uint8_t id = buzzvm_stack_at(vm, 5)->i.value; + newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee = buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; + map::iterator it = neighbors_status_map.find(id); + if (it != neighbors_status_map.end()) + neighbors_status_map.erase(it); + neighbors_status_map.insert(make_pair(id, newRS)); + return vm->state; +} + +mavros_msgs::Mavlink get_status() +/* +/ return neighbors status from BVM +/----------------------------------------*/ +{ + mavros_msgs::Mavlink payload_out; + map::iterator it; + for (it = neighbors_status_map.begin(); it != neighbors_status_map.end(); ++it) + { + payload_out.payload64.push_back(it->first); + payload_out.payload64.push_back(it->second.gps_strenght); + payload_out.payload64.push_back(it->second.batt_lvl); + payload_out.payload64.push_back(it->second.xbee); + payload_out.payload64.push_back(it->second.flight_status); + } + // Add Robot id and message number to the published message + payload_out.sysid = (uint8_t)neighbors_status_map.size(); + + return payload_out; +} + +int buzzuav_takepicture(buzzvm_t vm) +/* +/ Buzz closure to take a picture here. +/----------------------------------------*/ +{ + buzz_cmd = IMAGE_START_CAPTURE; + return buzzvm_ret0(vm); +} + +int buzzuav_setgimbal(buzzvm_t vm) +/* +/ Buzz closure to change locally the gimbal orientation +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // time + buzzvm_lload(vm, 2); // pitch + buzzvm_lload(vm, 3); // roll + buzzvm_lload(vm, 4); // yaw + 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); + rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value; + rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value; + rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; + 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 = DO_MOUNT_CONTROL; + return buzzvm_ret0(vm); +} + +int buzzuav_storegoal(buzzvm_t vm) +/* +/ Buzz closure to store locally a GPS destination from the fleet +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // altitude + buzzvm_lload(vm, 2); // longitude + buzzvm_lload(vm, 3); // latitude + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + double goal[3]; + goal[0] = buzzvm_stack_at(vm, 3)->f.value; + goal[1] = buzzvm_stack_at(vm, 2)->f.value; + goal[2] = buzzvm_stack_at(vm, 1)->f.value; + if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1) + { + if (wplist_map.size() <= 0) + parse_gpslist(); + goal[0] = wplist_map.begin()->second.latitude; + goal[1] = wplist_map.begin()->second.longitude; + goal[2] = wplist_map.begin()->second.altitude; + wplist_map.erase(wplist_map.begin()->first); + } + + double rb[3]; + + rb_from_gps(goal, rb, cur_pos); + 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]); + } + return buzzvm_ret0(vm); +} + +int buzzuav_arm(buzzvm_t vm) +/* +/ Buzz closure to arm +/---------------------------------------*/ +{ + cur_cmd = COMPONENT_ARM_DISARM; + printf(" Buzz requested Arm \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_disarm(buzzvm_t vm) +/* +/ Buzz closure to disarm +/---------------------------------------*/ +{ + cur_cmd = COMPONENT_ARM_DISARM + 1; + printf(" Buzz requested Disarm \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_takeoff(buzzvm_t vm) +/* +/ Buzz closure to takeoff +/---------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); /* Altitude */ + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value; + height = goto_pos[2]; + cur_cmd = NAV_TAKEOFF; + printf(" Buzz requested Take off !!! \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_land(buzzvm_t vm) +/* +/ Buzz closure to land +/-------------------------------------------------------------*/ +{ + cur_cmd = NAV_LAND; + printf(" Buzz requested Land !!! \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_gohome(buzzvm_t vm) +/* +/ Buzz closure to return Home +/-------------------------------------------------------------*/ +{ + cur_cmd = NAV_RETURN_TO_LAUNCH; + printf(" Buzz requested gohome !!! \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +double* getgoto() +/* +/ return the GPS goal +/-------------------------------------------------------------*/ +{ + return goto_pos; +} + +std::map> getgrid() +/* +/ return the grid +/-------------------------------------------------------------*/ +{ + return grid; +} + +float* getgimbal() +/* +/ return current gimbal commands +---------------------------------------*/ +{ + return rc_gimbal; +} + +int getcmd() +/* +/ return current mavros command to the FC +---------------------------------------*/ +{ + return cur_cmd; +} + +int bzz_cmd() +/* +/ return and clean the custom command from Buzz to the FC +----------------------------------------------------------*/ +{ + int cmd = buzz_cmd; + buzz_cmd = 0; + return cmd; +} + +void rc_set_goto(int id, double latitude, double longitude, double altitude) +/* +/ update interface RC GPS goal input +-----------------------------------*/ +{ + rc_id = id; + rc_goto_pos[0] = latitude; + rc_goto_pos[1] = longitude; + rc_goto_pos[2] = altitude; +} + +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) +/* +/ update interface RC gimbal control input +-----------------------------------*/ +{ + rc_id = id; + rc_gimbal[0] = yaw; + rc_gimbal[1] = roll; + rc_gimbal[2] = pitch; + rc_gimbal[3] = t; +} + +void rc_call(int rc_cmd_in) +/* +/ update interface RC command input +-----------------------------------*/ +{ + rc_cmd = rc_cmd_in; +} + +void set_obstacle_dist(float dist[]) +/* +/ update interface proximity value array +-----------------------------------*/ +{ + for (int i = 0; i < 5; i++) + obst[i] = dist[i]; +} + +void set_battery(float voltage, float current, float remaining) +/* +/ update interface battery value array +-----------------------------------*/ +{ + batt[0] = voltage; + batt[1] = current; + batt[2] = remaining; +} + +int buzzuav_update_battery(buzzvm_t vm) +/* +/ update BVM battery table +-----------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); + buzzvm_pushf(vm, batt[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); + buzzvm_pushf(vm, batt[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); + buzzvm_pushi(vm, (int)batt[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +/* +/ Set of function to update interface variable of xbee network status +----------------------------------------------------------------------*/ +void set_deque_full(bool state) +{ + deque_full = state; +} + +void set_rssi(float value) +{ + rssi = round(value); +} + +void set_raw_packet_loss(float value) +{ + raw_packet_loss = value; +} + +void set_filtered_packet_loss(float value) +{ + filtered_packet_loss = round(100 * value); +} + +/*void set_api_rssi(float value) +{ + api_rssi = value; +}*/ + +int buzzuav_update_xbee_status(buzzvm_t vm) +/* +/ update BVM xbee_status table +-----------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1)); + buzzvm_pushi(vm, static_cast(deque_full)); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1)); + buzzvm_pushi(vm, rssi); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1)); + buzzvm_pushf(vm, raw_packet_loss); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1)); + buzzvm_pushi(vm, filtered_packet_loss); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1)); + buzzvm_pushf(vm, api_rssi); + buzzvm_tput(vm); + buzzvm_gstore(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 +-----------------------------------*/ +{ + 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) +{ + buzz_utility::Pos_struct pos_arr; + pos_arr.x = range; + pos_arr.y = bearing; + pos_arr.z = elevation; + map::iterator it = neighbors_map.find(id); + if (it != neighbors_map.end()) + neighbors_map.erase(it); + neighbors_map.insert(make_pair(id, pos_arr)); +} + +// update at each step the VM table +void update_neighbors(buzzvm_t vm) +{ + // Reset neighbor information + buzzneighbors_reset(vm); + // Get robot id and update neighbor information + map::iterator it; + for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it) + { + buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z); + } +} + +// Clear neighbours pos +void clear_neighbours_pos(){ + neighbors_map.clear(); +} + +int buzzuav_update_currentpos(buzzvm_t vm) +/* +/ Update the BVM position table +/------------------------------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "pose", 1)); + buzzvm_pusht(vm); + 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_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 0)); + buzzvm_pushf(vm, cur_pos[1]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + 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)); + 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); + // 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; +} + +void flight_status_update(uint8_t state) +/* +/ Update the interface status variable +/------------------------------------------------------*/ +{ + status = state; +} + +int buzzuav_update_flight_status(buzzvm_t vm) +/* +/ Create the generic robot table with status, remote controller current comand and destination +/ and current position of the robot +/------------------------------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); + buzzvm_pushi(vm, rc_cmd); + buzzvm_tput(vm); + buzzvm_dup(vm); + rc_cmd = 0; + buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); + buzzvm_pushi(vm, status); + buzzvm_tput(vm); + buzzvm_gstore(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_pushi(vm, rc_id); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +int buzzuav_update_prox(buzzvm_t vm) +/* +/ Create an obstacle Buzz table from proximity sensors +/ Acessing proximity in buzz script +/ proximity[0].angle and proximity[0].value - front +/ "" "" "" - right and back +/ proximity[3].angle and proximity[3].value - left +/ proximity[4].angle = -1 and proximity[4].value -bottom +-------------------------------------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); + buzzvm_pusht(vm); + buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); + buzzvm_gstore(vm); + + // Fill into the proximity table + buzzobj_t tProxRead; + float angle = 0; + for (size_t i = 0; i < 4; ++i) + { + // Create table for i-th read + buzzvm_pusht(vm); + tProxRead = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[i + 1]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + // Store read table in the proximity table + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, i); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + angle += 1.5708; + } + // Create table for bottom read + angle = -1; + buzzvm_pusht(vm); + tProxRead = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + // Store read table in the proximity table + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, 4); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + return vm->state; +} + +int dummy_closure(buzzvm_t vm) +/* +/ Dummy closure for use during update testing +----------------------------------------------------*/ +{ + return buzzvm_ret0(vm); +} +} diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp new file mode 100644 index 0000000..3816f55 --- /dev/null +++ b/src/rosbuzz.cpp @@ -0,0 +1,24 @@ +/** @file rosbuzz.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 "roscontroller.h" + +int main(int argc, char** argv) +/* + / This program implements Buzz in a ROS node using mavros_msgs. + -----------------------------------------------------------------*/ +{ + // Initialize rosBuzz node + ros::init(argc, argv, "rosBuzz"); + ros::NodeHandle nh_priv("~"); + ros::NodeHandle nh; + rosbuzz_node::roscontroller RosController(nh, nh_priv); + + RosController.RosControllerRun(); + return 0; +} diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp new file mode 100644 index 0000000..4e1f2b8 --- /dev/null +++ b/src/roscontroller.cpp @@ -0,0 +1,1517 @@ +/** @file roscontroller.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 "roscontroller.h" +#include +namespace rosbuzz_node +{ +const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; + +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): +logical_clock(ros::Time()), previous_step_time(ros::Time()) +/* +/ roscontroller class Constructor +---------------*/ +{ + ROS_INFO("Buzz_node"); + // Obtain parameters from ros parameter server + Rosparameters_get(n_c_priv); + // Initialize publishers, subscribers and client + Initialize_pub_sub(n_c); + // Compile the .bzz file to .basm, .bo and .bdbg + std::string fname = Compile_bzz(bzzfile_name); + bcfname = fname + ".bo"; + dbgfname = fname + ".bdb"; + 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); + armstate = 0; + multi_msg = true; + setpoint_counter = 0; + fcu_timeout = TIMEOUT; + cur_pos.longitude = 0; + cur_pos.latitude = 0; + cur_pos.altitude = 0; + + // set stream rate - wait for the FC to be started + SetStreamRate(0, 10, 1); + + // Get Home position - wait for none-zero value + while (cur_pos.latitude == 0.0f) + { + ROS_INFO("Waiting for GPS. "); + ros::Duration(0.5).sleep(); + ros::spinOnce(); + } + + // Get Robot Id - wait for Xbee to be started + // or else parse it from the launch file parameter + if (xbeeplugged) + { + GetRobotId(); + } + else + { + 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.fromSec(0); + logical_time_rate = 0; + time_sync_jumped = false; + out_msg_time=0; + // 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() +/* +/ roscontroller class Destructor +---------------------*/ +{ + // Destroy the BVM + buzz_utility::buzz_script_destroy(); + // Close the data logging file + log.close(); +} + +void roscontroller::GetRobotId() +/* +/ Get robot ID from the Xbee service +---------------------*/ +{ + mavros_msgs::ParamGet::Request robot_id_srv_request; + robot_id_srv_request.param_id = "id"; + mavros_msgs::ParamGet::Response robot_id_srv_response; + while (!xbeestatus_srv.call(robot_id_srv_request, robot_id_srv_response)) + { + ros::Duration(0.1).sleep(); + ROS_WARN("ROSBUZZ is waiting for Xbee device ID"); + } + + robot_id = robot_id_srv_response.value.integer; +} + +void roscontroller::send_MPpayload() +{ + MPpayload_pub.publish(buzzuav_closures::get_status()); +} + +void roscontroller::RosControllerRun() +/* +/rosbuzz_node main loop method +/--------------------------------------------------*/ +{ + // Set the Buzz bytecode + if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) + { + ROS_INFO("[%i] INIT DONE!!!", robot_id); + int packet_loss_tmp, time_step = 0; + double cur_packet_loss = 0; + ROS_INFO("[%i] Bytecode file found and set", robot_id); + std::string standby_bo = Compile_bzz(stand_by) + ".bo"; + // Intialize the update monitor + 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 + 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(); + state_publisher(); + grid_publisher(); + send_MPpayload(); + // Check updater state and step code + if(update) buzz_update::update_routine(); + if (time_step == BUZZRATE) + { + time_step = 0; + cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1))); + packet_loss_tmp = 0; + if (cur_packet_loss < 0) + cur_packet_loss = 0; + else if (cur_packet_loss > 1) + cur_packet_loss = 1; + } + else + { + packet_loss_tmp += (int)buzz_utility::get_inmsg_size(); + time_step++; + } + if (debug) + ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + + // log data + // hardware time now + 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; + } + 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<<(int)inmsgdata.size()<<","<< message_number<<","; + log<< out_msg_time<<","; + log < ros::Duration(1.0 / (float)BUZZRATE)) + ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, + loop_rate.cycleTime().toSec()); + // Safety land if the data coming from the flight controller are too old + if (fcu_timeout <= 0) + buzzuav_closures::rc_call(NAV_LAND); + else + fcu_timeout -= 1 / BUZZRATE; + timer_step += 1; + // Force a refresh on neighbors array once in a while + maintain_pos(timer_step); + // DEBUG + // std::cout<< "HOME: " << home.latitude << ", " << home.longitude; + } + } +} + +void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) +/* +/ Get all required parameters from the ROS launch file +/-------------------------------------------------------*/ +{ + // Obtain .bzz file name from launch file parameter + if (n_c.getParam("bzzfile_name", bzzfile_name)) + ; + else + { + ROS_ERROR("Provide a .bzz file to run in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain debug mode from launch file parameter + if (n_c.getParam("debug", debug)) + ; + else + { + ROS_ERROR("Provide a debug mode in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain rc service option from parameter server + if (n_c.getParam("rcclient", rcclient)) + { + if (rcclient == true) + { + if (!n_c.getParam("rcservice_name", rcservice_name)) + { + ROS_ERROR("Provide a name topic name for rc service in Launch file"); + system("rosnode kill rosbuzz_node"); + } + } + else if (rcclient == false) + { + ROS_INFO("RC service is disabled"); + } + } + else + { + ROS_ERROR("Provide a rc client option: yes or no in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain out payload name + n_c.getParam("out_payload", out_payload); + // Obtain in payload name + n_c.getParam("in_payload", in_payload); + // Obtain standby script to run during update + n_c.getParam("stand_by", stand_by); + + if (n_c.getParam("xbee_plugged", xbeeplugged)) + ; + else + { + ROS_ERROR("Provide the xbee plugged boolean in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (!xbeeplugged) + { + if (n_c.getParam("name", robot_name)) + ; + else + { + ROS_ERROR("Provide the xbee plugged boolean in Launch file"); + system("rosnode kill rosbuzz_node"); + } + } + else + n_c.getParam("xbee_status_srv", xbeesrv_name); + + if (!n_c.getParam("capture_image_srv", capture_srv_name)) + { + capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; + } + + GetSubscriptionParameters(n_c); +} + +void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) +/* +/Obtains publisher, subscriber and services from yml config file +/-----------------------------------------------------------------------------------*/ +{ + std::string topic; + if (node_handle.getParam("topics/gps", topic)) + ; + else + { + ROS_ERROR("Provide a gps topic in YAML file"); + system("rosnode kill rosbuzz_node"); + } + 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")); + + node_handle.getParam("topics/obstacles", topic); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/LaserScan")); + + node_handle.getParam("topics/battery", topic); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/BatteryState")); + + 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)) + ; + else + { + ROS_ERROR("Provide a fc client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/setpoint", setpoint_name)) + ; + else + { + ROS_ERROR("Provide a Set Point name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/armclient", armclient)) + ; + else + { + ROS_ERROR("Provide an arm client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/modeclient", modeclient)) + ; + else + { + ROS_ERROR("Provide a mode client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/stream", stream_client_name)) + ; + else + { + ROS_ERROR("Provide a mode client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } +} + +void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) +/* +/ Create all required subscribers, publishers and ROS service clients +/-------------------------------------------------------*/ +{ + // subscribers + + Subscribe(n_c); + + 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", MAX_NUMBER_OF_ROBOTS); + bvmstate_pub = n_c.advertise("bvmstate", 5); + grid_pub = n_c.advertise("grid", 5); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); + + // Service Clients + arm_client = n_c.serviceClient(armclient); + mode_client = n_c.serviceClient(modeclient); + mav_client = n_c.serviceClient(fcclient_name); + if (rcclient == true) + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); + ROS_INFO("Ready to receive Mav Commands from RC client"); + xbeestatus_srv = n_c.serviceClient(xbeesrv_name); + capture_srv = n_c.serviceClient(capture_srv_name); + stream_client = n_c.serviceClient(stream_client_name); + + multi_msg = true; +} + +void roscontroller::Subscribe(ros::NodeHandle& n_c) +/* +/ Robot independent subscribers +/--------------------------------------*/ +{ + for (std::map::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) + { + if (it->second == "mavros_msgs/ExtendedState") + { + flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); + } + else if (it->second == "mavros_msgs/State") + { + flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); + } + else if (it->second == "sensor_msgs/BatteryState") + { + battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); + } + else if (it->second == "sensor_msgs/NavSatFix") + { + 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::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; + } +} + +std::string roscontroller::Compile_bzz(std::string bzzfile_name) +/* +/ Create Buzz bytecode from the bzz script inputed +/-------------------------------------------------------*/ +{ + // Compile the buzz code .bzz to .bo + 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/"; + bzzfile_in_compile << " -b " << path << name << ".bo"; + bzzfile_in_compile << " -d " << path << name << ".bdb "; + bzzfile_in_compile << bzzfile_name; + + ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str()); + + system(bzzfile_in_compile.str().c_str()); + + return path + name; +} + +void roscontroller::neighbours_pos_publisher() +/* +/ Publish neighbours pos and id in neighbours pos topic +/----------------------------------------------------*/ +{ + auto current_time = ros::Time::now(); + map::iterator it; + rosbuzz::neigh_pos neigh_pos_array; + neigh_pos_array.header.frame_id = "/world"; + neigh_pos_array.header.stamp = current_time; + for (it = raw_neighbours_pos_map.begin(); it != raw_neighbours_pos_map.end(); ++it) + { + sensor_msgs::NavSatFix neigh_tmp; + neigh_tmp.header.stamp = current_time; + neigh_tmp.header.frame_id = "/world"; + neigh_tmp.position_covariance_type = it->first; // custom robot id storage + neigh_tmp.latitude = (it->second).x; + neigh_tmp.longitude = (it->second).y; + neigh_tmp.altitude = (it->second).z; + neigh_pos_array.pos_neigh.push_back(neigh_tmp); + // DEBUG + // cout<<"iterator it val: "<< it-> first << " After convertion: " + // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<> 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) { + // 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 = 0.01;//gridMap.getResolution(); + grid_msg.info.width = g_w; + grid_msg.info.height = g_h; + 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; + 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-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); + } +} + + +void roscontroller::Arm() +/* +/ Functions handling the local MAV ROS flight controller +/-------------------------------------------------------*/ +{ + mavros_msgs::CommandBool arming_message; + arming_message.request.value = armstate; + if (arm_client.call(arming_message)) + { + if (arming_message.response.success == 1) + ROS_WARN("FC Arm Service called!"); + else + ROS_WARN("FC Arm Service call failed!"); + } + else + { + ROS_WARN("FC Arm Service call failed!"); + } +} + +void roscontroller::prepare_msg_and_publish() +/* +/Prepare Buzz messages payload for each step and publish +/----------------------------------------------------------------------------------------------------- +/ Message format of payload (Each slot is uint64_t) +/ +/ +/| | | | +| | / +/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs +with size......... | / +/|_____|_____|_____|________________________________________________|______________________________| +/----------------------------------------------------------------------------------------------------*/ +{ + // get the payload to be sent + uint64_t* payload_out_ptr = buzz_utility::obt_out_msg(); + uint64_t position[3]; + // Appened current position to message + double tmp[3]; + tmp[0] = cur_pos.latitude; + tmp[1] = cur_pos.longitude; + tmp[2] = cur_pos.altitude; + memcpy(position, tmp, 3 * sizeof(uint64_t)); + mavros_msgs::Mavlink payload_out; + // Add Robot id and message number to the published message + if (message_number < 0) + message_number = 0; + else + message_number++; + + //header variables + uint16_t tmphead[4]; + tmphead[1] = (uint16_t)message_number; + get_logical_time(); + 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]; + 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; + memcpy(rheader, tmphead, sizeof(uint64_t)); + // push header into the buffer + payload_out.payload64.push_back(rheader[0]); + payload_out.payload64.push_back(position[0]); + 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 + payload_out.payload64.push_back(ros::Time::now().toNSec()); + payload_out.payload64.push_back(logical_clock.toNSec()); + //uint64_t ltrate64 = 0; + //memcpy((void*)<rate64, (void*)&logical_time_rate, sizeof(uint64_t)); + //payload_out.payload64.push_back(ltrate64); + } + else{ + // prepare rosbuzz msg header + 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]); + payload_out.payload64.push_back(position[0]); + payload_out.payload64.push_back(position[1]); + payload_out.payload64.push_back(position[2]); + } + // Append Buzz message + uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]); + for (int i = 0; i < out[0]; i++) + { + payload_out.payload64.push_back(payload_out_ptr[i]); + } + //Store out msg time + get_logical_time(); + out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec(); + // publish prepared messages in respective topic + payload_pub.publish(payload_out); + delete[] out; + delete[] payload_out_ptr; + /// 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*)(buzz_update::getupdate_out_msg_size()); + ; + int tot = 0; + mavros_msgs::Mavlink update_packets; + fprintf(stdout, "Appending code into message ...\n"); + fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize); + // allocate mem and clear it + buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize); + memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); + // Append updater msg size + *(uint16_t*)(buff_send + tot) = updater_msgSize; + tot += sizeof(uint16_t); + // Append updater msgs + memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize); + tot += updater_msgSize; + // Destroy the updater 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)); + free(buff_send); + // Send a constant number to differenciate updater msgs + update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE); + for (int i = 0; i < total_size; i++) + { + update_packets.payload64.push_back(payload_64[i]); + } + // Add Robot id and message number to the published message + if (message_number < 0) + message_number = 0; + else + message_number++; + update_packets.sysid = (uint8_t)robot_id; + update_packets.msgid = (uint32_t)message_number; + payload_pub.publish(update_packets); + delete[] payload_64; + } +} + +void roscontroller::flight_controller_service_call() +/* +/Flight controller service call every step if there is a command set from bzz +script +/-------------------------------------------------------------------------------*/ +{ + // flight controller client call if requested from Buzz + double* goto_pos; + float* gimbal; + switch (buzzuav_closures::bzz_cmd()) + { + case NAV_TAKEOFF: + goto_pos = buzzuav_closures::getgoto(); + cmd_srv.request.param7 = goto_pos[2]; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (!armstate) + { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + ros::Duration(0.5).sleep(); + // Registering HOME POINT. + home = cur_pos; + } + if (current_mode != "GUIDED") + SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo + 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 NAV_LAND: + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (current_mode != "LAND") + { + SetMode("LAND", 0); + armstate = 0; + Arm(); + } + 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"); + } + armstate = 0; + break; + + 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; + 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 COMPONENT_ARM_DISARM: + if (!armstate) + { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + } + break; + + case COMPONENT_ARM_DISARM+1: + if (armstate) + { + armstate = 0; + SetMode("LOITER", 0); + Arm(); + } + break; + + 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 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]; + cmd_srv.request.command = 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 IMAGE_START_CAPTURE: + 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; + } +} + +void roscontroller::maintain_pos(int tim_step) +/* +/Refresh neighbours Position for every ten 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; + } +} + +void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) +/* +/Puts neighbours position into local struct storage that is cleared every 10 +step +/---------------------------------------------------------------------------------*/ +{ + map::iterator it = neighbours_pos_map.find(id); + if (it != neighbours_pos_map.end()) + neighbours_pos_map.erase(it); + neighbours_pos_map.insert(make_pair(id, pos_arr)); +} + +void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) +/* +/Puts raw neighbours position into local storage for neighbours pos publisher +/-----------------------------------------------------------------------------------*/ +{ + map::iterator it = raw_neighbours_pos_map.find(id); + if (it != raw_neighbours_pos_map.end()) + raw_neighbours_pos_map.erase(it); + raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); +} + +void roscontroller::set_cur_pos(double latitude, double longitude, double altitude) +/* +/Set the current position of the robot callback +/--------------------------------------------------------*/ +{ + cur_pos.latitude = latitude; + cur_pos.longitude = longitude; + cur_pos.altitude = altitude; +} + +float roscontroller::constrainAngle(float x) +/* +/ Wrap the angle between -pi, pi +----------------------------------------------------------- */ +{ + x = fmod(x + M_PI, 2 * M_PI); + if (x < 0.0) + x += 2 * M_PI; + return x - M_PI; +} + +void roscontroller::gps_rb(POSE nei_pos, double out[]) +/* +/ Compute Range and Bearing of a neighbor in a local reference frame +/ from GPS coordinates +----------------------------------------------------------- */ +{ + 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[1] = atan2(ned_y, ned_x); + out[2] = 0.0; +} + +void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) +/* +/ Get GPS from NED and a reference GPS point (struct input) +----------------------------------------------------------- */ +{ + gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude); +} + +void roscontroller::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) +/* +/ Get GPS from NED and a reference GPS point +----------------------------------------------------------- */ +{ + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); +}; + +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->percentage); + // DEBUG + // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, + // msg->current, msg ->remaining); +} + +void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg) +/* +/Update flight extended status into BVM from subscriber for solos +/---------------------------------------------------------------------*/ +{ + // http://wiki.ros.org/mavros/CustomModes + std::cout << "Message: " << msg->mode << std::endl; + if (msg->mode == "GUIDED") + buzzuav_closures::flight_status_update(2); + else if (msg->mode == "LAND") + buzzuav_closures::flight_status_update(1); + else + buzzuav_closures::flight_status_update(7); // default to fit mavros::extended_state +} + +void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg) +/* +/Update flight extended status into BVM from subscriber +------------------------------------------------------------*/ +{ + buzzuav_closures::flight_status_update(msg->landed_state); +} + +void roscontroller::global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) +/* +/ Update current GPS position into BVM from subscriber +/-------------------------------------------------------------*/ +{ + // reset timeout counter + fcu_timeout = TIMEOUT; + 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& msg) +/* +/ Update current position for flight controller NED offset +/-------------------------------------------------------------*/ +{ + 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( + 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::rel_alt_callback(const std_msgs::Float64::ConstPtr& msg) +/* +/ Update altitude into BVM from subscriber +/-------------------------------------------------------------*/ +{ + // DEBUG + // ROS_INFO("Altitude in: %f", msg->data); + cur_pos.z = (double)msg->data; +} + +void roscontroller::obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg) +/* +/Set obstacle Obstacle distance table into BVM from subscriber +/-------------------------------------------------------------*/ +{ + float obst[5]; + for (int i = 0; i < 5; i++) + obst[i] = msg->ranges[i]; + buzzuav_closures::set_obstacle_dist(obst); +} + +void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) +/* +/ Publisher to send the flight controller navigation commands in local coordinates +/-------------------------------------------------------------*/ +{ + // http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html + // http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned + + geometry_msgs::PoseStamped moveMsg; + moveMsg.header.stamp = ros::Time::now(); + moveMsg.header.seq = setpoint_counter++; + moveMsg.header.frame_id = 1; + + // DEBUG + // 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; + + 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) { + localsetpoint_nonraw_pub.publish(moveMsg); + } +} + +void roscontroller::SetMode(std::string mode, int delay_miliseconds) +/* +/ Use setmode service of the flight controller +/-------------------------------------------------------------*/ +{ + // wait if necessary + if (delay_miliseconds != 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds)); + } + // set mode + mavros_msgs::SetMode set_mode_message; + set_mode_message.request.base_mode = 0; + set_mode_message.request.custom_mode = mode; + current_mode = mode; + if (mode_client.call(set_mode_message)) + { + ; // DEBUG + // ROS_INFO("Set Mode Service call successful!"); + } + else + { + ROS_INFO("Set Mode Service call failed!"); + } +} + +void roscontroller::SetStreamRate(int id, int rate, int on_off) +/* +/ Set the streamrate for mavros publications +/-------------------------------------------------------------*/ +{ + mavros_msgs::StreamRate message; + message.request.stream_id = id; + message.request.message_rate = rate; + message.request.on_off = on_off; + + while (!stream_client.call(message)) + { + ROS_INFO("Set stream rate call failed!, trying again..."); + ros::Duration(0.1).sleep(); + } + ROS_WARN("Set stream rate call successful"); +} + +void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) +/* +/Push payload into BVM FIFO +/---------------------------------------------------------------------------------------- +/ Message format of payload (Each slot is uint64_t) +/ _________________________________________________________________________________________________ +/| | | | | + * | +/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs + * with size......... | +/|_____|_____|_____|________________________________________________|______________________________| + +-----------------------------------------------------------------------------------------------------*/ +{ + // decode msg header + 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; + // 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) + { + uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); + uint64_t message_obt[obt_msg_size]; + // Go throught the obtained payload + for (int i = 0; i < (int)msg->payload64.size(); i++) + { + message_obt[i] = (uint64_t)msg->payload64[i]; + } + + uint8_t* pl = (uint8_t*)malloc(obt_msg_size); + memset(pl, 0, obt_msg_size); + // 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 (unMsgSize > 0) + { + buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + buzz_update::code_message_inqueue_process(); + } + free(pl); + } + // BVM FIFO message + else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || + (int)mtype == (int)BUZZ_MESSAGE_NO_TIME) + { + uint64_t message_obt[msg->payload64.size() - 1]; + // Go throught the obtained payload + for (int i = 1; i < (int)msg->payload64.size(); i++) + { + message_obt[i - 1] = (uint64_t)msg->payload64[i]; + } + // Extract neighbours position from payload + double neighbours_pos_payload[3]; + 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]); + POSE nei_pos; + nei_pos.latitude = neighbours_pos_payload[0]; + nei_pos.longitude = neighbours_pos_payload[1]; + 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((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,ros::Time::now().toNSec()); + 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 + buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], + cvt_neighbours_pos_payload[2]); + // Put RID and pos + raw_neighbours_pos_put((int)out[1], raw_neigh_pos); + // 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){ + // 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)); + } + } +} + +bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res) +/* +/ Catch the ROS service call from a custom remote controller (Mission Planner) +/ and send the requested commands to Buzz +----------------------------------------------------------- */ +{ + int rc_cmd; + switch (req.command) + { + case NAV_TAKEOFF: + ROS_INFO("RC_call: TAKE OFF!!!!"); + rc_cmd = NAV_TAKEOFF; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case NAV_LAND: + ROS_INFO("RC_Call: LAND!!!!"); + rc_cmd = NAV_LAND; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case COMPONENT_ARM_DISARM: + rc_cmd = COMPONENT_ARM_DISARM; + armstate = req.param1; + if (armstate) + { + ROS_INFO("RC_Call: ARM!!!!"); + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + } + else + { + ROS_INFO("RC_Call: DISARM!!!!"); + buzzuav_closures::rc_call(rc_cmd + 1); + res.success = true; + } + break; + case NAV_RETURN_TO_LAUNCH: + ROS_INFO("RC_Call: GO HOME!!!!"); + rc_cmd = NAV_RETURN_TO_LAUNCH; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case NAV_WAYPOINT: + ROS_INFO("RC_Call: GO TO!!!! "); + buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); + rc_cmd = NAV_WAYPOINT; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case DO_MOUNT_CONTROL: + ROS_INFO("RC_Call: Gimbal!!!! "); + buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); + rc_cmd = DO_MOUNT_CONTROL; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_REQUEST_UPDATE: + rc_cmd = CMD_REQUEST_UPDATE; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_SYNC_CLOCK: + rc_cmd = CMD_SYNC_CLOCK; + buzzuav_closures::rc_call(rc_cmd); + ROS_INFO("<---------------- Time Sync requested ----------->"); + res.success = true; + break; + default: + buzzuav_closures::rc_call(req.command); + ROS_ERROR("----> Received unregistered command: ", req.command); + res.success = true; + break; + } + return true; +} + +void roscontroller::get_number_of_robots() +/* +/ Garbage collector for the number of robots in the swarm +--------------------------------------------------------------------------*/ +{ + int cur_robots = buzz_utility::get_swarmsize(); + if (no_of_robots == 0) + { + no_of_robots = cur_robots; + } + else + { + if (no_of_robots != cur_robots && no_cnt == 0) + { + no_cnt++; + old_val = cur_robots; + } + else if (no_cnt != 0 && old_val == cur_robots) + { + no_cnt++; + if (no_cnt >= 150 || cur_robots > no_of_robots) + { + no_of_robots = cur_robots; + no_cnt = 0; + } + } + else + { + no_cnt = 0; + } + } +} + +/* +/ Set of functions to grab network quality data from Zbee service +--------------------------------------------------------------------------*/ +bool roscontroller::GetDequeFull(bool& result) +{ + mavros_msgs::ParamGet::Request srv_request; + srv_request.param_id = "deque_full"; + mavros_msgs::ParamGet::Response srv_response; + + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = (srv_response.value.integer == 1) ? true : false; + return srv_response.success; +} + +bool roscontroller::GetRssi(float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + srv_request.param_id = "rssi"; + mavros_msgs::ParamGet::Response srv_response; + + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::TriggerAPIRssi(const uint8_t short_id) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "trig_rssi_api_avg"; + } + else + { + srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + return srv_response.success; +} + +bool roscontroller::GetAPIRssi(const uint8_t short_id, float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "get_rssi_api_avg"; + } + else + { + srv_request.param_id = "get_rssi_api_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "pl_raw_avg"; + } + else + { + srv_request.param_id = "pl_raw_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "pl_filtered_avg"; + } + else + { + srv_request.param_id = "pl_filtered_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +void roscontroller::get_xbee_status() +/* + * Call all the xbee node services and update the xbee status + ------------------------------------------------------------------ */ +{ + bool result_bool; + float result_float; + const uint8_t all_ids = 0xFF; + if (GetDequeFull(result_bool)) + { + buzzuav_closures::set_deque_full(result_bool); + } + if (GetRssi(result_float)) + { + buzzuav_closures::set_rssi(result_float); + } + if (GetRawPacketLoss(all_ids, result_float)) + { + buzzuav_closures::set_raw_packet_loss(result_float); + } + if (GetFilteredPacketLoss(all_ids, result_float)) + { + buzzuav_closures::set_filtered_packet_loss(result_float); + } + // This part needs testing since it can overload the xbee module + /* + * if(GetAPIRssi(all_ids, result_float)) + * { + * buzzuav_closures::set_api_rssi(result_float); + * } + * 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(); ) + { + 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){ + 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); + if(offsetCount>0 && !time_sync_jumped){ + int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); + if(correction < TIME_SYNC_JUMP_THR && correction > 0 ){ + set_logical_time_correction(correction); + } + } + 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 + ------------------------------------------------------------------ */ +{ + 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; + 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", + deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); + neighbours_time_map.erase(it); + } + int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); + if(offset > TIME_SYNC_JUMP_THR){ + set_logical_time_correction(offset);// + time_diff * logical_time_rate ); + time_sync_jumped = true; + } + 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)); +} + + 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); + } +}