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