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();
		}
	}
예제 #2
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);
		}
	}