dsa_only.cpp
Go to the documentation of this file.
00001 
00060 //##################
00061 //#### includes ####
00062 
00063 // standard includes
00064 #include <unistd.h>
00065 
00066 // ROS includes
00067 #include <ros/ros.h>
00068 
00069 // ROS message includes
00070 #include <schunk_sdh/TactileSensor.h>
00071 #include <schunk_sdh/TactileMatrix.h>
00072 
00073 // ROS service includes
00074 #include <cob_srvs/Trigger.h>
00075 #include <cob_srvs/SetOperationMode.h>
00076 
00077 // ROS diagnostic msgs
00078 #include <diagnostic_msgs/DiagnosticArray.h>
00079 #include <diagnostic_msgs/KeyValue.h>
00080 
00081 #include <schunk_sdh/dsa.h>
00082 
00083 #include <boost/lexical_cast.hpp>
00084 #include <boost/bind.hpp>
00085 
00086 template <typename T> bool read_vector(ros::NodeHandle &n_, const std::string &key, std::vector<T> & res){
00087     XmlRpc::XmlRpcValue namesXmlRpc;
00088     if (!n_.hasParam(key))
00089     {
00090         return false;
00091     }
00092 
00093     n_.getParam(key, namesXmlRpc);
00095     res.resize(namesXmlRpc.size());
00096     for (int i = 0; i < namesXmlRpc.size(); i++)
00097     {
00098         res[i] = (T)namesXmlRpc[i];
00099     }
00100     return true;
00101 }
00102 
00108 class DsaNode
00109 {
00110         public:
00112                 ros::NodeHandle nh_;
00113         private:
00114                 // declaration of topics to publish
00115                 ros::Publisher topicPub_TactileSensor_;
00116                 ros::Publisher topicPub_Diagnostics_;
00117                 
00118                 // topic subscribers
00119 
00120                 // service servers
00121 
00122                 // actionlib server
00123 
00124                 // service clients
00125                 //--
00126 
00127                 // other variables
00128                 SDH::cDSA *dsa_;
00129                 SDH::UInt32 last_data_publish_; // time stamp of last data publishing
00130 
00131                 std::string dsadevicestring_;
00132                 int dsadevicenum_;
00133                 int maxerror_; // maximum error count allowed
00134 
00135                 bool isDSAInitialized_;
00136                 int error_counter_;
00137                 bool polling_; // try to publish on each response
00138                 bool auto_publish_;
00139                 bool use_rle_;
00140                 bool debug_;
00141                 double frequency_;
00142                 
00143                 
00144                 ros::Timer timer_dsa,timer_publish, timer_diag;
00145                 
00146                 std::vector<int> dsa_reorder_;
00147         public:
00153                 DsaNode():nh_("~"),dsa_(0),last_data_publish_(0),isDSAInitialized_(false),error_counter_(0)
00154                 {
00155                         topicPub_Diagnostics_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00156                         topicPub_TactileSensor_ = nh_.advertise<schunk_sdh::TactileSensor>("tactile_data", 1);
00157                 }
00158 
00162                 ~DsaNode() 
00163                 {
00164                         if(isDSAInitialized_)
00165                                 dsa_->Close();
00166                         if(dsa_)
00167                                 delete dsa_;
00168                 }
00169 
00170                 void shutdown(){
00171                     timer_dsa.stop();
00172                     timer_publish.stop();
00173                     timer_diag.stop();
00174                     nh_.shutdown();
00175                 }
00176 
00177 
00181                 bool init()
00182                 {
00183                         // implementation of topics to publish
00184  
00185                         nh_.param("dsadevicestring", dsadevicestring_, std::string(""));
00186                         if (dsadevicestring_.empty()) return false;
00187 
00188                         nh_.param("dsadevicenum", dsadevicenum_, 0);
00189                         nh_.param("maxerror", maxerror_, 8);
00190                         
00191                         double publish_frequency, diag_frequency;
00192                         
00193                         nh_.param("debug", debug_, false);
00194                         nh_.param("polling", polling_, false);
00195                         nh_.param("use_rle", use_rle_, true);
00196                         nh_.param("diag_frequency", diag_frequency, 5.0);
00197                         frequency_ = 30.0;
00198                         if(polling_) nh_.param("poll_frequency", frequency_, 5.0);
00199                         nh_.param("publish_frequency", publish_frequency, 0.0);
00200                         
00201                         auto_publish_ = true;
00202 
00203                                 
00204                         if(polling_){
00205                             timer_dsa = nh_.createTimer(ros::Rate(frequency_).expectedCycleTime(),boost::bind(&DsaNode::pollDsa,  this));
00206                         }else{
00207                             timer_dsa = nh_.createTimer(ros::Rate(frequency_*2.0).expectedCycleTime(),boost::bind(&DsaNode::readDsaFrame,  this));
00208                             if(publish_frequency > 0.0){
00209                                 auto_publish_ = false;
00210                                 timer_publish = nh_.createTimer(ros::Rate(publish_frequency).expectedCycleTime(),boost::bind(&DsaNode::publishTactileData, this));
00211                             }
00212                         }
00213 
00214                         timer_diag = nh_.createTimer(ros::Rate(diag_frequency).expectedCycleTime(),boost::bind(&DsaNode::publishDiagnostics, this));
00215                         
00216                         if(!read_vector(nh_, "dsa_reorder", dsa_reorder_)){
00217                             dsa_reorder_.resize(6);
00218                             dsa_reorder_[0] = 2; // t1
00219                             dsa_reorder_[1] = 3; // t2
00220                             dsa_reorder_[2] = 4; // f11
00221                             dsa_reorder_[3] = 5; // f12
00222                             dsa_reorder_[4] = 0; // f21
00223                             dsa_reorder_[5] = 1; // f22
00224                         }
00225                         
00226                         return true;
00227                 }
00228                 bool stop(){
00229                         if(dsa_){
00230                             if(isDSAInitialized_)
00231                                 dsa_->Close();
00232                             delete dsa_;
00233                         }
00234                         dsa_ = 0;
00235                         isDSAInitialized_ = false;
00236                         return true;
00237                 }
00238 
00239                 bool start()
00240                 {
00241 
00242                         if (isDSAInitialized_ == false)
00243                         {
00244                                 //Init tactile data
00245                                 if(!dsadevicestring_.empty())  {
00246                                         try
00247                                         {
00248                                                 dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
00249                                                 if(!polling_)
00250                                                     dsa_->SetFramerate( frequency_, use_rle_ );
00251                                                 else
00252                                                     dsa_->SetFramerate( 0, use_rle_ );
00253                                                 
00254                                                 ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str());
00255                                                 // ROS_INFO("Set sensitivity to 1.0");
00256                                                 // for(int i=0; i<6; i++)
00257                                                 //      dsa_->SetMatrixSensitivity(i, 1.0);
00258                                                 error_counter_ = 0;
00259                                                 isDSAInitialized_ = true;
00260                                         }
00261                                         catch (SDH::cSDHLibraryException* e)
00262                                         {
00263                                                 isDSAInitialized_ = false;
00264                                                 ROS_ERROR("An exception was caught: %s", e->what());
00265                                                 delete e;
00266                                                 
00267                                                 shutdown();
00268                                                 return false;
00269                                         }
00270                                 }
00271                         }
00272                         
00273                         return true;
00274                 }
00275 
00276         void readDsaFrame()
00277         {
00278                 if(debug_) ROS_DEBUG("readDsaFrame");
00279 
00280                 if(isDSAInitialized_)
00281                 {
00282                         try
00283                         {
00284                                 SDH::UInt32 last_time;
00285                                 last_time = dsa_->GetFrame().timestamp;
00286                                 dsa_->UpdateFrame();
00287                                 if(last_time != dsa_->GetFrame().timestamp){ // new data
00288                                     if(error_counter_ > 0) --error_counter_;
00289                                     if(auto_publish_) publishTactileData();
00290                                 }
00291                                 
00292                         }
00293                         catch (SDH::cSDHLibraryException* e)
00294                         {
00295                                 ROS_ERROR("An exception was caught: %s", e->what());
00296                                 delete e;
00297                                 ++error_counter_;
00298                         }
00299                         if(error_counter_ > maxerror_) stop();
00300 
00301                 }else{
00302                     start();
00303                 }
00304         }
00305         
00306         void pollDsa(){
00307                 if(debug_) ROS_DEBUG("pollDsa");
00308 
00309                 if(isDSAInitialized_)
00310                 {
00311                         try
00312                         {
00313                                 dsa_->SetFramerate( 0, use_rle_ );
00314                                 readDsaFrame();
00315                                 
00316                         }
00317                         catch (SDH::cSDHLibraryException* e)
00318                         {
00319                                 ROS_ERROR("An exception was caught: %s", e->what());
00320                                 delete e;
00321                                 ++error_counter_;
00322                         }
00323                         if(error_counter_ > maxerror_) stop();
00324 
00325                 }else{
00326                     start();
00327                 }
00328         }
00329         
00330         void publishTactileData()
00331         {
00332             if(debug_) ROS_DEBUG("publishTactileData %ul %ul",dsa_->GetFrame().timestamp, last_data_publish_);
00333             if(!isDSAInitialized_ || dsa_->GetFrame().timestamp == last_data_publish_) return; // no new frame available
00334             last_data_publish_ = dsa_->GetFrame().timestamp;
00335             
00336             schunk_sdh::TactileSensor msg;
00337             msg.header.stamp = ros::Time::now();
00338             int m, x, y;
00339             msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
00340             ROS_ASSERT(dsa_->GetSensorInfo().nb_matrices == dsa_reorder_.size());
00341             for ( unsigned int i = 0; i < dsa_reorder_.size(); i++ )
00342             {
00343                     m = dsa_reorder_[i];                                  
00344                     schunk_sdh::TactileMatrix &tm = msg.tactile_matrix[i];
00345                     tm.matrix_id = i;
00346                     tm.cells_x = dsa_->GetMatrixInfo( m ).cells_x;
00347                     tm.cells_y = dsa_->GetMatrixInfo( m ).cells_y;
00348                     tm.tactile_array.resize(tm.cells_x * tm.cells_y);
00349                     for ( y = 0; y < tm.cells_y; y++ )
00350                     {
00351                             for ( x = 0; x < tm.cells_x; x++ )
00352                                     tm.tactile_array[tm.cells_x*y + x] = dsa_->GetTexel( m, x, y );
00353                     }
00354             }
00355             //publish matrix
00356             topicPub_TactileSensor_.publish(msg);
00357             
00358         }
00359         void publishDiagnostics()
00360         {
00361             // publishing diagnotic messages
00362             diagnostic_msgs::DiagnosticArray diagnostics;
00363             diagnostics.status.resize(1);
00364             diagnostics.status[0].name = nh_.getNamespace();
00365             diagnostics.status[0].values.resize(1);
00366             diagnostics.status[0].values[0].key = "error_count";
00367             diagnostics.status[0].values[0].value = boost::lexical_cast<std::string>( error_counter_);
00368 
00369             // set data to diagnostics
00370             if (isDSAInitialized_)
00371             {
00372                 diagnostics.status[0].level = 0;
00373                 diagnostics.status[0].message = "DSA tactile sensing initialized and running";
00374             }
00375             else if(error_counter_ == 0)
00376             {
00377                 diagnostics.status[0].level = 1;
00378                 diagnostics.status[0].message = "DSA not initialized";
00379             }
00380             else
00381             {
00382                 diagnostics.status[0].level = 2;
00383                 diagnostics.status[0].message = "DSA exceeded eror count";
00384             }
00385             // publish diagnostic message
00386             topicPub_Diagnostics_.publish(diagnostics);
00387             if(debug_) ROS_DEBUG_STREAM("publishDiagnostics " << diagnostics);
00388 
00389         }        
00390 }; //DsaNode
00391 
00397 int main(int argc, char** argv)
00398 {
00399         // initialize ROS, spezify name of node
00400         ros::init(argc, argv, "schunk_dsa");
00401         
00402         DsaNode dsa_node;
00403         
00404         if (!dsa_node.init()) return 0;
00405         
00406         dsa_node.start();
00407         
00408         ROS_INFO("...dsa node running...");
00409 
00410         ros::spin();
00411         
00412         ROS_INFO("...dsa node shut down...");
00413         return 0;
00414 }
00415 


schunk_sdh
Author(s): Florian Weisshardt
autogenerated on Mon Oct 6 2014 07:29:15