コード例 #1
0
	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
	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();
		}
	}