void robot_frontlaser_handler(carmen_robot_laser_message *frontlaser) { int i; double x, y; if (localize_msg->timestamp == 0) return; fprintf(stderr, "F"); carmen_localize_correct_laser(frontlaser, localize_msg); for(i = 0; i < frontlaser->num_readings; i++) { x = frontlaser->laser_pose.x+cos(-M_PI/2 + M_PI*i/180.0 + frontlaser->laser_pose.theta)*frontlaser->range[i]; y = frontlaser->laser_pose.y+sin(-M_PI/2 + M_PI*i/180.0 + frontlaser->laser_pose.theta)*frontlaser->range[i]; carmen_fprintf(outfile, "%.2f %.2f\n", x, y); } }
static void laser_handler(carmen_robot_laser_message *laser) { int i, c, n; static int cluster_map[500]; static int cluster_cnt; static double x[500], y[500]; if (static_map.map == NULL || odom.timestamp == 0.0) return; carmen_localize_correct_laser(laser, &odom); kill_people(); filter_motion(); for (i = 0; i < num_filters; i++) { filters[i].updated = 0; filters[i].last_type = filters[i].type; filters[i].invisible = 0; } if (dist(last_sensor_update_odom.x - odom.globalpos.x, last_sensor_update_odom.y - odom.globalpos.y) >= sensor_update_dist) { do_sensor_update = 1; for (i = 0; i < num_filters; i++) filters[i].do_motion_update = 1; } else do_sensor_update = 0; if (do_sensor_update) { last_sensor_update_odom.x = odom.globalpos.x; last_sensor_update_odom.y = odom.globalpos.y; } cluster_cnt = 0; for (i = 0; i < laser->num_readings; i++) { cluster_map[i] = 0; if (laser->range[i] < laser_max_range) { x[i] = laser->x + cos(laser->theta + (i-90)*M_PI/180.0) * laser->range[i]; y[i] = laser->y + sin(laser->theta + (i-90)*M_PI/180.0) * laser->range[i]; trace_laser(laser->x, laser->y, x[i], y[i]); if (!dot_filter(x[i], y[i]) && !map_filter(x[i], y[i], laser->range[i])) cluster_cnt = cluster(cluster_map, cluster_cnt, i, x, y); } } for (i = 0; i < num_filters; i++) { if (filters[i].invisible && !filters[i].updated) { if (++filters[i].invisible_cnt >= invisible_cnt) delete_filter(i); } else filters[i].invisible_cnt = 0; } //printf("cluster_cnt = %d\n", cluster_cnt); for (c = 1; c <= cluster_cnt; c++) { n = 0; for (i = 0; i < laser->num_readings; i++) if (cluster_map[i] == c) n++; //printf("cluster %d has %d readings\n", c, n); if (n >= new_filter_threshold) add_new_dot_filter(cluster_map, c, laser->num_readings, x, y); } #if 0 for (i = 0; i < num_filters; i++) { if (filters[i].type != filters[i].last_type) { n = filters[i].type; filters[i].type = filters[i].last_type; //publish_dot_msg(&filters[i], 1); filters[i].type = n; /* switch (filters[i].type) { case CARMEN_DOT_PERSON: printf("publishing add person %d msg\n", filters[i].id); break; case CARMEN_DOT_TRASH: printf("publishing add trash %d msg\n", filters[i].id); break; case CARMEN_DOT_DOOR: printf("publishing add door %d msg\n", filters[i].id); break; } */ } //if (filters[i].updated) //publish_dot_msg(&filters[i], 0); } #endif /* for (i = 0; i < num_filters; i++) { printf("vel = %.4f, ", person_filter_velocity(&filters[i].person_filter)); if (filters[i].type == CARMEN_DOT_PERSON) { printf("PERSON (x=%.2f, y=%.2f) (vx=%.4f, vy=%.4f, vxy=%.4f)\n", filters[i].person_filter.x, filters[i].person_filter.y, filters[i].person_filter.px, filters[i].person_filter.py, filters[i].person_filter.pxy); } else if (filters[i].type == CARMEN_DOT_TRASH) printf("TRASH (x=%.2f, y=%.2f) (vx=%.4f, vy=%.4f, vxy=%.4f)\n", filters[i].trash_filter.x, filters[i].trash_filter.y, filters[i].trash_filter.px, filters[i].trash_filter.py, filters[i].trash_filter.pxy); else printf("DOOR (x=%.2f, y=%.2f, t=%.2f) (vx=%.4f, vy=%.4f, vxy=%.4f, vt=%.4f)\n", filters[i].door_filter.x, filters[i].door_filter.y, filters[i].door_filter.t, filters[i].door_filter.px, filters[i].door_filter.py, filters[i].door_filter.pxy, filters[i].door_filter.pt); } printf("\n"); */ publish_all_dot_msgs(); }