From 701991adc2145568c8ec1cca38be26e10c5e7ba5 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 12:20:43 -0500 Subject: [PATCH] generalized some parts of RRT and complete export occ_grid --- CMakeLists.txt | 1 + buzz_scripts/include/rrtstar.bzz | 124 ++++++++++------------------- buzz_scripts/include/uavstates.bzz | 8 +- include/buzzuav_closures.h | 4 + include/roscontroller.h | 5 ++ package.xml | 2 + src/buzzuav_closures.cpp | 35 ++++---- src/roscontroller.cpp | 44 +++++++++- 8 files changed, 125 insertions(+), 98 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b7e4b0..672d68d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs mavros_msgs sensor_msgs + nav_msgs message_generation ) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index b84fec9..9777dde 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -6,54 +6,41 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 +GSCALE = {.x=1, .y=1} map = nil cur_cell = {} nei_cell = {} -function UAVinit_map(m_navigation) { +# create a map with its length to fit the goal (rel. pos.) and the current position as the center +function dyn_init_map(m_navigation) { # create a map big enough for the goal - init_map(2*math.round(math.vec2.length(m_navigation))+10) + init_map(math.round(2*math.vec2.length(m_navigation))+10) # center the robot on the grid cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) } -function UAVpathPlanner(m_navigation, m_pos) { +# start the RRT* to reach the goal (rel. pos.) +function pathPlanner(m_goal, m_pos, kh4) { # place the robot on the grid - cur_cell = getcell(m_pos,0) + cur_cell = getcell(m_pos) # create the goal in the map grid - mapgoal = math.vec2.add(m_navigation,cur_cell) - mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) + mapgoal = getcell(math.vec2.add(m_goal, m_pos)) + + #print_map(map) + #export_map(map) # search for a path - return RRTSTAR(mapgoal,math.vec2.new(5,5)) #size(map[1])/20.0,size(map[1])/20.0)) + return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) } -function Kh4pathPlanner(m_goal, m_pos) { - # move 0,0 to a corner instead of the center - m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) - - # place the robot on the grid - cur_cell = getcell(m_pos,1) - log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) - log("Going to cell: ", m_goal.x, " ", m_goal.y) - - # search for a path - print_map(map) - # print_map_argos(map) - return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) -} - -function getcell(m_curpos, kh4) { +function getcell(m_curpos) { var cell = {} - if(kh4) { # for khepera playground - cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y)) - } else { # for uav map relative to home - cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) - cell = math.vec2.add(cell, m_curpos) - cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) - } + # relative to center (start position) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y) + cell = math.vec2.add(cell, offset) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) + if(cell.x > size(map)) cell.x = size(map) if(cell.y > size(map[1])) @@ -72,7 +59,7 @@ function updateMap(m_pos) { } function UAVgetneiobs (m_curpos) { - cur_cell = getcell(m_curpos,0) + cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) @@ -81,7 +68,7 @@ function UAVgetneiobs (m_curpos) { function getneiobs (m_curpos) { var foundobstacle = 0 - cur_cell = getcell(m_curpos,1) + cur_cell = getcell(m_curpos) var old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -127,7 +114,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - cur_cell = getcell(m_curpos,1) + cur_cell = getcell(m_curpos) reduce(proximity, function(key, value, acc) { @@ -170,13 +157,13 @@ function getproxobs (m_curpos) { function check_path(m_path, goal_l, m_curpos, kh4) { var pathisblocked = 0 var nb = goal_l - cur_cell = getcell(m_curpos,kh4) + cur_cell = getcell(m_curpos) var Cvec = cur_cell while(nb <= size(m_path) and nb <= goal_l+1) { var Nvec = getvec(m_path, nb) if(kh4 == 0) { Nvec = vec_from_gps(Nvec.x,Nvec.y,1) - Nvec = getcell(Nvec,kh4) + Nvec = getcell(Nvec) } if(doesItIntersect(Cvec, Nvec)) { log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") @@ -225,7 +212,7 @@ function RRTSTAR(GOAL,TOL) { minCounter = 0; if(size(pointList) != 0) { - log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) + #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) ipt=1 while(ipt <= size(pointList)) { pointNumber = {} @@ -329,8 +316,7 @@ function RRTSTAR(GOAL,TOL) { } if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPathGPS(Q,numberOfPoints) - print_pos(Path) + Path = getPath(Q,numberOfPoints, 1) } else { log("FAILED TO FIND A PATH!!!") Path = nil @@ -424,62 +410,40 @@ function doesItIntersect(point,vector) { return 0 } -function getPathGPS(Q,nb){ +# create a table with only the path's points in order +function getPath(Q,nb,gps){ #log("-----------CONVERTING PATH!!!") - path={} + var pathL={} var npt=0 - var isrecursive = 0 # get the path from the point list - while(nb > 1 and isrecursive!=1){ + while(nb > 1) { npt=npt+1 - path[npt] = {} - path[npt][1]=Q[nb][1] - path[npt][2]=Q[nb][2] + pathL[npt] = {} + pathL[npt][1]=Q[nb][1] + pathL[npt][2]=Q[nb][2] if(nb != Q[Q[nb][3]][3]) nb = Q[nb][3]; else { - isrecursive = 1 - path={} log("ERROR - Recursive path !!!") + return nil } } - # re-order the list and make the path points absolute - pathR={} - var totpt = npt + 1 - while(npt > 0){ - tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) - pathR[totpt-npt] = {} - pathR[totpt-npt][1]=tmpgoal.latitude - pathR[totpt-npt][2]=tmpgoal.longitude - npt = npt - 1 - } - return pathR -} - -# create a table with only the path's points in order -function getPath(Q,nb){ - path={} - var npt=0 - - # log("get the path from the point list") - while(nb > 1){ - npt=npt+1 - path[npt] = {} - path[npt][1]=Q[nb][1] - path[npt][2]=Q[nb][2] - nb = Q[nb][3]; - } - # log("re-order the list") # table_print(path.mat) - pathR={} + var pathR={} var totpt = npt + 1 while(npt > 0){ - tmpgoal = getvec(path,npt) pathR[totpt-npt] = {} - pathR[totpt-npt][1]=tmpgoal.x - pathR[totpt-npt][2]=tmpgoal.y + if(gps) { + tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell)) + pathR[totpt-npt][1]=tmpgoal.latitude + pathR[totpt-npt][2]=tmpgoal.longitude + } else { + tmpgoal = getvec(path,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y + } npt = npt - 1 } #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 501416d..d3b015d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -104,15 +104,16 @@ function gotoWPRRT(transf) { if(Path==nil){ # create the map if(map==nil) { - UAVinit_map(rc_goal) + dyn_init_map(rc_goal) homegps.lat = position.latitude homegps.long = position.longitude } # add proximity sensor and neighbor obstacles to the map while(Path==nil) { updateMap(m_pos) - Path = UAVpathPlanner(rc_goal, m_pos) + Path = pathPlanner(rc_goal, m_pos) } + print_pos(Path) cur_goal_l = 1 } else if(cur_goal_l <= size(Path)) { var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude @@ -126,8 +127,9 @@ function gotoWPRRT(transf) { rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) while(Path == nil) { updateMap(m_pos) - Path = UAVpathPlanner(rc_goal, m_pos) + Path = pathPlanner(rc_goal, m_pos) } + print_pos(Path) cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 56f0bd6..f14e1a7 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -94,6 +94,10 @@ void set_currentpos(double latitude, double longitude, double altitude); * returns the current go to position */ double* getgoto(); +/* + * returns the current grid + */ +std::map> getgrid(); /* * returns the current Buzz state */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 17533a4..0e9e547 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" @@ -124,6 +125,7 @@ private: ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; ros::Publisher uavstate_pub; + ros::Publisher grid_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; @@ -179,6 +181,9 @@ private: /*UAVState publisher*/ void uavstate_publisher(); + /*Grid publisher*/ + void grid_publisher(); + /*BVM message payload publisher*/ void send_MPpayload(); diff --git a/package.xml b/package.xml index 6f4fc98..0fa6866 100644 --- a/package.xml +++ b/package.xml @@ -48,6 +48,8 @@ mavros_msgs sensor_msgs sensor_msgs + nav_msgs + nav_msgs message_generation message_runtime diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6b380d6..980948b 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -36,6 +36,7 @@ std::map targets_map; std::map wplist_map; std::map neighbors_map; std::map neighbors_status_map; +std::map> grid; /****************************************/ /****************************************/ @@ -178,26 +179,24 @@ int buzz_exportmap(buzzvm_t vm) / Buzz closure to export a 2D map /----------------------------------------*/ { - /* Make sure one parameter has been passed */ buzzvm_lnum_assert(vm, 1); - /* Get the parameter */ + // Get the parameter buzzvm_lload(vm, 1); - buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // matrix - /* Get the table */ + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzobj_t t = buzzvm_stack_at(vm, 1); - /* Copy the values into a vector */ - std::vector mat; - for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i) - { - /* Duplicate the table */ + for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) { buzzvm_dup(vm); - /* Push the index */ buzzvm_pushi(vm, i); - /* Get the value */ buzzvm_tget(vm); - /* Store it in the vector (assume all values are float, no mistake...) */ - mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value); - /* Get rid of the value, now useless */ + std::map row; + for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { + buzzvm_dup(vm); + buzzvm_pushi(vm, j); + buzzvm_tget(vm); + row.insert(std::pair(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + buzzvm_pop(vm); + } + grid.insert(std::pair>(i,row)); buzzvm_pop(vm); } return buzzvm_ret0(vm); @@ -457,6 +456,14 @@ double* getgoto() return goto_pos; } +std::map> getgrid() +/* +/ return the grid +/-------------------------------------------------------------*/ +{ + return grid; +} + float* getgimbal() /* / return current gimbal commands diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9fe34f9..c5a4354 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -114,8 +114,10 @@ void roscontroller::RosControllerRun() // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { - // Neighbours of the robot published with id in respective topic + // Publish topics neighbours_pos_publisher(); + uavstate_publisher(); + grid_publisher(); send_MPpayload(); // Check updater state and step code update_routine(); @@ -337,6 +339,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); uavstate_pub = n_c.advertise("uavstate", 5); + grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); // Service Clients @@ -447,6 +450,45 @@ void roscontroller::uavstate_publisher() uavstate_pub.publish(uavstate_msg); } +void roscontroller::grid_publisher() +/* +/ Publish current Grid from Buzz script +/----------------------------------------------------*/ +{ + std::map> grid = buzzuav_closures::getgrid(); + std::map>::iterator itr = grid.begin(); + int g_w = itr->second.size(); + int g_h = grid.size(); + + if(g_w!=0 && g_h!=0) { + auto current_time = ros::Time::now(); + nav_msgs::OccupancyGrid grid_msg; + grid_msg.header.frame_id = "/world"; + grid_msg.header.stamp = current_time; + grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. + //grid_msg.info.resolution = gridMap.getResolution(); + grid_msg.info.width = g_w; + grid_msg.info.height = g_h; + grid_msg.info.origin.position.x = 0.0; + grid_msg.info.origin.position.y = 0.0; + grid_msg.info.origin.position.z = 0.0; + grid_msg.info.origin.orientation.x = 0.0; + grid_msg.info.origin.orientation.y = 0.0; + grid_msg.info.origin.orientation.z = 0.0; + grid_msg.info.origin.orientation.w = 1.0; + grid_msg.data.resize(g_w*g_h); + + for (itr=grid.begin(); itr!=grid.end(); ++itr) { + std::map::iterator itc = itr->second.begin(); + for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { + grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0); + } + } + grid_pub.publish(grid_msg); + } +} + + void roscontroller::Arm() /* / Functions handling the local MAV ROS flight controller