/*
* Hokuyo TOP-URG provides 270 degree of sensing with 1081 points,
* meaning that a data point appears at every 0.25 degree.
*
* 0 to 135 degree is used to control the car drive straight aligned with
* the right-hand side of the wall.
* 135 to 270 degree is used to detect objects.
*
* We use a trigonometric function to calculate x and y of the range.
*/
void scanValues(const sensor_msgs::LaserScan laser)
{
int i;
int n_ranges; /* TOP-URG should have 1081 points of array. */
int idx_c; /* range index of the center point. */
int idx_r1, idx_r2; /* range index of the two right points by the wall. */
float x[TOP_URG_RANGES], y[TOP_URG_RANGES], dist[TOP_URG_RANGES];
float rad;
/* ranges.size and intensities.size should be the same. */
n_ranges = laser.ranges.size(); /* = laser.intensities.size() */
if (n_ranges != TOP_URG_RANGES) {
ROS_INFO("Ranges mismatched.\n");
n_ranges = (TOP_URG_RANGES > n_ranges) ? n_ranges : TOP_URG_RANGES;
}
/* get the center point. */
idx_c = n_ranges / 2;
/* get all the x and y positions with the corresponding distance. */
for (i = 0; i < n_ranges; i++) {
rad = laser.angle_min + i * laser.angle_increment + rad(90);
x[i] = laser.ranges[i] * cos(rad);
y[i] = laser.ranges[i] * sin(rad);
dist[i] = laser.ranges[i];
#ifdef DEBUG
printf("(x[%d], y[%d]) = (%f, %f)\n", i, i, x[i], y[i]);
#endif
}
/* object detection is performed for the left-hand side. */
// detect_object(x, y, dist, n_ranges/2, n_ranges);
#if 1
/* for the front head (wall distance check). */
if (y[idx_c] < dist_wall && y[idx_c] >= 0.002) {
printf("TURN %f\n", y[idx_c]);
/* should turn. */
setSharedObstInfo(x[idx_c], y[idx_c], WALL);
return ;
}
#else
for (i = idx_c - 20; i < idx_c + 20; i++) {
/* for the front head (wall distance check). */
if (y[i] < dist_wall && y[i] >= 0.002) {
printf("TURN %f\n", y[i]);
/* should turn. */
setSharedObstInfo(x[i], y[i], WALL);
return ;
}
}
#endif
/* direction adjustment is performed for the right-hand side. */
adjust_direction(x, y, dist, 200, 300);
}
static void
do_stn(point *p, double X, const char *prefix, int dir, int labOnly)
{
leg *l, *lp;
double dX;
const stn *s;
int odir = dir;
for (s = p->stns; s; s = s->next) {
img_write_item(pimg_out, img_LABEL, s->flags, s->label, X, 0, p->p.z);
}
if (labOnly || p->fBroken) {
return;
}
lp = &headleg;
for (l = lp->next; l; lp = l, l = lp->next) {
dir = odir;
if (l->fDone) {
/* this case happens if a recursive call causes the next leg to be
* removed, leaving our next pointing to a leg which has been dealt
* with... */
continue;
}
if (l->prefix != prefix) {
continue;
}
if (l->to == p) {
if (l->broken & BREAK_TO) continue;
lp->next = l->next;
/* adjust direction of extension if necessary */
dir = adjust_direction(dir, l->to->dir);
dir = adjust_direction(dir, l->dir);
dX = hypot(l->fr->p.x - l->to->p.x, l->fr->p.y - l->to->p.y);
if (dir == ELEFT) dX = -dX;
img_write_item(pimg_out, img_MOVE, 0, NULL, X + dX, 0, l->fr->p.z);
img_write_item(pimg_out, img_LINE, l->flags, l->prefix,
X, 0, l->to->p.z);
l->fDone = 1;
do_stn(l->fr, X + dX, l->prefix, dir, (l->broken & BREAK_FR));
l = lp;
} else if (l->fr == p) {
if (l->broken & BREAK_FR) continue;
lp->next = l->next;
/* adjust direction of extension if necessary */
dir = adjust_direction(dir, l->fr->dir);
dir = adjust_direction(dir, l->dir);
dX = hypot(l->fr->p.x - l->to->p.x, l->fr->p.y - l->to->p.y);
if (dir == ELEFT) dX = -dX;
img_write_item(pimg_out, img_MOVE, 0, NULL, X, 0, l->fr->p.z);
img_write_item(pimg_out, img_LINE, l->flags, l->prefix,
X + dX, 0, l->to->p.z);
l->fDone = 1;
do_stn(l->to, X + dX, l->prefix, dir, (l->broken & BREAK_TO));
l = lp;
}
}
lp = &headleg;
for (l = lp->next; l; lp = l, l = lp->next) {
dir = odir;
if (l->fDone) {
/* this case happens iff a recursive call causes the next leg to be
* removed, leaving our next pointing to a leg which has been dealt
* with... */
continue;
}
if (l->to == p) {
if (l->broken & BREAK_TO) continue;
lp->next = l->next;
/* adjust direction of extension if necessary */
dir = adjust_direction(dir, l->to->dir);
dir = adjust_direction(dir, l->dir);
dX = hypot(l->fr->p.x - l->to->p.x, l->fr->p.y - l->to->p.y);
if (dir == ELEFT) dX = -dX;
img_write_item(pimg_out, img_MOVE, 0, NULL, X + dX, 0, l->fr->p.z);
img_write_item(pimg_out, img_LINE, l->flags, l->prefix,
X, 0, l->to->p.z);
l->fDone = 1;
do_stn(l->fr, X + dX, l->prefix, dir, (l->broken & BREAK_FR));
l = lp;
} else if (l->fr == p) {
if (l->broken & BREAK_FR) continue;
lp->next = l->next;
/* adjust direction of extension if necessary */
dir = adjust_direction(dir, l->fr->dir);
dir = adjust_direction(dir, l->dir);
dX = hypot(l->fr->p.x - l->to->p.x, l->fr->p.y - l->to->p.y);
if (dir == ELEFT) dX = -dX;
img_write_item(pimg_out, img_MOVE, 0, NULL, X, 0, l->fr->p.z);
img_write_item(pimg_out, img_LINE, l->flags, l->prefix,
X + dX, 0, l->to->p.z);
l->fDone = 1;
do_stn(l->to, X + dX, l->prefix, dir, (l->broken & BREAK_TO));
l = lp;
}
}
}
