void inflateBlacklist(GridMap& map, std::set<tf::Vector3> positions, int radius)
{
CHECK_RADIUS(radius);
static GridMask* grid_mask = NULL;
static int last_radius = -1;
if (!grid_mask || radius != last_radius)
{
delete grid_mask;
grid_mask = makeCircularMask(radius);
last_radius = radius;
}
ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell)
{
// If the cell isn't unknown, set it as unreachable.
if (cell == GridMap::UNKNOWN)
return cell;
else
return std::max(cell, mask_cell);
};
for (tf::Vector3 pos : positions)
{
index_t idx = map.getIdxForPosition(pos);
ROS_WARN_STREAM("Applying mask at idx=" << idx);
apply_mask_at_idx(map, *grid_mask, op, idx);
}
}
void
nas_start(nas_t *nas)
{
nas_sta_t *sta;
/* reset all STAs */
bzero((void *)nas->sta, sizeof(nas->sta));
bzero((void *)nas->sta_hashed, sizeof(nas->sta_hashed));
nas->MIC_failures = 0;
nas->MIC_countermeasures = 0;
nas->prev_MIC_error = 0;
/* start session count down timer */
if (CHECK_RADIUS(nas->mode))
nas_start_watchdog(nas);
/* initiate/request pairwise key exchange */
if (!(nas->flags & NAS_FLAG_WDS))
return;
sta = lookup_sta(nas, (struct ether_addr *)nas->remote,
SEARCH_ENTER);
if (!sta) {
#ifdef BCMDBG
char eabuf[ETHER_ADDR_STR_LEN];
#endif
dbg(nas, "sta %s not available", ether_etoa(nas->remote, eabuf));
return;
}
/* Assume the peer use the same cipher and akm */
if (CHECK_PSK(nas->mode)) {
sta->suppl.pmk_len = nas->wpa->pmk_len;
bcopy(nas->wpa->pmk, sta->suppl.pmk, nas->wpa->pmk_len);
}
/*
* There is no beacon/proberesp/assocreq across WDS, therefore
* we have no way to know what cipher and akm the peer is configured
* for. But assuming the peer uses the same configuration seems
* reasonable. No WEP when doing WPA over WDS.
*/
wpa_set_suppl(nas->wpa, sta, nas->mode, nas->wsec, CRYPTO_ALGO_OFF);
#ifdef BCMSUPPL
if (nas->flags & NAS_FLAG_SUPPLICANT)
wpa_request(nas->wpa, sta);
#endif
if (nas->flags & NAS_FLAG_AUTHENTICATOR)
wpa_start(nas->wpa, sta);
}
void freeRobotPose(GridMap& map, index_t idx, int radius)
{
CHECK_RADIUS(radius);
static GridMask* grid_mask = NULL;
static int last_radius = -1;
if (!grid_mask || radius != last_radius)
{
delete grid_mask;
grid_mask = makeCircularMask(radius);
last_radius = radius;
}
ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell)
{
// Mark the cell as known & free
return 0;
};
apply_mask_at_idx(map, *grid_mask, op, idx);
}
void inflateObstacles(GridMap& map, int radius, int8_t value)
{
CHECK_RADIUS(radius);
static GridMask* grid_mask = NULL;
static int last_radius = -1;
if (!grid_mask || radius != last_radius)
{
delete grid_mask;
grid_mask = makeCircularMask(radius, value);
last_radius = radius;
}
ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell)
{
return std::max(cell, mask_cell);
};
GridCellSelectorFunction pred = boost::bind(&GridMap::isObstacle, map, _2);
apply_mask_if(map, *grid_mask, op, pred);
}
static Bool
initlisa(ModeInfo * mi, lisas * loop)
{
lisacons *lc = &Lisa[MI_SCREEN(mi)];
lisafuncs **lf = loop->function;
XPoint *lp;
int phase, pctr, fctr, xctr, yctr, extra_points;
double xprod, yprod, xsum, ysum;
xMaxLines = (XMaxRequestSize(MI_DISPLAY(mi))-3)/2;
/* printf("Got xMaxLines = %d\n", xMaxLines); */
loop->nsteps = MI_CYCLES(mi);
if (loop->nsteps == 0)
loop->nsteps = 1;
if (MI_NPIXELS(mi) > 2) {
loop->color = STARTCOLOR;
loop->cstep = (loop->nsteps > MI_NPIXELS(mi)) ? loop->nsteps / MI_NPIXELS(mi) : 1;
} else {
loop->color = MI_WHITE_PIXEL(mi);
loop->cstep = 0;
}
extra_points = loop->cstep - (loop->nsteps % loop->cstep);
lc->maxcycles = (MAXCYCLES * loop->nsteps) - 1;
loop->cstep = ( loop->nsteps > MI_NPIXELS(mi) ) ? loop->nsteps / MI_NPIXELS(mi) : 1;
/* printf("Got cstep = %d\n", loop->cstep); */
loop->melting = 0;
loop->nfuncs = 1;
loop->pistep = 2.0 * M_PI / (double) loop->nsteps;
loop->center.x = lc->width / 2;
loop->center.y = lc->height / 2;
loop->radius = (int) MI_SIZE(mi);
CHECK_RADIUS(loop, lc);
loop->dx = NRAND(XVMAX);
loop->dy = NRAND(YVMAX);
loop->dx++;
loop->dy++;
#if defined STARTFUNC
lf[0] = &Function[STARTFUNC];
#else /* STARTFUNC */
lf[0] = &Function[NRAND(NUMSTDFUNCS)];
#endif /* STARTFUNC */
if ((lp = loop->lastpoint = (XPoint *)
calloc(loop->nsteps+extra_points, sizeof (XPoint))) == NULL) {
free_lisa(lc);
return False;
}
phase = lc->loopcount % loop->nsteps;
#if defined DEBUG
printf( "nsteps = %d\tcstep = %d\tmrs = %d\textra_points = %d\n",
loop->nsteps, loop->cstep, xMaxLines, extra_points );
PRINT_FUNC(lf[0]);
#endif /* DEBUG */
for (pctr = 0; pctr < loop->nsteps; pctr++) {
loop->phi = (double) (pctr - phase) * loop->pistep;
loop->theta = (double) (pctr + phase) * loop->pistep;
fctr = loop->nfuncs;
xsum = ysum = 0.0;
while (fctr--) {
xprod = yprod = (double) loop->radius;
xctr = lf[fctr]->nx;
yctr = lf[fctr]->ny;
while (xctr--)
xprod *= sin(lf[fctr]->xcoeff[xctr] * loop->theta);
while (yctr--)
yprod *= sin(lf[fctr]->ycoeff[yctr] * loop->phi);
xsum += xprod;
ysum += yprod;
}
if (loop->nfuncs > 1) {
xsum /= 2.0;
ysum /= 2.0;
}
xsum += (double) loop->center.x;
ysum += (double) loop->center.y;
lp[pctr].x = (int) ceil(xsum);
lp[pctr].y = (int) ceil(ysum);
}
/* this fills in the extra points, so we can use segment-drawing calls */
for (pctr = loop->nsteps; pctr < loop->nsteps + extra_points; pctr++) {
lp[pctr].x=lp[pctr - loop->nsteps].x;
lp[pctr].y=lp[pctr - loop->nsteps].y;
}
#if defined DRAWLINES
loop->linewidth = LINEWIDTH; /* #### make this a resource */
if (loop->linewidth == 0)
loop->linewidth = 1;
if (loop->linewidth < 0)
loop->linewidth = NRAND(-loop->linewidth) + 1;
XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), loop->linewidth,
LINESTYLE, LINECAP, LINEJOIN);
#endif /* DRAWLINES */
if ( loop->cstep < xMaxLines ) {
/* we can send each color segment in a single request
* because the max request length is long enough
* and because we have padded out the array to have extra elements
* to support calls which would otherwise fall off the end*/
for (pctr = 0; pctr < loop->nsteps; pctr+=loop->cstep) {
/* Set the color */
SET_COLOR();
#if defined DRAWLINES
XDrawLines(MI_DISPLAY(mi), MI_WINDOW(mi),
MI_GC(mi), &lp[pctr], loop->cstep, CoordModeOrigin );
#else /* DRAWLINES */
XDrawPoints(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi),
&lp[pctr], loop->cstep, CoordModeOrigin );
#endif /* DRAWLINES */
}
} else { /* do it one by one as before */
for (pctr = 0; pctr < loop->nsteps; pctr++ ) {
SET_COLOR();
#if defined DRAWLINES
XDrawLine(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi),
lp[pctr].x, lp[pctr].y,
lp[pctr+1 % loop->nsteps].x, lp[pctr+1 % loop->nsteps].y);
#else /* DRAWLINES */
XDrawPoint(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi),
lp[pctr].x, lp[pctr].y);
#endif /* DRAWLINES */
}
}
#if defined DRAWLINES
XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), 1,
LINESTYLE, LINECAP, LINEJOIN);
#endif /* DRAWLINES */
return True;
}
static Bool
drawlisa(ModeInfo * mi, lisas * loop)
{
XPoint *np;
XPoint *lp = loop->lastpoint;
lisacons *lc = &Lisa[MI_SCREEN(mi)];
lisafuncs **lf = loop->function;
int phase = lc->loopcount % loop->nsteps;
int pctr, fctr, xctr, yctr, extra_points;
double xprod, yprod, xsum, ysum;
/* why carry this around in the struct when we can calculate it on demand? */
extra_points = loop->cstep - (loop->nsteps % loop->cstep);
/* Allocate the np (new point) array (with padding) */
if ((np = (XPoint *) calloc(loop->nsteps+extra_points, sizeof (XPoint))) == NULL) {
free_lisa(lc);
return False;
}
/* Update the center */
loop->center.x += loop->dx;
loop->center.y += loop->dy;
CHECK_RADIUS(loop, lc);
/* check for overlaps -- where the figure might go off the screen */
if ((loop->center.x - loop->radius) <= 0) {
loop->center.x = loop->radius;
loop->dx = NRAND(XVMAX);
} else if ((loop->center.x + loop->radius) >= lc->width) {
loop->center.x = lc->width - loop->radius;
loop->dx = -NRAND(XVMAX);
};
if ((loop->center.y - loop->radius) <= 0) {
loop->center.y = loop->radius;
loop->dy = NRAND(YVMAX);
} else if ((loop->center.y + loop->radius) >= lc->height) {
loop->center.y = lc->height - loop->radius;
loop->dy = -NRAND(YVMAX);
};
/* Now draw the points, and erase the ones from the last cycle */
for (pctr = 0; pctr < loop->nsteps; pctr++) {
fctr = loop->nfuncs;
loop->phi = (double) (pctr - phase) * loop->pistep;
loop->theta = (double) (pctr + phase) * loop->pistep;
xsum = ysum = 0;
while (fctr--) {
xctr = lf[fctr]->nx;
yctr = lf[fctr]->ny;
if (additive) {
xprod = yprod = 0.0;
while (xctr--)
xprod += sin(lf[fctr]->xcoeff[xctr] * loop->theta);
while (yctr--)
yprod += sin(lf[fctr]->ycoeff[yctr] * loop->phi);
if (loop->melting) {
if (fctr) {
xsum += xprod * (double) (loop->nsteps - loop->melting) /
(double) loop->nsteps;
ysum += yprod * (double) (loop->nsteps - loop->melting) /
(double) loop->nsteps;
} else {
xsum += xprod * (double) loop->melting / (double) loop->nsteps;
ysum += yprod * (double) loop->melting / (double) loop->nsteps;
}
} else {
xsum = xprod;
ysum = yprod;
}
if (!fctr) {
xsum = xsum * (double) loop->radius / (double) lf[fctr]->nx;
ysum = ysum * (double) loop->radius / (double) lf[fctr]->ny;
}
} else {
if (loop->melting) {
if (fctr) {
yprod = xprod = (double) loop->radius *
(double) (loop->nsteps - loop->melting) /
(double) (loop->nsteps);
} else {
yprod = xprod = (double) loop->radius *
(double) (loop->melting) / (double) (loop->nsteps);
}
} else {
xprod = yprod = (double) loop->radius;
}
while (xctr--)
xprod *= sin(lf[fctr]->xcoeff[xctr] * loop->theta);
while (yctr--)
yprod *= sin(lf[fctr]->ycoeff[yctr] * loop->phi);
xsum += xprod;
ysum += yprod;
}
}
if ((loop->nfuncs > 1) && (!loop->melting)) {
xsum /= (double) loop->nfuncs;
ysum /= (double) loop->nfuncs;
}
xsum += (double) loop->center.x;
ysum += (double) loop->center.y;
np[pctr].x = (int) ceil(xsum);
np[pctr].y = (int) ceil(ysum);
}
/* fill in extra points */
for (pctr=loop->nsteps; pctr < loop->nsteps+extra_points; pctr++) {
np[pctr].x = np[pctr - loop->nsteps].x;
np[pctr].y = np[pctr - loop->nsteps].y;
}
if (loop->melting) {
if (!--loop->melting) {
loop->nfuncs = 1;
loop->function[0] = loop->function[1];
}
}
/* reset starting color each time to prevent ass-like appearance */
loop->color = STARTCOLOR;
if (loop->cstep < xMaxLines) {
/* printf("Drawing dashes\n"); */
for (pctr = 0; pctr < loop->nsteps; pctr+=loop->cstep) {
#if defined DRAWLINES
XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), loop->linewidth,
LINESTYLE, LINECAP, LINEJOIN);
/* erase the last cycle's point */
XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi));
XDrawLines(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi), &lp[pctr],
loop->cstep, CoordModeOrigin);
/* Set the new color */
SET_COLOR();
/* plot this cycle's point */
XDrawLines(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi),
&np[pctr], loop->cstep, CoordModeOrigin);
XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), 1,
LINESTYLE, LINECAP, LINEJOIN);
#else
/* erase the last cycle's point */
XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi));
XDrawPoints(MI_DISPLAY(mi), MI_WINDOW(mi), MI_GC(mi),
&lp[pctr], loop->cstep, CoordModeOrigin);
/* Set the new color */
SET_COLOR();
/* plot this cycle's point */
XDrawPoints(MI_DISPLAY(mi), MI_WINDOW(mi),
MI_GC(mi), &np[pctr], loop->cstep, CoordModeOrigin);
#endif
}
} else { /* on my system, cstep is larger than 65532/2 if we get here */
for (pctr = 0; pctr < loop->nsteps; pctr++) {
#if defined DRAWLINES
XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), loop->linewidth,
LINESTYLE, LINECAP, LINEJOIN);
/* erase the last cycle's point */
XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi));
XDrawLine(MI_DISPLAY(mi), MI_WINDOW(mi),
MI_GC(mi), lp[pctr].x, lp[pctr].y,
lp[(pctr + 1) % loop->nsteps].x,
lp[(pctr + 1) % loop->nsteps].y);
/* Set the new color */
SET_COLOR();
/* plot this cycle's point */
XDrawLine(MI_DISPLAY(mi), MI_WINDOW(mi),
MI_GC(mi), np[pctr].x, np[pctr].y,
np[(pctr + 1) % loop->nsteps].x,
np[(pctr + 1) % loop->nsteps].y);
XSetLineAttributes(MI_DISPLAY(mi), MI_GC(mi), 1,
LINESTYLE, LINECAP, LINEJOIN);
#else /* DRAWLINES */
/* erase the last cycle's point */
XSetForeground(MI_DISPLAY(mi), MI_GC(mi), MI_BLACK_PIXEL(mi));
XDrawPoint(MI_DISPLAY(mi), MI_WINDOW(mi),
MI_GC(mi), lp[pctr].x, lp[pctr].y);
/* Set the new color */
SET_COLOR();
/* plot this cycle's point */
XDrawPoint(MI_DISPLAY(mi), MI_WINDOW(mi),
MI_GC(mi), np[pctr].x, np[pctr].y);
#endif /* DRAWLINES */
}
}
(void) free((void *) lp);
loop->lastpoint = np;
return True;
}
