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); }
/*! * \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); } }