generalized some parts of RRT and complete export occ_grid
This commit is contained in:
dave
parent
94ad933452
commit
701991adc2
|
|
@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
std_msgs
|
||||
mavros_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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<int, std::map<int,int>> getgrid();
|
||||
/*
|
||||
* returns the current Buzz state
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <std_msgs/UInt8.h>
|
||||
#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();
|
||||
|
||||
|
|
|
|||
|
|
@ -48,6 +48,8 @@
|
|||
<run_depend>mavros_msgs</run_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@ std::map<int, buzz_utility::RB_struct> targets_map;
|
|||
std::map<int, buzz_utility::RB_struct> wplist_map;
|
||||
std::map<int, buzz_utility::Pos_struct> neighbors_map;
|
||||
std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
|
||||
std::map<int, std::map<int,int>> 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<float> 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<int, int> 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<int,int>(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
|
||||
buzzvm_pop(vm);
|
||||
}
|
||||
grid.insert(std::pair<int,std::map<int, int>>(i,row));
|
||||
buzzvm_pop(vm);
|
||||
}
|
||||
return buzzvm_ret0(vm);
|
||||
|
|
@ -457,6 +456,14 @@ double* getgoto()
|
|||
return goto_pos;
|
||||
}
|
||||
|
||||
std::map<int, std::map<int,int>> getgrid()
|
||||
/*
|
||||
/ return the grid
|
||||
/-------------------------------------------------------------*/
|
||||
{
|
||||
return grid;
|
||||
}
|
||||
|
||||
float* getgimbal()
|
||||
/*
|
||||
/ return current gimbal commands
|
||||
|
|
|
|||
|
|
@ -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<mavros_msgs::Mavlink>("fleet_status", 5);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
|
||||
uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
|
||||
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
|
||||
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(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<int, std::map<int,int>> grid = buzzuav_closures::getgrid();
|
||||
std::map<int, std::map<int,int>>::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<int,int>::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
|
||||
|
|
|
|||
Loading…
Reference in New Issue