void AudioThread::run() {
#ifdef __APPLE__
pthread_t tID = pthread_self(); // ID of this thread
int priority = sched_get_priority_max( SCHED_RR) - 1;
sched_param prio = {priority}; // scheduling priority of thread
pthread_setschedparam(tID, SCHED_RR, &prio);
#endif
std::cout << "Audio thread initializing.." << std::endl;
if (dac.getDeviceCount() < 1) {
std::cout << "No audio devices found!" << std::endl;
return;
}
setupDevice((outputDevice.load() == -1) ? (dac.getDefaultOutputDevice()) : outputDevice.load());
std::cout << "Audio thread started." << std::endl;
inputQueue = (AudioThreadInputQueue *)getInputQueue("AudioDataInput");
threadQueueNotify = (DemodulatorThreadCommandQueue*)getOutputQueue("NotifyQueue");
while (!terminated) {
AudioThreadCommand command;
cmdQueue.pop(command);
if (command.cmd == AudioThreadCommand::AUDIO_THREAD_CMD_SET_DEVICE) {
setupDevice(command.int_value);
}
if (command.cmd == AudioThreadCommand::AUDIO_THREAD_CMD_SET_SAMPLE_RATE) {
setSampleRate(command.int_value);
}
}
if (deviceController[parameters.deviceId] != this) {
deviceController[parameters.deviceId]->removeThread(this);
} else {
try {
if (dac.isStreamOpen()) {
if (dac.isStreamRunning()) {
dac.stopStream();
}
dac.closeStream();
}
} catch (RtAudioError& e) {
e.printMessage();
}
}
if (threadQueueNotify != NULL) {
DemodulatorThreadCommand tCmd(DemodulatorThreadCommand::DEMOD_THREAD_CMD_AUDIO_TERMINATED);
tCmd.context = this;
threadQueueNotify->push(tCmd);
}
std::cout << "Audio thread done." << std::endl;
}
void RS485::openDevice()
{
try
{
if(_fileDescriptor->descriptor != -1) closeDevice();
_lockfile = "/var/lock" + _settings->device.substr(_settings->device.find_last_of('/')) + ".lock";
int lockfileDescriptor = open(_lockfile.c_str(), O_WRONLY | O_EXCL | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
if(lockfileDescriptor == -1)
{
if(errno != EEXIST)
{
_out.printCritical("Couldn't create lockfile " + _lockfile + ": " + strerror(errno));
return;
}
int processID = 0;
std::ifstream lockfileStream(_lockfile.c_str());
lockfileStream >> processID;
if(getpid() != processID && kill(processID, 0) == 0)
{
_out.printCritical("CRC RS485 device is in use: " + _settings->device);
return;
}
unlink(_lockfile.c_str());
lockfileDescriptor = open(_lockfile.c_str(), O_WRONLY | O_EXCL | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
if(lockfileDescriptor == -1)
{
_out.printCritical("Couldn't create lockfile " + _lockfile + ": " + strerror(errno));
return;
}
}
dprintf(lockfileDescriptor, "%10i", getpid());
close(lockfileDescriptor);
//std::string chmod("chmod 666 " + _lockfile);
//system(chmod.c_str());
_fileDescriptor = _bl->fileDescriptorManager.add(open(_settings->device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY ));
if(_fileDescriptor->descriptor == -1)
{
_out.printCritical("Couldn't open CRC RS485 device \"" + _settings->device + "\": " + strerror(errno));
return;
}
setupDevice();
}
catch(const std::exception& ex)
{
_out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(BaseLib::Exception& ex)
{
_out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(...)
{
_out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__);
}
}
void Manager::setup(const std::string & expr)
{
std::string::size_type dp = expr.find('.');
if(dp == std::string::npos)
BOOST_THROW_EXCEPTION(ManagerException() << mstd::error_message("Invalid setup syntax - '.' expected"));
std::string::size_type ep = expr.find('=', dp);
if(ep == std::string::npos)
BOOST_THROW_EXCEPTION(ManagerException() << mstd::error_message("Invalid setup syntax - '=' expected"));
std::string name = expr.substr(0, dp);
boost::trim(name);
std::string prop = expr.substr(dp + 1, ep - dp - 1);
boost::trim(prop);
std::string value = expr.substr(ep + 1);
boost::trim(value);
boost::mutex::scoped_lock lock(mutex_);
if(name == "device")
setupDevice(prop, value, lock);
else if(name == "manager")
{
if(prop == "realtime")
realtime_ = value == "1" || value == "true";
} else if(prop == "level")
setupLevel(name, value, lock);
else if(prop == "group")
setupGroup(name, value, lock);
else
BOOST_THROW_EXCEPTION(ManagerException() << mstd::error_message("Unknown command: " + expr));
}
void QDeclarativeCamera::setDeviceId(const QString &name)
{
if (name == m_currentCameraInfo.deviceName())
return;
setupDevice(name);
}
static int xordevProbe(struct pci_dev *dev, const struct pci_device_id *id) {
int retval = 0;
printk(KERN_INFO "xordev: Probing PCI device %x:%x:%x.", dev->vendor, dev->device, dev->devfn);
TRY(OR_RETURN(retval), retval, pci_enable_device(dev), "not enable PCI device");
TRY(OR_RETURN(retval), retval, setupDevice(dev), "not setup PCI device");
return retval;
}
BOOLEAN openDevice(Device device) {
fprintf(stdout, "- Opening device in %p\n", device);
/* open the video device */
device->fd = EINTR_RETRY(open(device->deviceName, O_RDWR));
if (device->fd < 0) {
perror(device->deviceName);
return FALSE;
}
fprintf(stdout, " - Opened %s with fd=%d\n", device->deviceName, device->fd);
/* try to lock the device */
if (EINTR_RETRY(flock(device->fd, LOCK_EX | LOCK_NB)) < 0) {
perror("FLOCK");
fprintf(stdout, " * Can't lock device\n");
return FALSE;
}
/* check for a valid device */
if (EINTR_RETRY(xioctl(device->fd, VIDIOCGCAP, &device->vcapability)) < 0) {
perror("VIDIOCGCAP");
return FALSE;
}
initializeDevice(device);
showDeviceInformation(device);
return setupDevice(device);
}
void TICC1100::openDevice()
{
try
{
if(_fileDescriptor->descriptor != -1) closeDevice();
_lockfile = GD::bl->settings.lockFilePath() + "LCK.." + _settings->device.substr(_settings->device.find_last_of('/') + 1);
int lockfileDescriptor = open(_lockfile.c_str(), O_WRONLY | O_EXCL | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
if(lockfileDescriptor == -1)
{
if(errno != EEXIST)
{
_out.printCritical("Couldn't create lockfile " + _lockfile + ": " + strerror(errno));
return;
}
int processID = 0;
std::ifstream lockfileStream(_lockfile.c_str());
lockfileStream >> processID;
if(getpid() != processID && kill(processID, 0) == 0)
{
_out.printCritical("Rf device is in use: " + _settings->device);
return;
}
unlink(_lockfile.c_str());
lockfileDescriptor = open(_lockfile.c_str(), O_WRONLY | O_EXCL | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
if(lockfileDescriptor == -1)
{
_out.printCritical("Couldn't create lockfile " + _lockfile + ": " + strerror(errno));
return;
}
}
dprintf(lockfileDescriptor, "%10i", getpid());
close(lockfileDescriptor);
_fileDescriptor = _bl->fileDescriptorManager.add(open(_settings->device.c_str(), O_RDWR | O_NONBLOCK));
usleep(1000);
if(_fileDescriptor->descriptor == -1)
{
_out.printCritical("Couldn't open rf device \"" + _settings->device + "\": " + strerror(errno));
return;
}
setupDevice();
}
catch(const std::exception& ex)
{
_out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(BaseLib::Exception& ex)
{
_out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(...)
{
_out.printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__);
}
}
// MIDI device options.
void ChannelForm::setupMidiDevice (void)
{
Device *pDevice = NULL;
int iMidiItem = m_ui.MidiDeviceComboBox->currentIndex();
if (iMidiItem >= 0 && iMidiItem < m_midiDevices.count())
pDevice = m_midiDevices.at(iMidiItem);
setupDevice(pDevice,
Device::Midi, m_ui.MidiDriverComboBox->currentText());
}
// Audio device options.
void ChannelForm::setupAudioDevice (void)
{
Device *pDevice = NULL;
int iAudioItem = m_ui.AudioDeviceComboBox->currentIndex();
if (iAudioItem >= 0 && iAudioItem < m_audioDevices.count())
pDevice = m_audioDevices.at(iAudioItem);
setupDevice(pDevice,
Device::Audio, m_ui.AudioDriverComboBox->currentText());
}
DirectShowCamera::DirectShowCamera(int id, int width, int height, int frameRate, videoInput* vInput) :
AbstractCamera(id, width, height, frameRate) {
_videoInput = vInput;
//Query camera name
setupDevice();
_name = _videoInput->getDeviceName(_id);
}
void
pcl::OpenNIGrabber::onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode)
{
updateModeMaps (); // registering mapping from config modes to XnModes and vice versa
setupDevice (device_id, depth_mode, image_mode);
rgb_frame_id_ = "/openni_rgb_optical_frame";
depth_frame_id_ = "/openni_depth_optical_frame";
}
int main(void)
{
init();
uint8_t setupOK = setupDevice();
for (;;)
if (setupOK)
loop();
return 0;
}
/**
* @brief Setup default device
* @note If a device is already open, the source will seamlessly switch to the new device.
*/
void CameraSource::setupDefault()
{
QString deviceName = CameraDevice::getDefaultDeviceName();
bool isScreen = CameraDevice::isScreen(deviceName);
VideoMode mode = VideoMode(Settings::getInstance().getScreenRegion());
if (!isScreen) {
mode = VideoMode(Settings::getInstance().getCamVideoRes());
mode.FPS = Settings::getInstance().getCamVideoFPS();
}
setupDevice(deviceName, mode);
}
OptiTrackCamera::OptiTrackCamera(int id, int width, int height, int frameRate, int cameraCollectionIndex) :
AbstractCamera(id, width, height, frameRate), _camera(NULL), _NPFrame(NULL), _cameraCollectionIndex(
cameraCollectionIndex) {
//TODO Dynamic load OptiTrack library
CAMERA_COLLECTION->Item(cameraCollectionIndex, &_camera);
setupDevice();
setResolution(_width, _height);
setFrameRate(_frameRate);
/*Start camera capture*/
_camera->Start();
}
bool dabStick::createPluginWindow (int32_t rate,
QFrame *myFrame,
QSettings *s) {
(void)rate;
this -> myFrame = myFrame;
this -> dabSettings = s;
setupUi (myFrame);
outputRate = rateSelector -> currentText (). toInt ();
inputRate = getInputRate (outputRate);
gains = NULL;
// the connects
setupDevice (inputRate, outputRate);
// The device itself generates the data, so the reader is
// just doing nothing
return true;
}
void QDeclarativeCamera::setPosition(Position position)
{
QCamera::Position pos = QCamera::Position(position);
if (pos == m_currentCameraInfo.position())
return;
QString id;
if (pos == QCamera::UnspecifiedPosition) {
id = QCameraInfo::defaultCamera().deviceName();
} else {
QList<QCameraInfo> cameras = QCameraInfo::availableCameras(pos);
if (!cameras.isEmpty())
id = cameras.first().deviceName();
}
if (!id.isEmpty())
setupDevice(id);
}
bool videoInputCamera::initCamera() {
int dev_count = getDeviceCount();
if (dev_count==0) return false;
if ((cfg->device==SETTING_MIN) || (cfg->device==SETTING_DEFAULT)) cfg->device=0;
else if (cfg->device==SETTING_MAX) cfg->device=dev_count-1;
std::vector<CameraConfig> cfg_list = videoInputCamera::getCameraConfigs(cfg->device);
if (cfg_list.size()==0) return false;
if (cfg->cam_format==FORMAT_UNKNOWN) cfg->cam_format = cfg_list[0].cam_format;
setMinMaxConfig(cfg,cfg_list);
HRESULT hr = setupDevice();
if(FAILED(hr)) return false;
setupFrame();
if (cfg->frame) cam_buffer = new unsigned char[cfg->cam_width*cfg->cam_height*cfg->src_format];
else cam_buffer = new unsigned char[cfg->cam_width*cfg->cam_height*cfg->src_format];
return true;
}
void DriverNodelet::onInitImpl ()
{
ros::NodeHandle& nh = getNodeHandle(); // topics
ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters
// Allow remapping namespaces rgb, ir, depth, depth_registered
image_transport::ImageTransport it(nh);
ros::NodeHandle rgb_nh(nh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
ros::NodeHandle ir_nh(nh, "ir");
image_transport::ImageTransport ir_it(ir_nh);
ros::NodeHandle depth_nh(nh, "depth");
image_transport::ImageTransport depth_it(depth_nh);
ros::NodeHandle depth_registered_nh(nh, "depth_registered");
image_transport::ImageTransport depth_registered_it(depth_registered_nh);
ros::NodeHandle projector_nh(nh, "projector");
rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
publish_rgb_ = publish_ir_ = publish_depth_ = true;
// Check to see if we should enable debugging messages in libfreenect
// libfreenect_debug_ should be set before calling setupDevice
param_nh.param("debug" , libfreenect_debug_, false);
// Initialize the sensor, but don't start any streams yet. That happens in the connection callbacks.
updateModeMaps();
setupDevice();
// Initialize dynamic reconfigure
reconfigure_server_.reset( new ReconfigureServer(param_nh) );
reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));
// Camera TF frames
param_nh.param("rgb_frame_id", rgb_frame_id_, string("/openni_rgb_optical_frame"));
param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
NODELET_INFO("rgb_frame_id = '%s' ", rgb_frame_id_.c_str());
NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());
// Pixel offset between depth and IR images.
// By default assume offset of (5,4) from 9x7 correlation window.
// NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
//param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
//param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);
// The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
// camera_info_manager substitutes this for ${NAME} in the URL.
std::string serial_number = device_->getSerialNumber();
std::string rgb_name, ir_name;
if (serial_number.empty())
{
rgb_name = "rgb";
ir_name = "depth"; /// @todo Make it ir instead?
}
else
{
rgb_name = "rgb_" + serial_number;
ir_name = "depth_" + serial_number;
}
std::string rgb_info_url, ir_info_url;
param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
param_nh.param("depth_camera_info_url", ir_info_url, std::string());
double diagnostics_max_frequency, diagnostics_min_frequency;
double diagnostics_tolerance, diagnostics_window_time;
param_nh.param("enable_rgb_diagnostics", enable_rgb_diagnostics_, false);
param_nh.param("enable_ir_diagnostics", enable_ir_diagnostics_, false);
param_nh.param("enable_depth_diagnostics", enable_depth_diagnostics_, false);
param_nh.param("diagnostics_max_frequency", diagnostics_max_frequency, 30.0);
param_nh.param("diagnostics_min_frequency", diagnostics_min_frequency, 30.0);
param_nh.param("diagnostics_tolerance", diagnostics_tolerance, 0.05);
param_nh.param("diagnostics_window_time", diagnostics_window_time, 5.0);
// Suppress some of the output from loading camera calibrations (kinda hacky)
log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
logger_ccp->setLevel(log4cxx::Level::getFatal());
logger_cim->setLevel(log4cxx::Level::getWarn());
// Also suppress sync warnings from image_transport::CameraSubscriber. When subscribing to
// depth_registered/foo with Freenect registration disabled, the rectify nodelet for depth_registered/
// will complain because it receives depth_registered/camera_info (from the register nodelet), but
// the driver is not publishing depth_registered/image_raw.
log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
logger_its->setLevel(log4cxx::Level::getError());
ros::console::notifyLoggerLevelsChanged();
// Load the saved calibrations, if they exist
rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
ir_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh, ir_name, ir_info_url);
if (!rgb_info_manager_->isCalibrated())
NODELET_WARN("Using default parameters for RGB camera calibration.");
if (!ir_info_manager_->isCalibrated())
NODELET_WARN("Using default parameters for IR camera calibration.");
// Advertise all published topics
{
// Prevent connection callbacks from executing until we've set all the publishers. Otherwise
// connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
// assign to pub_depth_. Then pub_depth_.getNumSubscribers() returns 0, and we fail to start
// the depth generator.
boost::lock_guard<boost::mutex> lock(connect_mutex_);
// Instantiate the diagnostic updater
pub_freq_max_ = diagnostics_max_frequency;
pub_freq_min_ = diagnostics_min_frequency;
diagnostic_updater_.reset(new diagnostic_updater::Updater);
// Set hardware id
std::string hardware_id = std::string(device_->getProductName()) + "-" +
std::string(device_->getSerialNumber());
diagnostic_updater_->setHardwareID(hardware_id);
// Asus Xtion PRO does not have an RGB camera
if (device_->hasImageStream())
{
image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
if (enable_rgb_diagnostics_) {
pub_rgb_freq_.reset(new TopicDiagnostic("RGB Image", *diagnostic_updater_,
FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
diagnostics_tolerance, diagnostics_window_time)));
}
}
if (device_->hasIRStream())
{
image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
if (enable_ir_diagnostics_) {
pub_ir_freq_.reset(new TopicDiagnostic("IR Image", *diagnostic_updater_,
FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
diagnostics_tolerance, diagnostics_window_time)));
}
}
if (device_->hasDepthStream())
{
image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
if (enable_depth_diagnostics_) {
pub_depth_freq_.reset(new TopicDiagnostic("Depth Image", *diagnostic_updater_,
FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
diagnostics_tolerance, diagnostics_window_time)));
}
pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
if (device_->isDepthRegistrationSupported()) {
pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
}
}
}
// Create separate diagnostics thread
close_diagnostics_ = false;
diagnostics_thread_ = boost::thread(boost::bind(&DriverNodelet::updateDiagnostics, this));
// Create watch dog timer callback
if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
{
time_stamp_ = ros::Time(0,0);
watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
}
}
void Dialog::tabChanged(int index)
{
#ifdef QTM_EXAMPLES_SMALL_SCREEN
switch(index) {
case 0:
setupGeneral();
break;
case 1:
setupGeneral();
break;
case 2:
setupDevice();
break;
case 3:
setupDevice();
break;
case 4:
setupDevice();
break;
case 5:
setupDisplay();
break;
case 6:
setupStorage();
break;
case 7:
setupNetwork();
break;
case 8:
setupNetwork();
break;
case 9:
setupNetwork();
break;
case 10:
setupSaver();
break;
};
#else
switch(index) {
case 0:
setupGeneral();
break;
case 1:
setupDevice();
break;
case 2:
setupDisplay();
break;
case 3:
setupStorage();
break;
case 4:
setupNetwork();
break;
case 5:
setupSaver();
break;
};
#endif
}
