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(); }