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();
}
}
/*!
* \brief Destructor for SdhNode class
*/
~DsaNode()
{
if(isDSAInitialized_)
dsa_->Close();
if(dsa_)
delete dsa_;
}
/*!
* \brief Destructor for SdhNode class
*/
~SdhNode()
{
if(isDSAInitialized_)
dsa_->Close();
if(isInitialized_)
sdh_->Close();
delete sdh_;
}
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);
}
bool stop(){
if(dsa_){
if(isDSAInitialized_)
dsa_->Close();
delete dsa_;
}
dsa_ = 0;
isDSAInitialized_ = false;
return true;
}
/*!
* \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);
}
}
bool start()
{
if (isDSAInitialized_ == false)
{
//Init tactile data
if(!dsadevicestring_.empty()) {
try
{
dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
if(!polling_)
dsa_->SetFramerate( frequency_, use_rle_ );
else
dsa_->SetFramerate( 0, use_rle_ );
ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str());
// ROS_INFO("Set sensitivity to 1.0");
// for(int i=0; i<6; i++)
// dsa_->SetMatrixSensitivity(i, 1.0);
error_counter_ = 0;
isDSAInitialized_ = true;
}
catch (SDH::cSDHLibraryException* e)
{
isDSAInitialized_ = false;
ROS_ERROR("An exception was caught: %s", e->what());
delete e;
shutdown();
return false;
}
}
}
return true;
}
void pollDsa(){
if(debug_) ROS_DEBUG("pollDsa");
if(isDSAInitialized_)
{
try
{
dsa_->SetFramerate( 0, use_rle_ );
readDsaFrame();
}
catch (SDH::cSDHLibraryException* e)
{
ROS_ERROR("An exception was caught: %s", e->what());
delete e;
++error_counter_;
}
if(error_counter_ > maxerror_) stop();
}else{
start();
}
}
/*!
* \brief Executes the service callback for init.
*
* Connects to the hardware and initialized it.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Init( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == false)
{
//Init Hand connection
try
{
if(sdhdevicetype_.compare("RS232")==0)
{
sdh_->OpenRS232( sdhdevicenum_, 115200, 1, sdhdevicestring_.c_str());
ROS_INFO("Initialized RS232 for SDH");
isInitialized_ = true;
}
if(sdhdevicetype_.compare("PCAN")==0)
{
ROS_INFO("Starting initializing PEAKCAN");
sdh_->OpenCAN_PEAK(baudrate_, timeout_, id_read_, id_write_, sdhdevicestring_.c_str());
ROS_INFO("Initialized PEAK CAN for SDH");
isInitialized_ = true;
}
if(sdhdevicetype_.compare("ESD")==0)
{
ROS_INFO("Starting initializing ESD");
if(strcmp(sdhdevicestring_.c_str(), "/dev/can0") == 0)
{
ROS_INFO("Initializing ESD on device %s",sdhdevicestring_.c_str());
sdh_->OpenCAN_ESD(0, baudrate_, timeout_, id_read_, id_write_ );
}
else if(strcmp(sdhdevicestring_.c_str(), "/dev/can1") == 0)
{
ROS_INFO("Initializin ESD on device %s",sdhdevicestring_.c_str());
sdh_->OpenCAN_ESD(1, baudrate_, timeout_, id_read_, id_write_ );
}
else
{
ROS_ERROR("Currently only support for /dev/can0 and /dev/can1");
res.success.data = false;
res.error_message.data = "Currently only support for /dev/can0 and /dev/can1";
return true;
}
ROS_INFO("Initialized ESDCAN for SDH");
isInitialized_ = true;
}
}
catch (SDH::cSDHLibraryException* e)
{
ROS_ERROR("An exception was caught: %s", e->what());
res.success.data = false;
res.error_message.data = e->what();
delete e;
return true;
}
//Init tactile data
try
{
dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
//dsa_->SetFramerate( 0, true, false );
dsa_->SetFramerate( 1, true );
ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str());
// ROS_INFO("Set sensitivity to 1.0");
// for(int i=0; i<6; i++)
// dsa_->SetMatrixSensitivity(i, 1.0);
isDSAInitialized_ = true;
}
catch (SDH::cSDHLibraryException* e)
{
isDSAInitialized_ = false;
ROS_ERROR("An exception was caught: %s", e->what());
res.success.data = false;
res.error_message.data = e->what();
delete e;
return true;
}
}
else
{
ROS_WARN("...sdh already initialized...");
res.success.data = true;
res.error_message.data = "sdh already initialized";
}
res.success.data = true;
return true;
}