nav_msgs::OccupancyGrid OccupancyMap::getCostMap() {
nav_msgs::OccupancyGrid grid;
grid.info.width = map_->size_x;
grid.info.height = map_->size_y;
grid.info.resolution = map_->scale;
grid.info.origin.position.x = map_->origin_x - map_->size_x / 2 * map_->scale;
grid.info.origin.position.y = map_->origin_y - map_->size_y / 2 * map_->scale;
// Convert to player format
grid.data.resize(map_->size_x*map_->size_y);
ROS_ASSERT(map_->cells);
float max_cost = -std::numeric_limits<float>::infinity();
for (int i = 0; i < map_->size_x * map_->size_y; ++i) {
if (map_->cells[i].cost > max_cost && !isinff(map_->cells[i].cost)) {
max_cost = map_->cells[i].cost;
}
}
for (int i = 0; i < map_->size_x * map_->size_y; ++i) {
if (isinff(map_->cells[i].cost)) {
grid.data[i] = 100;
} else {
grid.data[i] = int(100.0 * map_->cells[i].cost / max_cost);
}
}
return grid;
}
ATF_TC_BODY(powf_inf_neg_x, tc)
{
const float x = -1.0L / 0.0L;
float z;
/*
* If y is odd, y > 0, and x is -Inf, -Inf is returned.
* If y is even, y > 0, and x is -Inf, +Inf is returned.
*/
z = powf(x, 3.0);
if (isinff(z) == 0 || signbit(z) == 0)
atf_tc_fail_nonfatal("powf(-Inf, 3.0) != -Inf");
z = powf(x, 4.0);
if (isinff(z) == 0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(-Inf, 4.0) != +Inf");
/*
* If y is odd, y < 0, and x is -Inf, -0.0 is returned.
* If y is even, y < 0, and x is -Inf, +0.0 is returned.
*/
z = powf(x, -3.0);
if (fabsf(z) > 0.0 || signbit(z) == 0) {
atf_tc_expect_fail("PR lib/45372");
atf_tc_fail_nonfatal("powf(-Inf, -3.0) != -0.0");
}
z = powf(x, -4.0);
if (fabsf(z) > 0.0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(-Inf -4.0) != +0.0");
}
void addSanitizedMultipliedByBuffer( sampleFrame* dst, const sampleFrame* src, float coeffSrc, ValueBuffer * coeffSrcBuf, int frames )
{
for( int f = 0; f < frames; ++f )
{
dst[f][0] += ( isinff( src[f][0] ) || isnanf( src[f][0] ) ) ? 0.0f : src[f][0] * coeffSrc * coeffSrcBuf->values()[f];
dst[f][1] += ( isinff( src[f][1] ) || isnanf( src[f][1] ) ) ? 0.0f : src[f][1] * coeffSrc * coeffSrcBuf->values()[f];
}
}
/**
* crank_box2_has_inf:
* @box: A Box.
*
* Checks whether box has infinite.
*
* Returns: Whether box has infinite.
*/
gboolean
crank_box2_has_inf (CrankBox2 *box)
{
return isinff (box->start.x) ||
isinff (box->start.y) ||
isinff (box->end.x) ||
isinff (box->end.y);
}
void
BPose::SetLocation(BPoint point, const BPoseView* poseView)
{
float scale = 1.0;
if (poseView->ViewMode() == kIconMode)
scale = poseView->IconSize() / 32.0;
fLocation = BPoint(floorf(point.x / scale), floorf(point.y / scale));
if (isinff(fLocation.x) || isinff(fLocation.y))
debugger("BPose::SetLocation() - infinite location");
fHasLocation = true;
}
void get_map_extrema(const float * map, unsigned int nside,
float * min, float * max)
{
size_t num_of_pixels = nside * nside * 12;
const float * end_of_map = map + num_of_pixels;
const float * cur_pixel = map;
int is_first = 1;
while(cur_pixel != end_of_map)
{
if(! isinff(*cur_pixel) &&
! isnanf(*cur_pixel) &&
*cur_pixel > -1.6e+30)
{
if(! is_first)
{
if(*cur_pixel < *min)
*min = *cur_pixel;
else if(*cur_pixel > *max)
*max = *cur_pixel;
}
else
{
*min = *max = *cur_pixel;
is_first = 0;
}
}
cur_pixel++;
}
}
ATF_TC_BODY(powf_one_neg_x, tc)
{
const float infp = 1.0L / 0.0L;
const float infn = -1.0L / 0.0L;
/*
* If x is -1.0, and y is +-Inf, 1.0 shall be returned.
*/
ATF_REQUIRE(isinff(infp) != 0);
ATF_REQUIRE(isinff(infn) != 0);
if (powf(-1.0, infp) != 1.0) {
atf_tc_expect_fail("PR lib/45372");
atf_tc_fail_nonfatal("powf(-1.0, +Inf) != 1.0");
}
if (powf(-1.0, infn) != 1.0) {
atf_tc_expect_fail("PR lib/45372");
atf_tc_fail_nonfatal("powf(-1.0, -Inf) != 1.0");
}
}
/*! \brief Function for sanitizing a buffer of infs/nans - returns true if those are found */
bool sanitize( sampleFrame * src, int frames )
{
bool found = false;
for( int f = 0; f < frames; ++f )
{
for( int c = 0; c < 2; ++c )
{
if( isinff( src[f][c] ) || isnanf( src[f][c] ) )
{
src[f][c] = 0.0f;
found = true;
}
}
}
return found;
}
void
plot_bitmap_to_cairo_surface(cairo_t * cairo_context,
double origin_x, double origin_y,
double size_x, double size_y,
double map_min, double map_max,
const float * bitmap,
unsigned int bitmap_width,
unsigned int bitmap_height)
{
const double pixel_width = size_x / bitmap_width;
const double pixel_height = size_y / bitmap_height;
const double dynamic_range = map_max - map_min;
const float * cur_pixel = bitmap;
unsigned int cur_y;
for(cur_y = bitmap_height; cur_y > 0; --cur_y)
{
unsigned int cur_x;
for(cur_x = 0; cur_x < bitmap_width; ++cur_x)
{
double value = *bitmap++;
color_t color;
if(isinff(value))
continue;
else if (isnanf(value) || value < -1.6e+30)
color.red = color.green = color.blue = 0.5;
else
{
double normalized_value = (value - map_min) / dynamic_range;
get_palette_color(normalized_value, &color);
}
cairo_rectangle(cairo_context,
origin_x + (cur_x * size_x) / bitmap_width,
origin_y + (cur_y * size_y) / bitmap_height,
pixel_width,
pixel_height);
cairo_set_source_rgb(cairo_context,
color.red,
color.green,
color.blue);
cairo_fill(cairo_context);
}
}
}
static int set_auto_gain(sensor_t *sensor, int enable, float gain_db, float gain_db_ceiling)
{
uint8_t reg;
int ret = cambus_readb(sensor->slv_addr, BANK_SEL, ®);
ret |= cambus_writeb(sensor->slv_addr, BANK_SEL, reg | BANK_SEL_SENSOR);
ret |= cambus_readb(sensor->slv_addr, COM8, ®);
ret |= cambus_writeb(sensor->slv_addr, COM8, (reg & (~COM8_AGC_EN)) | ((enable != 0) ? COM8_AGC_EN : 0));
if ((enable == 0) && (!isnanf(gain_db)) && (!isinff(gain_db))) {
float gain = IM_MAX(IM_MIN(fast_expf((gain_db / 20.0) * fast_log(10.0)), 32.0), 1.0);
int gain_temp = fast_roundf(fast_log2(IM_MAX(gain / 2.0, 1.0)));
int gain_hi = 0xF >> (4 - gain_temp);
int gain_lo = IM_MIN(fast_roundf(((gain / (1 << gain_temp)) - 1.0) * 16.0), 15);
ret |= cambus_writeb(sensor->slv_addr, GAIN, (gain_hi << 4) | (gain_lo << 0));
} else if ((enable != 0) && (!isnanf(gain_db_ceiling)) && (!isinff(gain_db_ceiling))) {
ATF_TC_BODY(powf_inf_pos_y, tc)
{
const float y = 1.0L / 0.0L;
float z;
/*
* If |x| < 1 and y is +Inf, +0.0 is returned.
* If |x| > 1 and y is +Inf, +Inf is returned.
*/
z = powf(0.1, y);
if (fabsf(z) > 0.0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(0.1, +Inf) != +0.0");
z = powf(1.1, y);
if (isinff(z) == 0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(1.1, +Inf) != +Inf");
}
ATF_TC_BODY(powf_inf_pos_x, tc)
{
const float x = 1.0L / 0.0L;
float z;
/*
* For y < 0, if x is +Inf, +0.0 is returned.
* For y > 0, if x is +Inf, +Inf is returned.
*/
z = powf(x, -2.0);
if (fabsf(z) > 0.0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(+Inf, -2.0) != +0.0");
z = powf(x, 2.0);
if (isinff(z) == 0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(+Inf, 2.0) != +Inf");
}
float
__cosf (float x)
{
float y[2], z = 0.0;
float ix;
int32_t n;
ix = __builtin_fabsf (x);
/* |x| ~< pi/4 */
if (ix <= pio4)
{
return __kernel_cosf (x, z);
/* cos(Inf or NaN) is NaN */
}
else if (isnanf (ix))
{
return x - x;
}
else if (isinff (ix))
{
__set_errno (EDOM);
return x - x;
}
/* argument reduction needed */
else
{
n = __ieee754_rem_pio2f (x, y);
switch (n & 3)
{
case 0:
return __kernel_cosf (y[0], y[1]);
case 1:
return -__kernel_sinf (y[0], y[1], 1);
case 2:
return -__kernel_cosf (y[0], y[1]);
default:
return __kernel_sinf (y[0], y[1], 1);
}
}
}
static int lFloatConst(int ch, int len, yystypepp * yylvalpp)
{
int alreadyComplained = 0;
assert((ch == '.') || (ch == 'e') || (ch == 'E'));
if (ch == '.') {
do {
APPEND_CHAR_S(ch, yylvalpp->symbol_name, len, MAX_SYMBOL_NAME_LEN);
ch = cpp->currentInput->getch(cpp->currentInput, yylvalpp);
} while (ch >= '0' && ch <= '9');
}
// Exponent:
if (ch == 'e' || ch == 'E') {
APPEND_CHAR_S(ch, yylvalpp->symbol_name, len, MAX_SYMBOL_NAME_LEN);
ch = cpp->currentInput->getch(cpp->currentInput, yylvalpp);
if (ch == '+') {
APPEND_CHAR_S(ch, yylvalpp->symbol_name, len, MAX_SYMBOL_NAME_LEN);
ch = cpp->currentInput->getch(cpp->currentInput, yylvalpp);
} else if (ch == '-') {
APPEND_CHAR_S(ch, yylvalpp->symbol_name, len, MAX_SYMBOL_NAME_LEN);
ch = cpp->currentInput->getch(cpp->currentInput, yylvalpp);
}
if (ch >= '0' && ch <= '9') {
while (ch >= '0' && ch <= '9') {
APPEND_CHAR_S(ch, yylvalpp->symbol_name, len, MAX_SYMBOL_NAME_LEN);
ch = cpp->currentInput->getch(cpp->currentInput, yylvalpp);
}
} else {
CPPErrorToInfoLog("EXPONENT INVALID");
}
}
cpp->currentInput->ungetch(cpp->currentInput, ch, yylvalpp);
assert(len <= MAX_SYMBOL_NAME_LEN);
yylvalpp->symbol_name[len] = '\0';
yylvalpp->sc_fval = (float) atof_dot(yylvalpp->symbol_name);
if (isinff(yylvalpp->sc_fval)) {
CPPErrorToInfoLog("FLOAT CONSTANT OVERFLOW");
}
return CPP_FLOATCONSTANT;
} // lFloatConst
float
roundf(float x)
{
float t;
if (isinff(x) || isnanf(x))
return (x);
if (x >= 0.0) {
t = floorf(x);
if (t - x <= -0.5)
t += 1.0;
return (t);
} else {
t = floorf(-x);
if (t + x <= -0.5)
t += 1.0;
return (-t);
}
}
ATF_TC_BODY(powf_inf_neg_y, tc)
{
#ifndef __vax__
const float y = -1.0L / 0.0L;
float z;
/*
* If |x| < 1 and y is -Inf, +Inf is returned.
* If |x| > 1 and y is -Inf, +0.0 is returned.
*/
z = powf(0.1, y);
if (isinff(z) == 0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(0.1, -Inf) != +Inf");
z = powf(1.1, y);
if (fabsf(z) > 0.0 || signbit(z) != 0)
atf_tc_fail_nonfatal("powf(1.1, -Inf) != +0.0");
#endif
}
char * dtostrf(float val, int width, unsigned int precision, char *buf)
{
int decpt, sign, reqd, pad;
const char *s, *e;
char *p;
int awidth = abs(width);
if (isnanf(val)) {
int ndigs = (val<0) ? 4 : 3;
awidth = (awidth > ndigs) ? awidth - ndigs : 0;
if (width<0) {
while (awidth) {
*buf++ = ' ';
awidth--;
}
}
if (copysignf(1.0f, val)<0) *buf++ = '-';
if (DTOA_UPPER) {
*buf++ = 'N'; *buf++ = 'A'; *buf++ = 'N';
} else {
*buf++ = 'n'; *buf++ = 'a'; *buf++ = 'n';
}
while (awidth) {
*buf++ = ' ';
awidth--;
}
*buf = 0;
return buf;
}
if (isinff(val)) {
int ndigs = (val<0) ? 4 : 3;
awidth = (awidth > ndigs) ? awidth - ndigs : 0;
if (width<0) {
while (awidth) {
*buf++ = ' ';
awidth--;
}
}
if (val<0) *buf++ = '-';
if (DTOA_UPPER) {
*buf++ = 'I'; *buf++ = 'N'; *buf++ = 'F';
} else {
*buf++ = 'i'; *buf++ = 'n'; *buf++ = 'f';
}
while (awidth) {
*buf++ = ' ';
awidth--;
}
*buf = 0;
return buf;
}
s = fcvtf(val, precision, &decpt, &sign);
if (precision == 0 && decpt == 0) {
s = (*s < '5') ? "0" : "1";
reqd = 1;
} else {
reqd = strlen(s);
if (reqd > decpt) reqd++;
if (decpt == 0) reqd++;
}
if (sign) reqd++;
p = buf;
e = p + reqd;
pad = width - reqd;
if (pad > 0) {
e += pad;
while (pad-- > 0) *p++ = ' ';
}
if (sign) *p++ = '-';
if (decpt == 0 && precision > 0) {
*p++ = '0';
*p++ = '.';
}
else if (decpt < 0 && precision > 0) {
*p++ = '0';
*p++ = '.';
e++;
while ( decpt < 0 ) {
decpt++;
*p++ = '0';
}
}
while (p < e) {
*p++ = *s++;
if (p == e) break;
if (--decpt == 0) *p++ = '.';
}
if (width < 0) {
pad = (reqd + width) * -1;
while (pad-- > 0) *p++ = ' ';
}
*p = 0;
//char format[20];
//sprintf(format, "%%%d.%df", width, precision);
//sprintf(buf, format, val);
return buf;
}
void OccupancyMap::addNeighbors(const Node &node, double max_occ_dist, bool allow_unknown) {
// TODO: Return to more efficient lazy-initialization
// // Check if we're neighboring nodes whose costs_ are uninitialized.
// // if (!full_init &&
// // ((ci + 1 >= init_lr.first) || (ci - 1 <= init_ul.first) ||
// // (cj + 1 >= init_lr.second) || (cj - 1 <= init_ul.second))) {
// // full_init = true;
// // for (int j = 0; j < map_->size_y; ++j) {
// // for (int i = 0; i < map_->size_x; ++i) {
// // // Only initialize costs_ that are outside original rectangle
// // if (!(init_ul.first <= i && i < init_lr.first &&
// // init_ul.second <= j && j < init_lr.second)) {
// // int ind = MAP_INDEX(map_, i, j);
// // costs_[ind] = std::numeric_limits<float>::infinity();
// // }
// // }
// // }
// // }
int ci = node.coord.first;
int cj = node.coord.second;
// Iterate over neighbors
for (int newj = cj - 1; newj <= cj + 1; ++newj) {
for (int newi = ci - 1; newi <= ci + 1; ++newi) {
// Skip self edges
if ((newi == ci && newj == cj) || !MAP_VALID(map_, newi, newj)) {
continue;
}
// fprintf(stderr, " Examining %i %i ", newi, newj);
int index = MAP_INDEX(map_, newi, newj);
map_cell_t *cell = map_->cells + index;
// If cell is occupied or too close to occupied cell, continue
if (isinff(cell->cost) ||
(!allow_unknown && cell->occ_state == map_cell_t::UNKNOWN)) {
// fprintf(stderr, "occupado\n");
continue;
}
// fprintf(stderr, "free\n");
double edge_cost = ci == newi || cj == newj ? 1 : sqrt(2);
double heur_cost = 0.0;
if (stopi_ != -1 && stopj_ != -1) {
heur_cost = hypot(newi - stopi_, newj - stopj_);
}
double total_cost = node.true_cost + edge_cost + cell->cost + heur_cost;
if (total_cost < costs_[index]) {
// fprintf(stderr, " Better path: new cost= % 6.2f\n", ttl_cost);
// If node has finite cost, it's in queue and needs to be removed
if (!isinf(costs_[index])) {
Q_->erase(Node(make_pair(newi, newj), costs_[index], 0.0));
}
costs_[index] = total_cost;
prev_i_[index] = ci;
prev_j_[index] = cj;
Q_->insert(Node(make_pair(newi, newj),
node.true_cost + edge_cost + cell->cost,
total_cost));
}
}
}
}
int32_t
__ieee754_rem_pio2f (float x, float *y)
{
float ax, z, n, r, w, t, e0;
float tx[3];
int32_t i, nx;
ax = __builtin_fabsf (x);
if (ax <= pio4)
{
y[0] = x;
y[1] = 0;
return 0;
}
if (ax < pio3_4)
{
if (x > 0)
{
z = x - pio2_1;
if (!__float_and_test28 (ax, pio2_24b))
{
y[0] = z - pio2_1t;
y[1] = (z - y[0]) - pio2_1t;
}
else
{
z -= pio2_2;
y[0] = z - pio2_2t;
y[1] = (z - y[0]) - pio2_2t;
}
return 1;
}
else
{
z = x + pio2_1;
if (!__float_and_test28 (ax, pio2_24b))
{
y[0] = z + pio2_1t;
y[1] = (z - y[0]) + pio2_1t;
}
else
{
z += pio2_2;
y[0] = z + pio2_2t;
y[1] = (z - y[0]) + pio2_2t;
}
return -1;
}
}
if (ax <= pio2_2e7)
{
n = __floorf (ax * invpio2 + half);
i = (int32_t) n;
r = ax - n * pio2_1;
w = n * pio2_1t; /* 1st round good to 40 bit */
if (i < 32 && !__float_and_test24 (ax, npio2_hw[i - 1]))
{
y[0] = r - w;
}
else
{
float i, j;
j = __float_and8 (ax);
y[0] = r - w;
i = __float_and8 (y[0]);
if (j / i > 256.0 || j / i < 3.9062500e-3)
{ /* 2nd iterations needed, good to 57 */
t = r;
w = n * pio2_2;
r = t - w;
w = n * pio2_2t - ((t - r) - w);
y[0] = r - w;
i = __float_and8 (y[0]);
if (j / i > 33554432 || j / i < 2.9802322e-8)
{ /* 3rd iteration needed, 74 bits acc */
t = r;
w = n * pio2_3;
r = t - w;
w = n * pio2_3t - ((t - r) - w);
y[0] = r - w;
}
}
}
y[1] = (r - y[0]) - w;
if (x < 0)
{
y[0] = -y[0];
y[1] = -y[1];
return -i;
}
else
{
return i;
}
}
/* all other (large) arguments */
if (isnanf (x) || isinff (x))
{
y[0] = y[1] = x - x;
return 0;
}
/* set z = scalbn(|x|,ilogb(x)-7) */
e0 = __float_and8 (ax / 128.0);
z = ax / e0;
tx[0] = __floorf (z);
z = (z - tx[0]) * two8;
tx[1] = __floorf (z);
z = (z - tx[1]) * two8;
tx[2] = __floorf (z);
nx = 3;
while (tx[nx - 1] == zero)
nx--;
i = __fp_kernel_rem_pio2f (tx, y, e0, nx);
if (x < 0)
{
y[0] = -y[0];
y[1] = -y[1];
return -i;
}
return i;
}
int test1f(float x)
{
return isinff(x);
}
void operator()( sampleFrame& dst, const sampleFrame& src ) const
{
dst[0] += ( isinff( src[0] ) || isnanf( src[0] ) ) ? 0.0f : src[0] * m_coeff;
dst[1] += ( isinff( src[1] ) || isnanf( src[1] ) ) ? 0.0f : src[1] * m_coeff;
}
