void readDsaFrame()
	{
		if(debug_) ROS_DEBUG("readDsaFrame");

		if(isDSAInitialized_)
		{
			try
			{
				SDH::UInt32 last_time;
				last_time = dsa_->GetFrame().timestamp;
				dsa_->UpdateFrame();
				if(last_time != dsa_->GetFrame().timestamp){ // new data
				    if(error_counter_ > 0) --error_counter_;
				    if(auto_publish_) publishTactileData();
				}
				
			}
			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 Destructor for SdhNode class
		*/
		~DsaNode() 
		{
			if(isDSAInitialized_)
				dsa_->Close();
			if(dsa_)
				delete dsa_;
		}
Exemplo n.º 3
0
		/*!
		* \brief Destructor for SdhNode class
		*/
		~SdhNode() 
		{
			if(isDSAInitialized_)
				dsa_->Close();
			if(isInitialized_)
				sdh_->Close();
			delete sdh_;
		}
	void publishTactileData()
	{
	    if(debug_) ROS_DEBUG("publishTactileData %ul %ul",dsa_->GetFrame().timestamp, last_data_publish_);
	    if(!isDSAInitialized_ || dsa_->GetFrame().timestamp == last_data_publish_) return; // no new frame available
	    last_data_publish_ = dsa_->GetFrame().timestamp;
	    
	    schunk_sdh::TactileSensor msg;
	    msg.header.stamp = ros::Time::now();
	    int m, x, y;
	    msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
	    ROS_ASSERT(dsa_->GetSensorInfo().nb_matrices == dsa_reorder_.size());
	    for ( unsigned int i = 0; i < dsa_reorder_.size(); i++ )
	    {
		    m = dsa_reorder_[i];                                  
		    schunk_sdh::TactileMatrix &tm = msg.tactile_matrix[i];
		    tm.matrix_id = i;
		    tm.cells_x = dsa_->GetMatrixInfo( m ).cells_x;
		    tm.cells_y = dsa_->GetMatrixInfo( m ).cells_y;
		    tm.tactile_array.resize(tm.cells_x * tm.cells_y);
		    for ( y = 0; y < tm.cells_y; y++ )
		    {
			    for ( x = 0; x < tm.cells_x; x++ )
				    tm.tactile_array[tm.cells_x*y + x] = dsa_->GetTexel( m, x, y );
		    }
	    }
	    //publish matrix
	    topicPub_TactileSensor_.publish(msg);
	    
	}
		bool stop(){
			if(dsa_){
			    if(isDSAInitialized_)
				dsa_->Close();
			    delete dsa_;
			}
			dsa_ = 0;
			isDSAInitialized_ = false;
			return true;
		}
Exemplo n.º 6
0
	/*!
	* \brief Main routine to update dsa.
	*
	* Reads out current values.
	*/
	void updateDsa()
	{
		ROS_DEBUG("updateTactileData");

		if(isDSAInitialized_)
		{
			// read tactile data
			for(int i=0; i<7; i++)
			{
				try
				{
					//dsa_->SetFramerate( 0, true, true );
					dsa_->UpdateFrame();
				}
				catch (SDH::cSDHLibraryException* e)
				{
					ROS_ERROR("An exception was caught: %s", e->what());
					delete e;
				}
			}

			cob_sdh::TactileSensor msg;
			msg.header.stamp = ros::Time::now();
			int m, x, y;
			msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
			for ( m = 0; m < dsa_->GetSensorInfo().nb_matrices; m++ )
			{
				cob_sdh::TactileMatrix &tm = msg.tactile_matrix[m];
				tm.matrix_id = m;
				tm.cells_x = dsa_->GetMatrixInfo( m ).cells_x;
				tm.cells_y = dsa_->GetMatrixInfo( m ).cells_y;
				tm.tactile_array.resize(tm.cells_x * tm.cells_y);
				for ( y = 0; y < tm.cells_y; y++ )
				{
					for ( x = 0; x < tm.cells_x; x++ )
						tm.tactile_array[tm.cells_x*y + x] = dsa_->GetTexel( m, x, y );
				}
			}
			//publish matrix
			topicPub_TactileSensor_.publish(msg);
		}
	}	 
		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.º 9
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;
		}