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