bool start() { if (isDSAInitialized_ == false) { //Init tactile data if(!dsadevicestring_.empty()) { try { dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str()); if(!polling_) dsa_->SetFramerate( frequency_, use_rle_ ); else dsa_->SetFramerate( 0, use_rle_ ); ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str()); // ROS_INFO("Set sensitivity to 1.0"); // for(int i=0; i<6; i++) // dsa_->SetMatrixSensitivity(i, 1.0); error_counter_ = 0; isDSAInitialized_ = true; } catch (SDH::cSDHLibraryException* e) { isDSAInitialized_ = false; ROS_ERROR("An exception was caught: %s", e->what()); delete e; shutdown(); return false; } } } return true; }
void pollDsa(){ if(debug_) ROS_DEBUG("pollDsa"); if(isDSAInitialized_) { try { dsa_->SetFramerate( 0, use_rle_ ); readDsaFrame(); } catch (SDH::cSDHLibraryException* e) { ROS_ERROR("An exception was caught: %s", e->what()); delete e; ++error_counter_; } if(error_counter_ > maxerror_) stop(); }else{ start(); } }
/*! * \brief Executes the service callback for init. * * Connects to the hardware and initialized it. * \param req Service request * \param res Service response */ bool srvCallback_Init( cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res ) { if (isInitialized_ == false) { //Init Hand connection try { if(sdhdevicetype_.compare("RS232")==0) { sdh_->OpenRS232( sdhdevicenum_, 115200, 1, sdhdevicestring_.c_str()); ROS_INFO("Initialized RS232 for SDH"); isInitialized_ = true; } if(sdhdevicetype_.compare("PCAN")==0) { ROS_INFO("Starting initializing PEAKCAN"); sdh_->OpenCAN_PEAK(baudrate_, timeout_, id_read_, id_write_, sdhdevicestring_.c_str()); ROS_INFO("Initialized PEAK CAN for SDH"); isInitialized_ = true; } if(sdhdevicetype_.compare("ESD")==0) { ROS_INFO("Starting initializing ESD"); if(strcmp(sdhdevicestring_.c_str(), "/dev/can0") == 0) { ROS_INFO("Initializing ESD on device %s",sdhdevicestring_.c_str()); sdh_->OpenCAN_ESD(0, baudrate_, timeout_, id_read_, id_write_ ); } else if(strcmp(sdhdevicestring_.c_str(), "/dev/can1") == 0) { ROS_INFO("Initializin ESD on device %s",sdhdevicestring_.c_str()); sdh_->OpenCAN_ESD(1, baudrate_, timeout_, id_read_, id_write_ ); } else { ROS_ERROR("Currently only support for /dev/can0 and /dev/can1"); res.success.data = false; res.error_message.data = "Currently only support for /dev/can0 and /dev/can1"; return true; } ROS_INFO("Initialized ESDCAN for SDH"); isInitialized_ = true; } } catch (SDH::cSDHLibraryException* e) { ROS_ERROR("An exception was caught: %s", e->what()); res.success.data = false; res.error_message.data = e->what(); delete e; return true; } //Init tactile data try { dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str()); //dsa_->SetFramerate( 0, true, false ); dsa_->SetFramerate( 1, true ); ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str()); // ROS_INFO("Set sensitivity to 1.0"); // for(int i=0; i<6; i++) // dsa_->SetMatrixSensitivity(i, 1.0); isDSAInitialized_ = true; } catch (SDH::cSDHLibraryException* e) { isDSAInitialized_ = false; ROS_ERROR("An exception was caught: %s", e->what()); res.success.data = false; res.error_message.data = e->what(); delete e; return true; } } else { ROS_WARN("...sdh already initialized..."); res.success.data = true; res.error_message.data = "sdh already initialized"; } res.success.data = true; return true; }