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