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