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();
		}
	}
Exemplo n.º 3
0
		/*!
		* \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;
		}