void PSMove::initialize(void) { /* Initialize the calibration data structure: */ calibrationData.magnetometer=true; /* Try loading calibration data from a calibration file: */ std::string calibrationFileName="Calibration-PSMove-"; calibrationFileName.append(RawHID::Device::getSerialNumber()); try { IO::FilePtr calibFile=IO::openFile(calibrationFileName.c_str()); loadCalibrationData(*calibFile); } catch(std::runtime_error) { /* Ignore the error and reset calibration data to the default: */ initCalibrationData(getAccelerometerScale(),getGyroscopeScale(),getMagnetometerScale()); } /* Negate the magnetometer's x and z axes: */ for(int j=0;j<4;++j) { calibrationData.magnetometerMatrix(0,j)=-calibrationData.magnetometerMatrix(0,j); calibrationData.magnetometerMatrix(2,j)=-calibrationData.magnetometerMatrix(2,j); } /* Initialize the LED ball color: */ for(int i=0;i<3;++i) ledColor[i]=0x00U; ledColorChanged=true; showSamplingError=true; }
void onInitImpl() { ros::NodeHandle& nh = getNodeHandle(); ros::NodeHandle& pn = getPrivateNodeHandle(); // Retrieve parameters from server std::string device_serial; double open_camera_retry_period; double update_rate; pn.param<std::string>("frame_id", frame_id_, "/camera_optical_frame"); pn.param<std::string>("device_serial", device_serial, ""); pn.param<double>("open_camera_retry_period", open_camera_retry_period, 3); pn.param<double>("update_rate", update_rate, 30); pn.param<bool>("points_with_amplitudes", points_with_amplitudes_, false); // Open camera while (!camera_) { try { camera_ = boost::make_shared<PMDCamboardNano>(device_serial); NODELET_INFO("Opened PMD camera with serial number \"%s\"", camera_->getSerialNumber().c_str()); loadCalibrationData(); camera_->update(); camera_info_ = camera_->getCameraInfo(); } catch (PMDCameraNotOpenedException& e) { if (device_serial != "") NODELET_INFO("Unable to open PMD camera with serial number %s...", device_serial.c_str()); else NODELET_INFO("Unable to open PMD camera..."); } boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period)); } // Advertise topics ros::NodeHandle distance_nh(nh, "distance"); image_transport::ImageTransport distance_it(distance_nh); distance_publisher_ = distance_it.advertiseCamera("image", 1); ros::NodeHandle depth_nh(nh, "depth"); image_transport::ImageTransport depth_it(depth_nh); depth_publisher_ = depth_it.advertiseCamera("image", 1); ros::NodeHandle amplitude_nh(nh, "amplitude"); image_transport::ImageTransport amplitude_it(amplitude_nh); amplitude_publisher_ = amplitude_it.advertiseCamera("image", 1); points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points", 1); // Setup periodic callback to get new data from the camera update_timer_ = nh.createTimer(ros::Rate(update_rate).expectedCycleTime(), &DriverNodelet::updateCallback, this); // Setup dynamic reconfigure server reconfigure_server_.reset(new ReconfigureServer(pn)); reconfigure_server_->setCallback(boost::bind(&DriverNodelet::reconfigureCallback, this, _1, _2)); }
//=====[ Control Loop ]========================================================= void controlSetup(void) { // Init list of synchronous timers LIST_INIT(&timers_lst); // Schedule SMS handling task GSM(gsmSMSDelRead()); timer_setDelay(&sms_tmr, ms_to_ticks(SMS_CHECK_SEC*1000)); timer_setSoftint(&sms_tmr, sms_task, (iptr_t)&sms_tmr); synctimer_add(&sms_tmr, &timers_lst); // Schedule Console handling task timer_setDelay(&cmd_tmr, ms_to_ticks(CMD_CHECK_SEC*1000)); timer_setSoftint(&cmd_tmr, cmd_task, (iptr_t)&cmd_tmr); synctimer_add(&cmd_tmr, &timers_lst); // Setup Button handling task timer_setDelay(&btn_tmr, ms_to_ticks(BTN_CHECK_SEC*1000)); timer_setSoftint(&btn_tmr, btn_task, (iptr_t)&btn_tmr); // Setup console RX timeout console_init(&dbg_port.fd); ser_settimeouts(&dbg_port, 0, 1000); // Dump ADE7753 configuration meter_ade7753_dumpConf(); // Get bitmask of enabled channels chEnabled = ee_getEnabledChMask(); // Get bitmask of enabled channels chCritical = ee_getCriticalChMask(); // Enabling calibration only for enabled channels chCalib = chEnabled; // Setup channels calibration data for (uint8_t ch=0; ch<16; ch++) loadCalibrationData(ch); // Update signal level GSM(updateCSQ()); // Setup Re-Calibration Weeks resetCalibrationCountdown(); // Enabling the watchdog for the control loop WATCHDOG_ENABLE(); // Initi the analog MUX to current channel switchAnalogMux(curCh); }
void controlSetEnabled(uint16_t mask) { uint16_t chNew = (mask & (chEnabled ^ mask)); LOG_INFO("New ENABLED Channels 0x%04X\r\n", chNew); chCalib &= mask; // Remove disabled CHs chCalib |= chNew; // Newly enabled channels chEnabled = mask; // Load calibration data for new channels for (uint8_t pos = 0; chNew && pos < 16; ++pos, chNew>>=1) { if (chNew & BV16(0)) { loadCalibrationData(pos); } } }
void openCamera(ros::Timer &timer) { timer.stop(); while (!camera_) { try { boost::lock_guard<boost::recursive_mutex> lock(config_mutex_); camera_ = boost::make_shared<PMDCamboardNano>(device_serial, plugin_dir, source_plugin, process_plugin); device_serial = camera_->getSerialNumber().c_str(); updater.setHardwareIDf("%s", device_serial.c_str()); NODELET_INFO("Opened PMD camera with serial number \"%s\"", camera_->getSerialNumber().c_str()); loadCalibrationData(); NODELET_INFO("Loaded calibration data"); camera_->update(); camera_info_ = camera_->getCameraInfo(); } catch (PMDCameraNotOpenedException& e) { camera_state_ = CAMERA_NOT_FOUND; if (device_serial != "") { std::stringstream err; err << "Unable to open PMD camera with serial number " << device_serial; state_info_ = err.str(); NODELET_INFO("%s",state_info_.c_str()); } else { state_info_ = "Unable to open PMD camera.."; NODELET_INFO("%s",state_info_.c_str()); } boost::lock_guard<boost::recursive_mutex> lock(config_mutex_); camera_.reset(); } updater.update(); boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period)); } timer.start(); }