added popup on waypoint, batt level yellow color, fix gps location for pangeae and enhanced robustness to packet lost
This commit is contained in:
dave
parent
fd7490b6c9
commit
4fbdb07ce4
|
|
@ -22,18 +22,19 @@ function barrier_create() {
|
|||
timeW = 1
|
||||
# create barrier vstig
|
||||
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
|
||||
if(barrier!=nil) {
|
||||
barrier=nil
|
||||
if(hvs) {
|
||||
BARRIER_VSTIG = BARRIER_VSTIG -1
|
||||
hvs = 0
|
||||
}else{
|
||||
BARRIER_VSTIG = BARRIER_VSTIG +1
|
||||
hvs = 1
|
||||
}
|
||||
}
|
||||
#if(barrier!=nil) {
|
||||
# barrier=nil
|
||||
#if(hvs) {
|
||||
# BARRIER_VSTIG = BARRIER_VSTIG -1
|
||||
# hvs = 0
|
||||
#}else{
|
||||
# BARRIER_VSTIG = BARRIER_VSTIG +1
|
||||
# hvs = 1
|
||||
#}
|
||||
#}
|
||||
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
if(barrier==nil)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
function barrier_set(threshold, transf, resumef, bc) {
|
||||
|
|
@ -51,6 +52,7 @@ function barrier_ready(bc) {
|
|||
#log("BARRIER READY -------")
|
||||
barrier.put(id, bc)
|
||||
barrier.put("d", 0)
|
||||
barrier.put("n", 1)
|
||||
}
|
||||
|
||||
#
|
||||
|
|
@ -61,22 +63,33 @@ function barrier_wait(threshold, transf, resumef, bc) {
|
|||
barrier_create()
|
||||
|
||||
barrier.put(id, bc)
|
||||
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
|
||||
#if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
|
||||
if(barrier.get("n")<threshold)
|
||||
barrier.put("n", threshold)
|
||||
if(threshold>1) {
|
||||
neighbors.foreach(function (nei,data){
|
||||
barrier.get(nei)
|
||||
})
|
||||
}
|
||||
|
||||
# Not to miss any RC inputs, for waypoints, potential and deploy states
|
||||
check_rc_wp()
|
||||
|
||||
#log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
|
||||
if(barrier.size()-2 >= barrier.get("n")) {
|
||||
if(barrier_allgood(barrier,bc)) {
|
||||
barrier.put("d", 1)
|
||||
timeW = 0
|
||||
BVMSTATE = transf
|
||||
} else
|
||||
barrier.put("d", 0)
|
||||
#}
|
||||
}
|
||||
|
||||
if(timeW >= BARRIER_TIMEOUT) {
|
||||
log("------> Barrier Timeout !!!!")
|
||||
barrier = nil
|
||||
#barrier = nil
|
||||
timeW = 0
|
||||
BVMSTATE = resumef
|
||||
} else if(timeW % 100 == 0 and bc > 0)
|
||||
} else if(timeW % 10 == 0 and bc > 0)
|
||||
neighbors.broadcast("cmd", bc)
|
||||
|
||||
timeW = timeW+1
|
||||
|
|
@ -86,11 +99,12 @@ function barrier_wait(threshold, transf, resumef, bc) {
|
|||
function barrier_allgood(barrier, bc) {
|
||||
barriergood = 1
|
||||
barrier.foreach(function(key, value, robot){
|
||||
barrier.get(key)
|
||||
#log("VS entry : ", key, " ", value, " ", robot)
|
||||
if(key == "d"){
|
||||
if(value == 1)
|
||||
return 1
|
||||
} else if(value != bc)
|
||||
} else if(key!="n" and value != bc)
|
||||
barriergood = 0
|
||||
})
|
||||
return barriergood
|
||||
|
|
|
|||
|
|
@ -1,12 +1,17 @@
|
|||
GOTO_MAXVEL = 1.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTO_MAXDIST = 250 # m.
|
||||
GOTODIST_TOL = 0.4 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
|
||||
GPSlimit_CEPSUM = {.1={.lat=45.510400, .lng=-73.610421},
|
||||
.2={.lat=45.510896, .lng=-73.608731},
|
||||
.3={.lat=45.510355, .lng=-73.608404},
|
||||
.4={.lat=45.509840, .lng=-73.610072}}
|
||||
|
||||
GPSlimit_PANGEAE = {.1={.lat=29.020871, .lng=-13.712477},
|
||||
.2={.lat=29.019850, .lng=-13.712378},
|
||||
.3={.lat=29.019875, .lng=-13.710096},
|
||||
.4={.lat=29.021245, .lng=-13.710184}}
|
||||
|
||||
# Core naviguation function to travel to a GPS target location.
|
||||
function goto_gps(transf) {
|
||||
m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
|
||||
|
|
@ -17,7 +22,7 @@ function goto_gps(transf) {
|
|||
transf()
|
||||
} else {
|
||||
m_navigation = LimitSpeed(m_navigation, 1.0)
|
||||
gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)}
|
||||
gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit_PANGEAE[1].lat, GPSlimit_PANGEAE[1].lng, 0), .2=vec_from_gps(GPSlimit_PANGEAE[2].lat, GPSlimit_PANGEAE[2].lng, 0), .3=vec_from_gps(GPSlimit_PANGEAE[3].lat, GPSlimit_PANGEAE[3].lng, 0), .4=vec_from_gps(GPSlimit_PANGEAE[4].lat, GPSlimit_PANGEAE[4].lng, 0)}
|
||||
geofence(gf)
|
||||
#m_navigation = LCA(m_navigation)
|
||||
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
|
||||
|
|
|
|||
|
|
@ -43,14 +43,14 @@ function rc_cmd_listen() {
|
|||
} else if (flight.rc_cmd==901){
|
||||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
resetWP()
|
||||
check_rc_wp()
|
||||
barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
|
||||
barrier_ready(901)
|
||||
neighbors.broadcast("cmd", 901)
|
||||
} else if (flight.rc_cmd==902){
|
||||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
resetWP()
|
||||
check_rc_wp()
|
||||
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
|
||||
barrier_ready(902)
|
||||
neighbors.broadcast("cmd", 902)
|
||||
|
|
@ -58,12 +58,14 @@ function rc_cmd_listen() {
|
|||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
resetWP()
|
||||
check_rc_wp()
|
||||
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
|
||||
barrier_ready(903)
|
||||
neighbors.broadcast("cmd", 903)
|
||||
} else if (flight.rc_cmd==904){
|
||||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
resetWP()
|
||||
barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
|
||||
barrier_ready(904)
|
||||
neighbors.broadcast("cmd", 904)
|
||||
|
|
@ -90,7 +92,8 @@ function nei_cmd_listen() {
|
|||
} else {
|
||||
if(value==20) {
|
||||
AUTO_LAUNCH_STATE = "IDLE"
|
||||
BVMSTATE = "GOHOME"
|
||||
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
|
||||
barrier_ready(20)
|
||||
} else if(value==21 ) {
|
||||
BVMSTATE = "STOP"
|
||||
#neighbors.broadcast("cmd", 777)
|
||||
|
|
@ -150,7 +153,7 @@ function check_rc_wp() {
|
|||
WPtab[rc_goto.id]=ls
|
||||
}
|
||||
v_wp.put(WPtab_id,WPtab)
|
||||
} else
|
||||
} else # Individual waypoint
|
||||
v_wp.put(rc_goto.id,ls)
|
||||
reset_rc()
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ function idle() {
|
|||
# Core state function to launch the robot: takeoff and wait for others, or stop (land)
|
||||
function launch() {
|
||||
BVMSTATE = "LAUNCH"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
log("AUTO_LAUNCH_STATE: ", AUTO_LAUNCH_STATE)
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
||||
homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
|
||||
|
|
@ -43,7 +44,6 @@ function launch() {
|
|||
barrier_ready(22)
|
||||
} else {
|
||||
log("Altitude: ", pose.position.altitude)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
} else {
|
||||
|
|
@ -85,14 +85,15 @@ function goinghome() {
|
|||
#m_navigation = LCA(m_navigation)
|
||||
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
|
||||
}
|
||||
}
|
||||
} else
|
||||
BVMSTATE = AUTO_LAUNCH_STATE
|
||||
}
|
||||
|
||||
# Core state function to stop and land.
|
||||
function stop() {
|
||||
BVMSTATE = "STOP"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
if(pose.position.altitude <= 0.2) {
|
||||
BVMSTATE = "STOP"
|
||||
|
|
@ -118,8 +119,8 @@ function indiWP() {
|
|||
wp = unpackWP2i(wpi)
|
||||
if(wp.pro == 0) {
|
||||
wpreached = 0
|
||||
storegoal(wp.lat, wp.lon, pose.position.altitude)
|
||||
var ls = packWP2i(wp.lat, wp.lon, 1)
|
||||
storegoal(wp.lat, wp.lng, pose.position.altitude)
|
||||
var ls = packWP2i(wp.lat, wp.lng, 1)
|
||||
v_wp.put(id,ls)
|
||||
return
|
||||
}
|
||||
|
|
@ -257,11 +258,13 @@ function voronoicentroid() {
|
|||
BVMSTATE="DEPLOY"
|
||||
check_rc_wp()
|
||||
wptab = v_wp.get(WPtab_id)
|
||||
if(wptab==nil or V_TYPE == 2)
|
||||
if(wptab==nil)
|
||||
return
|
||||
else if(not(size(wptab) > 0))
|
||||
return
|
||||
log("WP table size:", size(wptab))
|
||||
if(V_TYPE == 2)
|
||||
return
|
||||
it_pts = 0
|
||||
att = {}
|
||||
foreach(wptab, function(key, value){
|
||||
|
|
@ -269,7 +272,7 @@ function voronoicentroid() {
|
|||
if(key > 99)
|
||||
log("Nothing planed for the repulsors yet....")
|
||||
else if(key > 49)
|
||||
att[it_pts]=vec_from_gps(wp.lat, wp.lon, 0)
|
||||
att[it_pts]=vec_from_gps(wp.lat, wp.lng, 0)
|
||||
it_pts = it_pts + 1
|
||||
})
|
||||
# Boundaries from Geofence
|
||||
|
|
|
|||
|
|
@ -62,16 +62,17 @@ function gps_from_vec(vec) {
|
|||
return Lgoal
|
||||
}
|
||||
|
||||
GPSoffset = {.lat=45.50, .lon=-73.61}
|
||||
GPSoffset = {.lat=45.50, .lng=-73.61}
|
||||
GPSoffsetL = {.lat=29.01, .lng=-13.70}
|
||||
|
||||
function packWP2i(in_lat, in_long, processed) {
|
||||
var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
|
||||
var dlon = math.round((in_long - GPSoffset.lon)*1000000)
|
||||
var dlat = math.round((in_lat - GPSoffsetL.lat)*1000000)
|
||||
var dlon = math.round((in_long - GPSoffsetL.lng)*1000000)
|
||||
return {.dla=dlat, .dlo=dlon*10+processed}
|
||||
}
|
||||
|
||||
function unpackWP2i(wp_int){
|
||||
var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0
|
||||
var pro = wp_int.dlo-dlon*10
|
||||
return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro}
|
||||
return {.lat=wp_int.dla/1000000.0+GPSoffsetL.lat, .lng=dlon/1000000.0+GPSoffsetL.lng, .pro=pro}
|
||||
}
|
||||
|
|
@ -289,8 +289,8 @@ bool onSegment(Point p, Point q, Point r)
|
|||
// 2 --> Counterclockwise
|
||||
int orientation(Point p, Point q, Point r)
|
||||
{
|
||||
int val = (q.y - p.y) * (r.x - q.x) -
|
||||
(q.x - p.x) * (r.y - q.y);
|
||||
int val =round((q.y - p.y) * (r.x - q.x)*100 -
|
||||
(q.x - p.x) * (r.y - q.y)*100);
|
||||
|
||||
if (val == 0) return 0; // colinear
|
||||
return (val > 0)? 1: 2; // clock or counterclock wise
|
||||
|
|
@ -305,8 +305,10 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
|
|||
int o2 = orientation(p1, q1, q2);
|
||||
int o3 = orientation(p2, q2, p1);
|
||||
int o4 = orientation(p2, q2, q1);
|
||||
|
||||
//ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4);
|
||||
|
||||
// General case
|
||||
// General case
|
||||
if (o1 != o2 && o3 != o4)
|
||||
return true;
|
||||
|
||||
|
|
@ -341,92 +343,6 @@ void sortclose_polygon(vector <Point> *P){
|
|||
P->push_back((*P)[0]);
|
||||
}
|
||||
|
||||
int buzzuav_geofence(buzzvm_t vm)
|
||||
{
|
||||
bool onedge = false;
|
||||
Point P;
|
||||
Point V[4];
|
||||
int tmp;
|
||||
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);
|
||||
|
||||
if(buzzdict_size(t->t.value) != 5) {
|
||||
ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_tget(vm);
|
||||
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
|
||||
buzzvm_tget(vm);
|
||||
tmp = buzzvm_stack_at(vm, 1)->f.value;
|
||||
//ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
|
||||
if(i==0)
|
||||
P.x = tmp;
|
||||
else
|
||||
V[i-1].x = tmp;
|
||||
buzzvm_pop(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
|
||||
buzzvm_tget(vm);
|
||||
tmp = buzzvm_stack_at(vm, 1)->f.value;
|
||||
//ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
|
||||
if(i==0)
|
||||
P.y = tmp;
|
||||
else
|
||||
V[i-1].y = tmp;
|
||||
buzzvm_pop(vm);
|
||||
|
||||
buzzvm_pop(vm);
|
||||
}
|
||||
// TODO: use vector
|
||||
//sortclose_polygon(&V);
|
||||
|
||||
// simple polygon: rectangle, 4 points
|
||||
int n = 4;
|
||||
// Create a point for line segment from p to infinite
|
||||
Point extreme = {10000, P.y};
|
||||
|
||||
// Count intersections of the above line with sides of polygon
|
||||
int count = 0, i = 0;
|
||||
do
|
||||
{
|
||||
int next = (i+1)%n;
|
||||
|
||||
// Check if the line segment from 'p' to 'extreme' intersects
|
||||
// with the line segment from 'polygon[i]' to 'polygon[next]'
|
||||
if (doIntersect(V[i], V[next], P, extreme))
|
||||
{
|
||||
// If the point 'p' is colinear with line segment 'i-next',
|
||||
// then check if it lies on segment. If it lies, return true,
|
||||
// otherwise false
|
||||
if (orientation(V[i], P, V[next]) == 0) {
|
||||
onedge = onSegment(V[i], P, V[next]);
|
||||
if(onedge)
|
||||
break;
|
||||
}
|
||||
|
||||
count++;
|
||||
}
|
||||
i = next;
|
||||
} while (i != 0);
|
||||
|
||||
//ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
|
||||
|
||||
if((count%2 == 0) || onedge) {
|
||||
goto_gpsgoal[0] = cur_pos[0];
|
||||
goto_gpsgoal[1] = cur_pos[1];
|
||||
ROS_WARN("Geofencing trigered, not going any further!");
|
||||
}
|
||||
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
float pol_area(vector <Point> vert) {
|
||||
float a = 0.0;
|
||||
|
|
@ -487,9 +403,12 @@ void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
|
|||
double r = numerator( S, (*itc), (*itc), (*next) ) / d;
|
||||
double s = numerator( S, (*itc), S, D ) / d;
|
||||
|
||||
//ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
|
||||
(*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
|
||||
}
|
||||
}
|
||||
if(parallel || collinear)
|
||||
ROS_WARN("Lines are Collinear (%d) or Parallels (%d)",collinear,parallel);
|
||||
}
|
||||
|
||||
bool isSiteout(Point S, std::vector <Point> Poly) {
|
||||
|
|
@ -524,6 +443,71 @@ bool isSiteout(Point S, std::vector <Point> Poly) {
|
|||
return ((count%2 == 0) && !onedge);
|
||||
}
|
||||
|
||||
int buzzuav_geofence(buzzvm_t vm)
|
||||
{
|
||||
Point P;
|
||||
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);
|
||||
|
||||
if(buzzdict_size(t->t.value) < 5) {
|
||||
ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
std::vector <Point> polygon_bound;
|
||||
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
|
||||
Point tmp;
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_tget(vm);
|
||||
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
|
||||
buzzvm_tget(vm);
|
||||
if(i==0){
|
||||
P.x = buzzvm_stack_at(vm, 1)->f.value;
|
||||
//printf("px=%f\n",P.x);
|
||||
}else{
|
||||
tmp.x = buzzvm_stack_at(vm, 1)->f.value;
|
||||
//printf("c%dx=%f\n",i,tmp.x);
|
||||
}
|
||||
buzzvm_pop(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
|
||||
buzzvm_tget(vm);
|
||||
//ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
|
||||
if(i==0){
|
||||
P.y = buzzvm_stack_at(vm, 1)->f.value;
|
||||
//printf("py=%f\n",P.y);
|
||||
}else{
|
||||
tmp.y = buzzvm_stack_at(vm, 1)->f.value;
|
||||
//printf("c%dy=%f\n",i,tmp.y);
|
||||
}
|
||||
buzzvm_pop(vm);
|
||||
|
||||
if(i!=0)
|
||||
polygon_bound.push_back(tmp);
|
||||
|
||||
buzzvm_pop(vm);
|
||||
}
|
||||
sortclose_polygon(&polygon_bound);
|
||||
|
||||
// Check if we are in the zone
|
||||
if(isSiteout(P, polygon_bound)){
|
||||
Point Intersection;
|
||||
getintersection(Point(0.0, 0.0) , P, polygon_bound, &Intersection);
|
||||
double gps[3];
|
||||
double d[2]={Intersection.x,Intersection.y};
|
||||
gps_from_vec(d, gps);
|
||||
set_gpsgoal(gps);
|
||||
ROS_WARN("Geofencing trigered, not going any further (%f,%f)!",d[0],d[1]);
|
||||
}
|
||||
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int voronoi_center(buzzvm_t vm) {
|
||||
|
||||
float dist_max = 300;
|
||||
|
|
@ -565,23 +549,7 @@ int voronoi_center(buzzvm_t vm) {
|
|||
buzzvm_pop(vm);
|
||||
}
|
||||
sortclose_polygon(&polygon_bound);
|
||||
// Check if we are in the zone
|
||||
if(isSiteout(Point(0,0), polygon_bound)){
|
||||
//ROS_WARN("Not in the Zone!!!");
|
||||
double goal_tmp[2];
|
||||
do{
|
||||
goal_tmp[0] = polygon_bound[0].x + (rand()%100)/100.0*(polygon_bound[2].x- polygon_bound[0].x);
|
||||
goal_tmp[1] = polygon_bound[0].y + (rand()%100)/100.0*(polygon_bound[2].y- polygon_bound[0].y);
|
||||
//ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
|
||||
} while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound));
|
||||
ROS_WARN("Sending at a random location in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
|
||||
double gps[3];
|
||||
gps_from_vec(goal_tmp, gps);
|
||||
set_gpsgoal(gps);
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
||||
int count = buzzdict_size(t->t.value)-(Poly_vert+1);
|
||||
ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
|
||||
float *xValues = new float[count];
|
||||
|
|
@ -607,6 +575,22 @@ int voronoi_center(buzzvm_t vm) {
|
|||
|
||||
buzzvm_pop(vm);
|
||||
}
|
||||
|
||||
// Check if we are in the zone
|
||||
if(isSiteout(Point(0,0), polygon_bound) || count < 3) {
|
||||
//ROS_WARN("Not in the Zone!!!");
|
||||
double goal_tmp[2];
|
||||
do{
|
||||
goal_tmp[0] = polygon_bound[0].x + (rand()%100)/100.0*(polygon_bound[2].x- polygon_bound[0].x);
|
||||
goal_tmp[1] = polygon_bound[0].y + (rand()%100)/100.0*(polygon_bound[2].y- polygon_bound[0].y);
|
||||
//ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
|
||||
} while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound));
|
||||
ROS_WARN("Sending at a random location in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
|
||||
double gps[3];
|
||||
gps_from_vec(goal_tmp, gps);
|
||||
set_gpsgoal(gps);
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
VoronoiDiagramGenerator vdg;
|
||||
ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(),count);
|
||||
|
|
@ -920,7 +904,7 @@ void set_gpsgoal(double goal[3])
|
|||
{
|
||||
double rb[3];
|
||||
rb_from_gps(goal, rb, cur_pos);
|
||||
if (fabs(rb[0]) < 150.0) {
|
||||
if (fabs(rb[0]) < 250.0) {
|
||||
goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
|
||||
ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
|
||||
} else
|
||||
|
|
|
|||
Loading…
Reference in New Issue