int
main( int argc, char *argv[] ) {
init_trema( &argc, &argv );
delete_filter();
start_trema();
}
static void kill_people() {
int i;
for (i = 0; i < num_filters; i++)
if (!filters[i].updated)
if (++filters[i].person_filter.hidden_cnt >= kill_hidden_person_cnt &&
filters[i].type == CARMEN_DOT_PERSON)
delete_filter(i);
}
/**
* Called as part of the hashmap callback
* to cleanup the filters.
*/
static int filter_map_delete_cb(void *data, const unsigned char *key, uint32_t key_len, void *value) {
(void)data;
(void)key;
(void)key_len;
// Cast the inputs
bloom_filter_wrapper *filt = value;
// Delete, but not the underlying files
filt->should_delete = 0;
delete_filter(filt);
return 0;
}
/**
* Deletes old versions from the delta lists, and calls
* delete_filter on the filters in the destroyed list.
*
* Safety: Same as remove_delta_versions
*/
static void delete_old_versions(bloom_filtmgr *mgr, unsigned long long min_vsn) {
// Get the merged in pending ops, lock to avoid a race
pthread_mutex_lock(&mgr->write_lock);
filter_list *old = remove_delta_versions(mgr->delta, &mgr->delta, min_vsn);
pthread_mutex_unlock(&mgr->write_lock);
// Delete the filters now that we have merged into both trees
filter_list *next, *current = old;
while (current) {
if (current->type == DELETE) delete_filter(current->filter);
next = current->next;
free(current);
current = next;
}
}
/**
* Cleanup
* @arg mgr The manager to destroy
* @return 0 on success.
*/
int destroy_filter_manager(bloom_filtmgr *mgr) {
// Stop the vacuum thread
mgr->should_run = 0;
if (mgr->vacuum_thread) pthread_join(mgr->vacuum_thread, NULL);
// Nuke all the keys in the current version.
art_iter(mgr->filter_map, filter_map_delete_cb, mgr);
// Handle any delta operations
filter_list *next, *current = mgr->delta;
while (current) {
// Only delete pending creates, pending
// deletes are still in the primary tree
if (current->type == CREATE)
delete_filter(current->filter);
next = current->next;
free(current);
current = next;
}
// Free the clients
filtmgr_client *cl_next, *cl = mgr->clients;
while (cl) {
cl_next = cl->next;
free(cl);
cl = cl_next;
}
// Destroy the ART trees
destroy_art_tree(mgr->filter_map);
destroy_art_tree(mgr->alt_filter_map);
free((mgr->filter_map < mgr->alt_filter_map) ? mgr->filter_map : mgr->alt_filter_map);
// Free the manager
free(mgr);
return 0;
}
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();
}
/** Crée l'interface : boutons, intitulés, menus... */
void Ui_MainWindow::setupUi(QMainWindow *MainWindow) {
if (MainWindow->objectName().isEmpty())
MainWindow->setObjectName(QString::fromUtf8("MainWindow"));
actionQuitter = new QAction(MainWindow);
actionQuitter->setObjectName(QString::fromUtf8("actionQuitter"));
actionFichier = new QAction(MainWindow);
actionFichier->setObjectName(QString::fromUtf8("actionFichier"));
actionWebcam = new QAction(MainWindow);
actionWebcam->setObjectName(QString::fromUtf8("actionWebcam"));
centralwidget = new QWidget(MainWindow);
centralwidget->setObjectName(QString::fromUtf8("centralwidget"));
listWidget = new QListWidget(centralwidget);
new QListWidgetItem(listWidget);
listWidget->setObjectName(QString::fromUtf8("listWidget"));
listWidget->setDragDropMode(QAbstractItemView::InternalMove);
cvwidget = new QOpenCVWidget(this);
cvwidget->setObjectName(QString::fromUtf8("cvwidget"));
label_2 = new QLabel(centralwidget);
label_2->setObjectName(QString::fromUtf8("label_2"));
label_6 = new QLabel(centralwidget);
label_6->setObjectName(QString::fromUtf8("label_6"));
comboBox_4 = new QComboBox(centralwidget);
comboBox_4->setObjectName(QString::fromUtf8("comboBox_4"));
buttonBox_2 = new QDialogButtonBox(centralwidget);
buttonBox_2->setObjectName(QString::fromUtf8("buttonBox_2"));
apply_button = new QPushButton(tr("Appliquer"));
delete_button = new QPushButton(tr("Supprimer"));
buttonBox_2->addButton(apply_button, QDialogButtonBox::AcceptRole);
buttonBox_2->addButton(delete_button, QDialogButtonBox::RejectRole);
comboBox = new QComboBox(centralwidget);
comboBox->setObjectName(QString::fromUtf8("comboBox"));
MainWindow->setCentralWidget(centralwidget);
menubar = new QMenuBar(MainWindow);
menubar->setObjectName(QString::fromUtf8("menubar"));
menubar->setGeometry(QRect(0, 0, 540, 25));
menuFichier = new QMenu(menubar);
menuFichier->setObjectName(QString::fromUtf8("menuFichier"));
menuOuvrir = new QMenu(menuFichier);
menuOuvrir->setObjectName(QString::fromUtf8("menuOuvrir"));
MainWindow->setMenuBar(menubar);
statusbar = new QStatusBar(MainWindow);
statusbar->setObjectName(QString::fromUtf8("statusbar"));
MainWindow->setStatusBar(statusbar);
menubar->addAction(menuFichier->menuAction());
menuFichier->addAction(menuOuvrir->menuAction());
menuFichier->addAction(actionQuitter);
menuOuvrir->addAction(actionFichier);
menuOuvrir->addAction(actionWebcam);
may_the_force_be_with_you();
retranslateUi(MainWindow);
QObject::connect(actionQuitter, SIGNAL(triggered()), MainWindow, SLOT(close()));
QObject::connect(actionFichier, SIGNAL(triggered()), MainWindow, SLOT(select_file()));
QObject::connect(buttonBox_2, SIGNAL(accepted()), MainWindow, SLOT(apply_changes()));
QObject::connect(buttonBox_2, SIGNAL(rejected()), MainWindow, SLOT(delete_filter()));
QObject::connect(actionWebcam, SIGNAL(triggered()), MainWindow, SLOT(select_camera()));
QMetaObject::connectSlotsByName(MainWindow);
}