static void
touchpad_update_state(struct touchpad_dispatch *touchpad, uint32_t time)
{
int motion_index;
int center_x, center_y;
double dx = 0.0, dy = 0.0;
struct libinput_device *base = &touchpad->device->base;
if (touchpad->reset ||
touchpad->last_finger_state != touchpad->finger_state) {
touchpad->reset = 0;
touchpad->motion_count = 0;
touchpad->event_mask = TOUCHPAD_EVENT_NONE;
touchpad->event_mask_filter =
TOUCHPAD_EVENT_ABSOLUTE_X | TOUCHPAD_EVENT_ABSOLUTE_Y;
touchpad->last_finger_state = touchpad->finger_state;
process_fsm_events(touchpad, time);
return;
}
touchpad->last_finger_state = touchpad->finger_state;
if (!(touchpad->event_mask & TOUCHPAD_EVENT_REPORT))
return;
else
touchpad->event_mask &= ~TOUCHPAD_EVENT_REPORT;
if ((touchpad->event_mask & touchpad->event_mask_filter) !=
touchpad->event_mask_filter)
return;
touchpad->event_mask_filter = TOUCHPAD_EVENT_ABSOLUTE_ANY;
touchpad->event_mask = 0;
/* Avoid noise by moving center only when delta reaches a threshold
* distance from the old center. */
if (touchpad->motion_count > 0) {
center_x = hysteresis(touchpad->hw_abs.x,
touchpad->hysteresis.center_x,
touchpad->hysteresis.margin_x);
center_y = hysteresis(touchpad->hw_abs.y,
touchpad->hysteresis.center_y,
touchpad->hysteresis.margin_y);
} else {
center_x = touchpad->hw_abs.x;
center_y = touchpad->hw_abs.y;
}
touchpad->hysteresis.center_x = center_x;
touchpad->hysteresis.center_y = center_y;
touchpad->hw_abs.x = center_x;
touchpad->hw_abs.y = center_y;
/* Update motion history tracker */
motion_index = (touchpad->motion_index + 1) % TOUCHPAD_HISTORY_LENGTH;
touchpad->motion_index = motion_index;
touchpad->motion_history[motion_index].x = touchpad->hw_abs.x;
touchpad->motion_history[motion_index].y = touchpad->hw_abs.y;
if (touchpad->motion_count < 4)
touchpad->motion_count++;
if (touchpad->motion_count >= 4) {
touchpad_get_delta(touchpad, &dx, &dy);
filter_motion(touchpad, &dx, &dy, time);
if (touchpad->finger_state == TOUCHPAD_FINGERS_ONE) {
pointer_notify_motion(
base,
time,
li_fixed_from_double(dx),
li_fixed_from_double(dy));
} else if (touchpad->finger_state == TOUCHPAD_FINGERS_TWO) {
if (dx != 0.0)
pointer_notify_axis(
base,
time,
LIBINPUT_POINTER_AXIS_HORIZONTAL_SCROLL,
li_fixed_from_double(dx));
if (dy != 0.0)
pointer_notify_axis(
base,
time,
LIBINPUT_POINTER_AXIS_VERTICAL_SCROLL,
li_fixed_from_double(dy));
}
}
if (!(touchpad->state & TOUCHPAD_STATE_MOVE) &&
((int)dx || (int)dy)) {
touchpad->state |= TOUCHPAD_STATE_MOVE;
push_fsm_event(touchpad, FSM_EVENT_MOTION);
}
process_fsm_events(touchpad, time);
}
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();
}
