int main(){
setvbuf (stdout, (char *) NULL, _IONBF, 0);
state_t *state = calloc (1, sizeof *state);
state->pose = (pose_xyt_t*)calloc(1,sizeof(pose_xyt_t));
state->scan_pose = (scanpose_xyt_t*)calloc(1,sizeof(scanpose_xyt_t));
memset(&state->t_filt_mvag,0,sizeof(mvag_filter_t));
state->t_filt_mvag.window = 0;
state->lcm = lcm_create (NULL);
pose_xyt_t_subscribe (state->lcm,
"BOTLAB_ODOMETRY",
pose_xyt_handler, state);
rplidar_laser_t_subscribe (state->lcm,
"RPLIDAR_LASER",
rplidar_laser_handler, state);
while (1){
lcm_handle (state->lcm);
}
}
int main (int argc, char *argv[]) {
// so that redirected stdout won't be insanely buffered.
setvbuf (stdout, (char *) NULL, _IONBF, 0);
maebot_shared_state_t sharedState;
initState(&sharedState);
sharedState.running = 1;
lcm_t *lcm = lcm_create(NULL);
if(!lcm) return EXIT_FAILURE;
maebot_sensor_data_t_subscribe(lcm, "MAEBOT_SENSOR_DATA",
sensor_data_handler, &sharedState);
maebot_processed_sensor_data_t_subscribe(lcm, "MAEBOT_PROCESSED_SENSOR_DATA",
processed_sensor_data_handler, &sharedState);
maebot_motor_feedback_t_subscribe(lcm, "MAEBOT_MOTOR_FEEDBACK",
motor_feedback_handler, &sharedState);
pose_xyt_t_subscribe(lcm, "BOTLAB_ODOMETRY",
pose_xyt_handler, &sharedState);
pthread_t lcm_thread;
pthread_create(&lcm_thread, NULL, lcm_handler, lcm);
// Wait for first sensor data to come in
//printf("Waiting for a sensor datum to come in ... ");
waitForSensorUpdate(&sharedState.sData.utime_sama5, &sensor_data_mutex);
//printf("Done\n");
//printf("Waiting for a odometry datum to come in ... ");
waitForSensorUpdate(&sharedState.oData.utime, &sensor_data_mutex);
//printf("Done\n");
// printf("Waiting for a processed sensor datum to come in ... ");
waitForSensorUpdate(&sharedState.pData.utime_sama5, &sensor_data_mutex);
//printf("Done\n");
pthread_mutex_lock( &sensor_data_mutex);
for(int i=0; i<3; i++) {
sharedState.startup_int[i] = sharedState.sData.gyro_int[i];
}
pthread_mutex_unlock(&sensor_data_mutex);
//pthread_t print_thread;
//pthread_create(&print_thread, NULL, print_handler, &sharedState);
pthread_t plot_thread; // data for plots and graphs
pthread_create(&plot_thread, NULL, plot_handler, &sharedState);
for(;;){;}
// will probably never get here,
// but if for some reason I do, close gracefully
sharedState.running = 0;
usleep(10000); // a little time to let buffers clear
return EXIT_SUCCESS;
}
