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);
}
}
