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);
}
Esempio n. 2
0
/*
 * 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);
	}
}
Esempio n. 3
0
THD_FUNCTION(Thread4, arg)
{
    (void)arg;
    startAdc(); /* ADC runs in continuous mode with DMA */
    startSensors();
}
Esempio n. 4
0
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;
}