phidgetik_ros.cpp
Go to the documentation of this file.
00001 
00060 #include <cob_phidgets/phidgetik_ros.h>
00061 
00062 PhidgetIKROS::PhidgetIKROS(ros::NodeHandle nh, int serial_num, std::string board_name, XmlRpc::XmlRpcValue* sensor_params, SensingMode mode)
00063         :PhidgetIK(mode), _nh(nh), _serial_num(serial_num)
00064 {
00065         ros::NodeHandle tmpHandle("~");
00066         ros::NodeHandle nodeHandle(tmpHandle, board_name);
00067         _outputChanged.updated=false;
00068         _outputChanged.index=-1;
00069         _outputChanged.state=0;
00070 
00071         _pubAnalog = _nh.advertise<cob_phidgets::AnalogSensor>("analog_sensors", 1);
00072         _pubDigital = _nh.advertise<cob_phidgets::DigitalSensor>("digital_sensors", 1);
00073         _subDigital = _nh.subscribe("set_digital_sensor", 1, &PhidgetIKROS::onDigitalOutCallback, this);
00074 
00075         _srvDigitalOut = nodeHandle.advertiseService("set_digital", &PhidgetIKROS::setDigitalOutCallback, this);
00076         _srvDataRate = nodeHandle.advertiseService("set_data_rate", &PhidgetIKROS::setDataRateCallback, this);
00077         _srvTriggerValue = nodeHandle.advertiseService("set_trigger_value", &PhidgetIKROS::setTriggerValueCallback, this);
00078 
00079         if(init(_serial_num) != EPHIDGET_OK)
00080         {
00081                 ROS_ERROR("Error open Phidget Board on serial %d. Message: %s",_serial_num, this->getErrorDescription(this->getError()).c_str());
00082         }
00083         if(waitForAttachment(10000) != EPHIDGET_OK)
00084         {
00085                 ROS_ERROR("Error waiting for Attachment. Message: %s",this->getErrorDescription(this->getError()).c_str());
00086         }
00087         readParams(sensor_params);
00088 }
00089 
00090 PhidgetIKROS::~PhidgetIKROS()
00091 {
00092 }
00093 
00094 auto PhidgetIKROS::readParams(XmlRpc::XmlRpcValue* sensor_params) -> void
00095 {
00096         if(sensor_params != nullptr)
00097         {
00098                 for(auto& sensor : *sensor_params)
00099                 {
00100                         std::string name = sensor.first;
00101                         XmlRpc::XmlRpcValue value = sensor.second;
00102                         if(!value.hasMember("type"))
00103                         {
00104                                 ROS_ERROR("Sensor Param '%s' has no 'type' member. Ignoring param!", name.c_str());
00105                                 continue;
00106                         }
00107                         if(!value.hasMember("index"))
00108                         {
00109                                 ROS_ERROR("Sensor Param '%s' has no 'index' member. Ignoring param!", name.c_str());
00110                                 continue;
00111                         }
00112                         XmlRpc::XmlRpcValue value_type = value["type"];
00113                         XmlRpc::XmlRpcValue value_index = value["index"];
00114                         std::string type = value_type;
00115                         int index = value_index;
00116 
00117                         if(type == "analog")
00118                                 _indexNameMapAnalog.insert(std::make_pair(index, name));
00119                         else if(type == "digital_in")
00120                                 _indexNameMapDigitalIn.insert(std::make_pair(index, name));
00121                         else if(type == "digital_out")
00122                                 _indexNameMapDigitalOut.insert(std::make_pair(index, name));
00123                         else
00124                                 ROS_ERROR("Type '%s' in sensor param '%s' is unkown", type.c_str(), name.c_str());
00125 
00126                         if(value.hasMember("change_trigger"))
00127                         {
00128                                 XmlRpc::XmlRpcValue value_change_trigger = value["change_trigger"];
00129                                 int change_trigger = value_change_trigger;
00130                                 ROS_WARN("Setting change trigger to %d for sensor %s with index %d ",change_trigger, name.c_str(), index);
00131                                 setSensorChangeTrigger(index, change_trigger);
00132                         }
00133                         if(value.hasMember("data_rate"))
00134                         {
00135                                 XmlRpc::XmlRpcValue value_data_rate = value["data_rate"];
00136                                 int data_rate = value_data_rate;
00137                                 ROS_WARN("Setting data rate to %d for sensor %s with index %d ",data_rate, name.c_str(), index);
00138                                 setDataRate(index, data_rate);
00139                         }
00140                 }
00141         }
00142         //fill up rest of maps with default values
00143         int count = this->getInputCount();
00144         for(int i = 0; i < count; i++)
00145         {
00146                 _indexNameMapItr = _indexNameMapDigitalIn.find(i);
00147                 if(_indexNameMapItr == _indexNameMapDigitalIn.end())
00148                 {
00149                         std::stringstream ss;
00150                         ss << getDeviceSerialNumber() << "/" << "in/" << i;
00151                         _indexNameMapDigitalIn.insert(std::make_pair(i, ss.str()));
00152                 }
00153         }
00154         count = this->getOutputCount();
00155         for(int i = 0; i < count; i++)
00156         {
00157                 _indexNameMapItr = _indexNameMapDigitalOut.find(i);
00158                 if(_indexNameMapItr == _indexNameMapDigitalOut.end())
00159                 {
00160                         std::stringstream ss;
00161                         ss << getDeviceSerialNumber() << "/" << "out/" << i;
00162                         _indexNameMapDigitalOut.insert(std::make_pair(i, ss.str()));
00163                 }
00164         }
00165         count = this->getSensorCount();
00166         for(int i = 0; i < count; i++)
00167         {
00168                 _indexNameMapItr = _indexNameMapAnalog.find(i);
00169                 if(_indexNameMapItr == _indexNameMapAnalog.end())
00170                 {
00171                         std::stringstream ss;
00172                         ss << getDeviceSerialNumber() << "/" << i;
00173                         _indexNameMapAnalog.insert(std::make_pair(i, ss.str()));
00174                 }
00175         }
00176 
00177         //fill up reverse mapping
00178         count = this->getInputCount();
00179         for(int i = 0; i < count; i++)
00180         {
00181                 _indexNameMapItr = _indexNameMapDigitalIn.find(i);
00182                 if(_indexNameMapItr != _indexNameMapDigitalIn.end())
00183                 {
00184                         std::stringstream ss;
00185                         ss << getDeviceSerialNumber() << "/" << "in/" << i;
00186 
00187                         _indexNameMapDigitalInRev.insert(std::make_pair(_indexNameMapDigitalIn[i],i));
00188                 }
00189         }
00190         count = this->getOutputCount();
00191         for(int i = 0; i < count; i++)
00192         {
00193                 _indexNameMapItr = _indexNameMapDigitalOut.find(i);
00194                 if(_indexNameMapItr != _indexNameMapDigitalOut.end())
00195                 {
00196                         _indexNameMapDigitalOutRev.insert(std::make_pair(_indexNameMapDigitalOut[i],i));
00197                 }
00198         }
00199         count = this->getSensorCount();
00200         for(int i = 0; i < count; i++)
00201         {
00202                 _indexNameMapItr = _indexNameMapAnalog.find(i);
00203                 if(_indexNameMapItr != _indexNameMapAnalog.end())
00204                 {
00205                         _indexNameMapAnalogRev.insert(std::make_pair(_indexNameMapAnalog[i],i));
00206                 }
00207         }
00208 }
00209 
00210 auto PhidgetIKROS::update() -> void
00211 {
00212         int count = this->getInputCount();
00213         cob_phidgets::DigitalSensor msg_digit;
00214         std::vector<std::string> names;
00215         std::vector<signed char> states;
00216 
00217         //------- publish digital input states ----------//
00218         for(int i = 0; i < count; i++)
00219         {
00220                 std::string name;
00221                 _indexNameMapItr = _indexNameMapDigitalIn.find(i);
00222                 if(_indexNameMapItr != _indexNameMapDigitalIn.end())
00223                         name = (*_indexNameMapItr).second;
00224                 names.push_back(name);
00225                 states.push_back(this->getInputState(i));
00226         }
00227         msg_digit.header.stamp = ros::Time::now();
00228         msg_digit.uri = names;
00229         msg_digit.state = states;
00230 
00231         _pubDigital.publish(msg_digit);
00232 
00233         //------- publish digital output states ----------//
00234         names.clear();
00235         states.clear();
00236         count = this->getOutputCount();
00237         for(int i = 0; i < count; i++)
00238         {
00239                 std::string name;
00240                 _indexNameMapItr = _indexNameMapDigitalOut.find(i);
00241                 if(_indexNameMapItr != _indexNameMapDigitalOut.end())
00242                         name = (*_indexNameMapItr).second;
00243                 names.push_back(name);
00244                 states.push_back(this->getOutputState(i));
00245         }
00246         msg_digit.header.stamp = ros::Time::now();
00247         msg_digit.uri = names;
00248         msg_digit.state = states;
00249 
00250         _pubDigital.publish(msg_digit);
00251 
00252 
00253         //------- publish analog input states ----------//
00254         cob_phidgets::AnalogSensor msg_analog;
00255         names.clear();
00256         std::vector<short int> values;
00257         count = this->getSensorCount();
00258         for(int i = 0; i < count; i++)
00259         {
00260                 std::string name;
00261                 _indexNameMapItr = _indexNameMapAnalog.find(i);
00262                 if(_indexNameMapItr != _indexNameMapAnalog.end())
00263                         name = (*_indexNameMapItr).second;
00264                 names.push_back(name);
00265                 values.push_back(this->getSensorValue(i));
00266         }
00267         msg_analog.header.stamp = ros::Time::now();
00268         msg_analog.uri = names;
00269         msg_analog.value = values;
00270 
00271         _pubAnalog.publish(msg_analog);
00272 }
00273 
00274 auto PhidgetIKROS::inputChangeHandler(int index, int inputState) -> int
00275 {
00276         ROS_DEBUG("Board %d: Digital Input %d changed to State: %d", _serial_num, index, inputState);
00277         cob_phidgets::DigitalSensor msg;
00278         std::vector<std::string> names;
00279         std::vector<signed char> states;
00280 
00281         std::string name;
00282         _indexNameMapItr = _indexNameMapAnalog.find(index);
00283         if(_indexNameMapItr != _indexNameMapAnalog.end())
00284                 name = (*_indexNameMapItr).second;
00285         names.push_back(name);
00286         states.push_back(inputState);
00287 
00288         msg.header.stamp = ros::Time::now();
00289         msg.uri = names;
00290         msg.state = states;
00291         _pubDigital.publish(msg);
00292 
00293         return 0;
00294 }
00295 
00296 auto PhidgetIKROS::outputChangeHandler(int index, int outputState) -> int
00297 {
00298         ROS_DEBUG("Board %d: Digital Output %d changed to State: %d", _serial_num, index, outputState);
00299         std::lock_guard<std::mutex> lock{_mutex};
00300         _outputChanged.updated = true;
00301         _outputChanged.index = index;
00302         _outputChanged.state = outputState;
00303         return 0;
00304 }
00305 auto PhidgetIKROS::sensorChangeHandler(int index, int sensorValue) -> int
00306 {
00307         ROS_DEBUG("Board %d: Analog Input %d changed to Value: %d", _serial_num, index, sensorValue);
00308         cob_phidgets::AnalogSensor msg;
00309         std::vector<std::string> names;
00310         std::vector<short int> values;
00311 
00312         std::string name;
00313         _indexNameMapItr = _indexNameMapAnalog.find(index);
00314         if(_indexNameMapItr != _indexNameMapAnalog.end())
00315                 name = (*_indexNameMapItr).second;
00316         names.push_back(name);
00317         values.push_back(sensorValue);
00318 
00319         msg.header.stamp = ros::Time::now();
00320         msg.uri = names;
00321         msg.value = values;
00322 
00323         _pubAnalog.publish(msg);
00324 
00325         return 0;
00326 }
00327 
00328 auto PhidgetIKROS::setDigitalOutCallback(cob_phidgets::SetDigitalSensor::Request &req,
00329                                                                                 cob_phidgets::SetDigitalSensor::Response &res) -> bool
00330 {
00331         bool ret = false;
00332         _mutex.lock();
00333         _outputChanged.updated=false;
00334         _outputChanged.index=-1;
00335         _outputChanged.state=0;
00336         _mutex.unlock();
00337 
00338         // this check is nessesary because [] operator on Maps inserts an element if it is not found
00339         _indexNameMapRevItr = _indexNameMapDigitalOutRev.find(req.uri);
00340         if(_indexNameMapRevItr != _indexNameMapDigitalOutRev.end())
00341         {
00342                 ROS_INFO("Setting digital output %i to state %i", _indexNameMapDigitalOutRev[req.uri], req.state);
00343                 this->setOutputState(_indexNameMapDigitalOutRev[req.uri], req.state);
00344 
00345                 ros::Time start = ros::Time::now();
00346                 while((ros::Time::now().toSec() - start.toSec()) < 1.0)
00347                 {
00348                         _mutex.lock();
00349                         if(_outputChanged.updated == true)
00350                         {
00351                                 _mutex.unlock();
00352                                 break;
00353                         }
00354                         _mutex.unlock();
00355 
00356                         ros::Duration(0.025).sleep();
00357                 }
00358                 _mutex.lock();
00359                 res.uri = _indexNameMapDigitalOut[_outputChanged.index];
00360                 res.state = _outputChanged.state;
00361                 ROS_DEBUG("Sending response: updated: %u, index: %d, state: %d",_outputChanged.updated, _outputChanged.index, _outputChanged.state);
00362                 ret = (_outputChanged.updated && (_outputChanged.index == _indexNameMapDigitalOutRev[req.uri]));
00363                 _mutex.unlock();
00364         }
00365         else
00366         {
00367                 ROS_DEBUG("Could not find uri '%s' inside port uri mapping", req.uri.c_str());
00368                 res.uri = req.uri;
00369                 res.state = req.state;
00370                 ret = false;
00371         }
00372 
00373         return ret;
00374 }
00375 
00376 auto PhidgetIKROS::onDigitalOutCallback(const cob_phidgets::DigitalSensorConstPtr& msg) -> void
00377 {
00378         if(msg->uri.size() == msg->state.size())
00379         {
00380                 for(size_t i = 0; i < msg->uri.size(); i++)
00381                 {
00382                         // this check is nessesary because [] operator on Maps inserts an element if it is not found
00383                         _indexNameMapRevItr = _indexNameMapDigitalOutRev.find(msg->uri[i]);
00384                         if(_indexNameMapRevItr != _indexNameMapDigitalOutRev.end())
00385                         {
00386                                 ROS_INFO("Setting digital output %i to state %i", _indexNameMapDigitalOutRev[msg->uri[i]], msg->state[i]);
00387                                 this->setOutputState(_indexNameMapDigitalOutRev[msg->uri[i]], msg->state[i]);
00388                         }
00389                         else
00390                                 ROS_DEBUG("Could not find uri '%s' inside port uri mapping", msg->uri[i].c_str());
00391                 }
00392         }
00393         else
00394         {
00395                 ROS_ERROR("Received message with different uri and state container sizes");
00396         }
00397 }
00398 
00399 auto PhidgetIKROS::setDataRateCallback(cob_phidgets::SetDataRate::Request &req,
00400                                                                                 cob_phidgets::SetDataRate::Response &res) -> bool
00401 {
00402         this->setDataRate(req.index, req.data_rate);
00403         return true;
00404 }
00405 auto PhidgetIKROS::setTriggerValueCallback(cob_phidgets::SetTriggerValue::Request &req,
00406                                                                                 cob_phidgets::SetTriggerValue::Response &res) -> bool
00407 {
00408         this->setSensorChangeTrigger(req.index, req.trigger_value);
00409         return true;
00410 }
00411 
00412 auto PhidgetIKROS::attachHandler() -> int
00413 {
00414         int serialNo, version, numInputs, numOutputs, millis;
00415         int numSensors, triggerVal, ratiometric;
00416         const char *ptr, *name;
00417 
00418         CPhidget_getDeviceName((CPhidgetHandle)_iKitHandle, &name);
00419         CPhidget_getDeviceType((CPhidgetHandle)_iKitHandle, &ptr);
00420         CPhidget_getSerialNumber((CPhidgetHandle)_iKitHandle, &serialNo);
00421         CPhidget_getDeviceVersion((CPhidgetHandle)_iKitHandle, &version);
00422 
00423         CPhidgetInterfaceKit_getInputCount(_iKitHandle, &numInputs);
00424         CPhidgetInterfaceKit_getOutputCount(_iKitHandle, &numOutputs);
00425         CPhidgetInterfaceKit_getSensorCount(_iKitHandle, &numSensors);
00426         CPhidgetInterfaceKit_getRatiometric(_iKitHandle, &ratiometric);
00427 
00428         ROS_INFO("%s %d attached!!", name, serialNo);
00429 
00430         ROS_DEBUG("%s", ptr);
00431         ROS_DEBUG("Serial Number: %d\tVersion: %d", serialNo, version);
00432         ROS_DEBUG("Num Digital Inputs: %d\tNum Digital Outputs: %d", numInputs, numOutputs);
00433         ROS_DEBUG("Num Sensors: %d", numSensors);
00434         ROS_DEBUG("Ratiometric: %d", ratiometric);
00435 
00436         for(int i = 0; i < numSensors; i++)
00437         {
00438                 CPhidgetInterfaceKit_getSensorChangeTrigger(_iKitHandle, i, &triggerVal);
00439                 CPhidgetInterfaceKit_getDataRate(_iKitHandle, i, &millis);
00440 
00441                 ROS_DEBUG("Sensor#: %d > Sensitivity Trigger: %d", i, triggerVal);
00442                 ROS_DEBUG("Sensor#: %d > Data Rate: %d", i, millis);
00443         }
00444 
00445         return 0;
00446 }
00447 
00448 auto PhidgetIKROS::detachHandler() -> int
00449 {
00450         int serial_number;
00451     const char *device_name;
00452 
00453     CPhidget_getDeviceName ((CPhidgetHandle)_iKitHandle, &device_name);
00454     CPhidget_getSerialNumber((CPhidgetHandle)_iKitHandle, &serial_number);
00455     ROS_INFO("%s Serial number %d detached!", device_name, serial_number);
00456     return 0;
00457 }


cob_phidgets
Author(s): Florian Weisshardt, Benjamin Maidel
autogenerated on Sun Oct 5 2014 23:10:20