void ImuRosI::initDevice()
{
ROS_INFO("Opening device");
open(-1);
ROS_INFO("Waiting for IMU to be attached...");
int result = waitForAttachment(10000);
if(result)
{
is_connected_ = false;
error_number_ = result;
diag_updater_.force_update();
const char *err;
CPhidget_getErrorDescription(result, &err);
ROS_FATAL("Problem waiting for IMU attachment: %s Make sure the USB cable is connected and you have executed the phidgets_api/share/setup-udev.sh script.", err);
}
// calibrate on startup
calibrate();
// set the hardware id for diagnostics
diag_updater_.setHardwareIDf("%s-%d",
getDeviceName().c_str(),
getDeviceSerialNumber());
}
/*
* Retrieve a specific instance given an object path
*/
wbem::framework::Instance* wbem::performance::PerformanceMetricFactory::getInstance(
framework::ObjectPath &path, framework::attribute_names_t &attributes)
throw (wbem::framework::Exception)
{
LogEnterExit logging(__FUNCTION__, __FILE__, __LINE__);
// Verify attributes
checkAttributes(attributes);
framework::Instance *pInstance = NULL;
try
{
pInstance = new framework::Instance(path);
framework::Attribute instanceIdAttr = path.getKeyValue(INSTANCEID_KEY);
std::string deviceUid;
metric_type metric;
if (!splitInstanceID(instanceIdAttr, deviceUid, metric))
{
throw framework::ExceptionBadParameter(instanceIdAttr.asStr().c_str());
}
NVM_UID nvmUid;
uid_copy(deviceUid.c_str(), nvmUid);
// serialNumberStr is used in more than 1 attribute so getting here.
std::string serialNumberStr = getDeviceSerialNumber(nvmUid);
std::string metricName = getMetricElementNameFromType(metric) + " " + serialNumberStr;
framework::Attribute elementNameAttr(metricName, false);
(*pInstance).setAttribute(wbem::ELEMENTNAME_KEY, elementNameAttr, attributes);
const std::string metricDefId
= PerformanceMetricDefinitionFactory::getMetricId(metric);
framework::Attribute metricDefinitionAttr(metricDefId, false);
(*pInstance).setAttribute(wbem::METRICDEFINITION_ID_KEY, metricDefinitionAttr, attributes);
std::string metricDimm = METRIC_DIMM_STR + serialNumberStr;
framework::Attribute measuredElementNameAttr(metricDimm, false);
(*pInstance).setAttribute(wbem::MEASUREDELEMENTNAME_KEY, measuredElementNameAttr, attributes);
NVM_UINT64 metricValue = getValueForDeviceMetric(nvmUid, metric);
std::ostringstream stream;
stream << metricValue;
framework::Attribute metricValueAttr(stream.str(), false);
(*pInstance).setAttribute(wbem::METRICVALUE_KEY, metricValueAttr, attributes);
}
catch (framework::Exception) // clean up and re-throw
{
if (pInstance != NULL)
{
delete pInstance;
pInstance = NULL;
}
throw;
}
return pInstance;
}
void ImuRosI::phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (is_connected_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "The Phidget is connected.");
stat.add("Device Serial Number", getDeviceSerialNumber());
stat.add("Device Name", getDeviceName());
stat.add("Device Type", getDeviceType());
}
else
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget is not connected. Check the USB.");
}
if (error_number_ != 0)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget reports error.");
stat.add("Error Number", error_number_);
stat.add("Error message", getErrorDescription(error_number_));
}
}
void Phidget::detachHandler()
{
cout << "Phidget detached : " << getDeviceSerialNumber() << endl;
}
void list_devices(std::string &target_name, std::vector<VIDEO_DEVICE> &device_lists)
{
device_lists.clear();
DIR *dp;
struct dirent *ep;
dev_vec files;
dev_map links;
card_map cards;
struct v4l2_capability vcap;
dp = opendir("/dev");
if (dp == NULL) {
perror ("Couldn't open the directory");
return;
}
while ((ep = readdir(dp)))
if (is_v4l_dev(ep->d_name))
files.push_back(std::string("/dev/") + ep->d_name);
closedir(dp);
/* Find device nodes which are links to other device nodes */
for (dev_vec::iterator iter = files.begin();
iter != files.end(); ) {
char link[64+1];
int link_len;
std::string target;
link_len = readlink(iter->c_str(), link, 64);
if (link_len < 0) { /* Not a link or error */
iter++;
continue;
}
link[link_len] = '\0';
/* Only remove from files list if target itself is in list */
if (link[0] != '/') /* Relative link */
target = std::string("/dev/");
target += link;
if (find(files.begin(), files.end(), target) == files.end()) {
iter++;
continue;
}
/* Move the device node from files to links */
if (links[target].empty())
links[target] = *iter;
else
links[target] += ", " + *iter;
files.erase(iter);
}
std::sort(files.begin(), files.end(), sort_on_device_name);
for (dev_vec::iterator iter = files.begin();
iter != files.end(); ++iter)
{
//printf("process %s\n", iter->c_str());
int fd = open(iter->c_str(), O_RDWR);
if (fd < 0)
continue;
doioctl(fd, VIDIOC_QUERYCAP, &vcap);
close(fd);
if(1)
{
printf("\nVIDIOC_QUERYCAP - %s\n"
" driver = %s\n"
" card = %s\n"
" bus_info = %s\n"
" version = %d\n"
" capabilities = %d\n"
" device_caps = %d\n"
" reserved[3] = [%d, %d, %d]\n\n",
iter->c_str(),
vcap.driver,
vcap.card,
vcap.bus_info,
vcap.version,
vcap.capabilities,
67108865/*vcap.device_caps*/,
vcap.reserved[0], vcap.reserved[1], vcap.reserved[2] );
}
std::string card_name = (const char *)vcap.card;
if(target_name == card_name)
{
std::string bus_info((const char *)vcap.bus_info);
cards[bus_info].push_back(*iter);
}
}
for (card_map::iterator iter = cards.begin();
iter != cards.end(); ++iter) {
VIDEO_DEVICE device;
device.card_name = iter->first;
//get serial number
if(1)
{
device.serial_number = getDeviceSerialNumber((iter->second)[0].c_str());
}
for(int i=0; i<(iter->second).size(); ++i)
{
device.video_names.push_back((iter->second)[i]);
}
device_lists.push_back(device);
}
}
