Subscriber::Subscriber(ros::NodeHandle& nh, okvis::VioInterface* vioInterfacePtr,
const okvis::VioParametersReader& param_reader)
: vioInterface_(vioInterfacePtr)
{
param_reader.getParameters(vioParameters_);
imgTransport_ = 0;
if (param_reader.useDriver) {
#ifdef HAVE_LIBVISENSOR
if(param_reader.viSensor != nullptr)
sensor_ = std::static_pointer_cast<visensor::ViSensorDriver>(param_reader.viSensor);
initialiseDriverCallbacks();
std::vector<unsigned int> camRate(
vioParameters_.nCameraSystem.numCameras(),
vioParameters_.sensors_information.cameraRate);
startSensors(camRate, vioParameters_.imu.rate);
// init dynamic reconfigure
cameraConfigReconfigureService_.setCallback(boost::bind(&Subscriber::configCallback, this, _1, _2));
#else
LOG(ERROR) << "Configuration specified to directly access the driver. "
<< "However the visensor library was not found. Trying to set up ROS nodehandle instead";
setNodeHandle(nh);
#endif
} else
setNodeHandle(nh);
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
chEvtInit(&eventImuIrq);
chEvtInit(&eventMagnIrq);
chEvtInit(&eventImuRead);
chEvtInit(&eventMagnRead);
chEvtInit(&eventEKFDone);
palSetPadMode(GPIOB, 3, PAL_MODE_OUTPUT_PUSHPULL); // BLUE
palSetPadMode(GPIOB, 4, PAL_MODE_OUTPUT_PUSHPULL); // GREEN
palSetPadMode(GPIOB, 5, PAL_MODE_OUTPUT_PUSHPULL); // RED
chThdCreateStatic(waThreadLed, sizeof(waThreadLed), NORMALPRIO, ThreadLed, NULL );
I2CInitLocal();
configInit();
mavlinkInit();
initSensors();
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
TIM_Cmd(TIM5, ENABLE);
startEstimation();
startSensors();
radioInit();
motorsInit();
extStart(&EXTD1, &extcfg);
extChannelEnable(&EXTD1, 0);
extChannelEnable(&EXTD1, 1);
chEvtBroadcastFlags(&eventEKFDone, EVT_EKF_DONE);
while (TRUE) {
chThdSleepMilliseconds(1000);
}
}
THD_FUNCTION(Thread4, arg)
{
(void)arg;
startAdc(); /* ADC runs in continuous mode with DMA */
startSensors();
}
int main(void) {
fprintf(stderr, "Beginning Structure Test\n");
//make stdin non blocking
int flags = fcntl(fileno(stdin), F_GETFL, 0);
flags |= O_NONBLOCK;
flags = fcntl(fileno(stdin), F_SETFL, flags);
char userInput;
// setPriority();
fprintf(stderr, "Connecting to sensors.\n");
startSensors();
printf("Here 3");
//start data collection and print threads, initialize necessary mutex's
startThreads();
fprintf(stderr, "Collecting data.\n");
data.outFile = fopen("/home/pi/Desktop/ArmTrack/ArmTrackData.bin", "wb");
data.errors = 0;
data.reads = 0;
struct timeval last;
struct timeval curr;
struct timeval temp;
gettimeofday(&curr, NULL); //update current time
while(read(fileno(stdin), &userInput, 1) < 0) {
last.tv_sec = curr.tv_sec; last.tv_usec = curr.tv_usec; //update last time
gettimeofday(&curr, NULL); //update current time
data.time += (curr.tv_sec - last.tv_sec) + (curr.tv_usec - last.tv_usec) * .000001; //increment by difference between last and current time
getData();
//signal print thread
while (data.controlValues[4] != 2) {};
pthread_mutex_lock(&threadLocks[4]);
data.controlValues[4] = 1;
pthread_cond_signal(&threadSignals[4]);
pthread_mutex_unlock(&threadLocks[4]);
checkSensors();
//wait for 25ms cycle length
if (data.EMG.id == -1) {
do {
gettimeofday(&temp, NULL);
} while ( (temp.tv_sec - curr.tv_sec) + (temp.tv_usec - curr.tv_usec) * .000001 < .024993);
}
if (data.time > 13) {
endSession();
return 1;
}
}
endSession();
return 1;
}
