cob_sdh: cob_sdh_driver.cpp Source File

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: sdh
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00017  * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00018  *
00019  * Date of creation: Jan 2010
00020  * ToDo:
00021  *
00022  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00023  *
00024  * Redistribution and use in source and binary forms, with or without
00025  * modification, are permitted provided that the following conditions are met:
00026  *
00027  *     * Redistributions of source code must retain the above copyright
00028  *       notice, this list of conditions and the following disclaimer.
00029  *     * Redistributions in binary form must reproduce the above copyright
00030  *       notice, this list of conditions and the following disclaimer in the
00031  *       documentation and/or other materials provided with the distribution.
00032  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00033  *       Engineering and Automation (IPA) nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as 
00039  * published by the Free Software Foundation, either version 3 of the 
00040  * License, or (at your option) any later version.
00041  * 
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL for more details.
00046  * 
00047  * You should have received a copy of the GNU Lesser General Public 
00048  * License LGPL along with this program. 
00049  * If not, see <http://www.gnu.org/licenses/>.
00050  *
00051  ****************************************************************/
00052 
00053 //#define USE_ESD
00054 
00055 //##################
00056 //#### includes ####
00057 
00058 // standard includes
00059 //--
00060 #include <unistd.h>
00061 
00062 // ROS includes
00063 #include <ros/ros.h>
00064 #include <urdf/model.h>
00065 #include <actionlib/server/simple_action_server.h>
00066 
00067 // ROS message includes
00068 #include <cob_msgs/JointCommand.h>
00069 #include <sensor_msgs/JointState.h>
00070 #include <cob_actions/JointCommandAction.h>
00071 #include <cob_msgs/TactileSensor.h>
00072 #include <cob_msgs/TactileMatrix.h>
00073 #include <cob_msgs/TactileForce.h>
00074 #include <cob_msgs/ContactArea.h>
00075 
00076 // ROS service includes
00077 #include <cob_srvs/Trigger.h>
00078 #include <cob_srvs/SetOperationMode.h>
00079 #include <cob_srvs/GetFingerXYZ.h>
00080 #include <cob_srvs/demoinfo.h>
00081 #include <cob_srvs/Force.h>
00082 
00083 // external includes
00084 #include <cob_sdh/sdh.h>
00085 #include <cob_sdh/dsa.h>
00086 #include <cob_sdh/dsaboost.h>
00087 
00088 //########################
00089 //#### sdh node class ####
00090 class SdhDriverNode
00091 {
00092         public:
00093                 // create a handle for this node, initialize node
00094                 ros::NodeHandle nh_;
00095         private:
00096                 // declaration of topics to publish
00097                 ros::Publisher topicPub_JointState_;
00098                 ros::Publisher topicPub_TactileSensor_;
00099                  //ros::Publisher topicPub_Force_;
00100                  ros::Publisher topicPub_ContactArea_;   
00101                  
00102                 // service servers
00103                 ros::ServiceServer srvServer_Init_;
00104         ros::ServiceServer srvServer_Stop_;
00105         ros::ServiceServer srvServer_SetOperationMode_;
00106         ros::ServiceServer srvSetAxisTargetAcceleration_;
00107         ros::ServiceServer srvDemoInfo_;
00108         ros::ServiceServer srvGetFingerYXZ_;
00109         ros::ServiceServer srvUpdater_;
00110         ros::ServiceServer srvCloseHand_;
00111         ros::ServiceServer srvServer_Force_data_;   
00112         
00113 
00114         // actionlib server
00115                 actionlib::SimpleActionServer<cob_actions::JointCommandAction> as_;
00116                 std::string action_name_;
00117                 cob_actions::JointCommandFeedback feedback_;
00118                 cob_actions::JointCommandResult result_;
00119 
00120                 // service clients
00121                 //--
00122 
00123                 // other variables
00124                 SDH::cSDH *sdh_;
00125                 SDH::cDSA *dsa_;  
00126                 std::vector<SDH::cSDH::eAxisState> state_;
00127         SDH::cDBG cdbg;
00128         
00129                 
00130                 
00131 
00132                 std::string sdhdevicetype_;
00133                 std::string sdhdevicestring_;
00134                 int sdhdevicenum_;
00135                 std::string dsadevicestring_;
00136                 int dsadevicenum_;
00137                 int baudrate_, id_read_, id_write_;
00138                 double timeout_;
00139 
00140                 bool isInitialized_;
00141                 bool isDSAInitialized_;
00142                 int DOF_HW_,DOF_ROS_;
00143                 double pi_;
00144                 
00145                 cob_msgs::JointCommand command_;
00146                 
00147                 std::vector<std::string> JointNames_;
00148                 std::vector<std::string> JointNamesAll_;
00149                 std::vector<int> axes_;
00150                 std::vector<double> targetAngles_; // in degrees
00151                 std::vector<double> targetVelocity_; // in degrees
00152                 bool hasNewGoal_;
00153                 
00154         public:
00155                 // Constructor
00156                 SdhDriverNode(std::string name):
00157                         as_(nh_, name, boost::bind(&SdhDriverNode::executeCB, this, _1)),
00158                         action_name_(name)
00159                 {
00160                         pi_ = 3.1415926;
00161                 }
00162 
00163                 // Destructor
00164                 ~SdhDriverNode()
00165                 {
00166                         if(isDSAInitialized_)
00167                                 dsa_->Close();
00168                         if(isInitialized_)
00169                                 sdh_->Close();
00170                         delete sdh_;
00171                 }
00172                 
00173                 bool init()
00174                 {
00175                         // initialize global variables
00176                         isInitialized_ = false;
00177                         isDSAInitialized_ = false;
00178                         hasNewGoal_ = false;
00179 
00180                         // implementation of topics to publish
00181                         topicPub_JointState_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
00182                         topicPub_TactileSensor_ = nh_.advertise<cob_msgs::TactileSensor>("tactile_data", 1);
00183             //topicPub_Force_ = nh_.advertise<cob_msgs::TactileForce>("force_data", 1);
00184             topicPub_ContactArea_ = nh_.advertise<cob_msgs::ContactArea>("area_data", 1); 
00185 
00186                         // pointer to sdh
00187                         sdh_ = new SDH::cSDH(false, false, 0); //(_use_radians=false, bool _use_fahrenheit=false, int _debug_level=0)
00188                          
00189                         // implementation of service servers
00190                         srvServer_Force_data_ = nh_.advertiseService("Force", &SdhDriverNode::srvCallback_Force, this);
00191                         srvServer_Init_ = nh_.advertiseService("Init", &SdhDriverNode::srvCallback_Init, this);
00192             srvServer_Stop_ = nh_.advertiseService("Stop", &SdhDriverNode::srvCallback_Stop, this);
00193             srvServer_SetOperationMode_ = nh_.advertiseService("SetOperationMode", &SdhDriverNode::srvCallback_SetOperationMode, this);
00194             srvSetAxisTargetAcceleration_ = nh_.advertiseService("SetAxisTargetAcceleration", &SdhDriverNode::srvCallback_SetAxisTargetAcceleration, this);
00195             srvDemoInfo_ = nh_.advertiseService("DemoInfo", &SdhDriverNode::srvCallback_DemoInfo, this);
00196             srvGetFingerYXZ_ = nh_.advertiseService("GetFingerXYZ", &SdhDriverNode::srvCallback_GetFingerXYZ, this);
00197             srvUpdater_ = nh_.advertiseService("DSAUpdater", &SdhDriverNode::srvCallback_Updater, this);
00198             srvCloseHand_ = nh_.advertiseService("CloseHand", &SdhDriverNode::srvCallback_CloseHand, this);
00199               
00200             // getting harware parameters from parameter server
00201 #ifdef USE_ESD
00202                         nh_.param("sdhdevicetype", sdhdevicetype_, std::string("ESD"));
00203                         nh_.param("sdhdevicestring", sdhdevicestring_, std::string("/dev/can0"));
00204 #else
00205                         nh_.param("sdhdevicetype", sdhdevicetype_, std::string("PEAK"));
00206                         nh_.param("sdhdevicestring", sdhdevicestring_, std::string("/dev/pcan0"));
00207 #endif
00208                         nh_.param("sdhdevicenum", sdhdevicenum_, 0);
00209                         nh_.param("dsadevicestring", dsadevicestring_, std::string("/dev/ttyS%d"));
00210                         nh_.param("dsadevicenum", dsadevicenum_, 0);
00211                         
00212                         nh_.param("baudrate", baudrate_, 1000000);
00213                         nh_.param("timeout", timeout_, (double)0.04);
00214                         nh_.param("id_read", id_read_, 43);
00215                         nh_.param("id_write", id_write_, 42);
00216 
00217             // get JointNames from parameter server
00218                         ROS_INFO("getting JointNames from parameter server");
00219                         XmlRpc::XmlRpcValue JointNames_param;
00220                         if (nh_.hasParam("JointNames"))
00221                         {
00222                                 nh_.getParam("JointNames", JointNames_param);
00223                         }
00224                         else
00225                         {
00226                                 ROS_ERROR("Parameter JointNames not set");
00227                                 return false;
00228                         }
00229                         DOF_HW_ = JointNames_param.size(); // DOFs of sdh, NOTE: hardware has 8 DOFs, but two cuppled ones; joints palm_finger11 and palm_finger21 are actuated synchronously
00230                         JointNames_.resize(DOF_HW_);
00231                         for (int i = 0; i<DOF_HW_; i++ )
00232                         {
00233                                 JointNames_[i] = (std::string)JointNames_param[i];
00234                         }
00235                         std::cout << "JointNames = " << JointNames_param << std::endl;
00236 
00237             // get JointNamesAll from parameter server                  
00238                         ROS_INFO("getting JointNamesAll from parameter server");
00239                         XmlRpc::XmlRpcValue JointNamesAll_param;
00240                         if (nh_.hasParam("JointNamesAll"))
00241                         {
00242                                 nh_.getParam("JointNamesAll", JointNamesAll_param);
00243                         }
00244                         else
00245                         {
00246                                 ROS_ERROR("Parameter JointNamesAll not set");
00247                                 return false;
00248                         }
00249                         DOF_ROS_ = JointNamesAll_param.size(); // DOFs of sdh, NOTE: hardware has 8 DOFs, but two cuppled ones; joints palm_finger11 and palm_finger21 are actuated synchronously
00250                         JointNamesAll_.resize(DOF_ROS_);
00251                         for (int i = 0; i<DOF_ROS_; i++ )
00252                         {
00253                                 JointNamesAll_[i] = (std::string)JointNamesAll_param[i];
00254                         }
00255                         std::cout << "JointNamesAll = " << JointNamesAll_param << std::endl;
00256                         
00257                         // define axes to send to sdh
00258                         axes_.resize(DOF_HW_);
00259                         for(int i=0; i<DOF_HW_; i++)
00260                         {
00261                                 axes_[i] = i;
00262                         }
00263                         ROS_INFO("DOF_HW = %d, DOF_ROS = %d",DOF_HW_,DOF_ROS_);
00264                         
00265                         state_.resize(axes_.size());
00266                         
00267                         return true;
00268                 }
00269 
00270                 void executeCB(const cob_actions::JointCommandGoalConstPtr &goal)
00271                 {                       
00272                         if (!isInitialized_)
00273                         {
00274                                 ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
00275                                 as_.setAborted();
00276                                 return;
00277                         }
00278 
00279                         while (hasNewGoal_ == true ) usleep(10000);
00280                         
00281                         bool hasvelocity_ = goal->hasvelocity;
00282                         if (hasvelocity_ == true)
00283             {           
00284                         targetVelocity_.resize(DOF_HW_);
00285                         targetVelocity_[0] = goal->command.velocities[3]; // joint_palm_finger11
00286                         targetVelocity_[1] = goal->command.velocities[7]; // joint_finger21_finger22
00287                         targetVelocity_[2] = goal->command.velocities[8]; // joint_finger22_finger23
00288                         targetVelocity_[3] = goal->command.velocities[1]; // joint_thumb1_thumb2
00289                         targetVelocity_[4] = goal->command.velocities[2]; // joint_thumb2_thumb3
00290                         targetVelocity_[5] = goal->command.velocities[4]; // joint_finger11_finger12
00291                         targetVelocity_[6] = goal->command.velocities[5]; // joint_finger12_finger13
00292                                                  
00293                         std::cout << "received new veclocities goal: " << targetVelocity_[0] << " , " << targetVelocity_[1] << " , " << targetVelocity_[2] << " , " << targetVelocity_[3] << " , " << targetVelocity_[4] << " , " << targetVelocity_[5] << " , " << targetVelocity_[6] << std::endl;
00294                         }
00295             else 
00296                         {
00297                         targetAngles_.resize(DOF_HW_);
00298                         targetAngles_[0] = goal->command.positions[3]*180.0/3.14; // joint_palm_finger11
00299                         targetAngles_[1] = goal->command.positions[7]*180.0/3.14; // joint_finger21_finger22
00300                         targetAngles_[2] = goal->command.positions[8]*180.0/3.14; // joint_finger22_finger23
00301                         targetAngles_[3] = goal->command.positions[1]*180.0/3.14; // joint_thumb1_thumb2
00302                         targetAngles_[4] = goal->command.positions[2]*180.0/3.14; // joint_thumb2_thumb3
00303                         targetAngles_[5] = goal->command.positions[4]*180.0/3.14; // joint_finger11_finger12
00304                         targetAngles_[6] = goal->command.positions[5]*180.0/3.14; // joint_finger12_finger13
00305                                                  
00306                                         
00307                         std::cout << "received new position goal: " << targetAngles_[0] << " , " << targetAngles_[1] << " , " << targetAngles_[2] << " , " << targetAngles_[3] << " , " << targetAngles_[4] << " , " << targetAngles_[5] << " , " << targetAngles_[6] << std::endl;
00308             }   
00309                         
00310                 
00311                         hasNewGoal_ = true;
00312                         
00313                         usleep(500000); // needed sleep until sdh starts to change status from idle to moving
00314                         
00315                         bool finished = false;
00316                         while(finished == false)
00317                         {
00318                                 if (as_.isNewGoalAvailable())
00319                                 {
00320                                         ROS_WARN("%s: Aborted", action_name_.c_str());
00321                                         as_.setAborted();
00322                                         return;
00323                                 }
00324                                 for ( size_t i = 0; i < state_.size(); i++ )
00325                                 {
00326                                         ROS_DEBUG("state[%d] = %d",i,state_[i]);
00327                                         if (state_[i] == 0)
00328                                         {
00329                                                 finished = true;
00330                                         }
00331                                         else
00332                                         {       
00333                                                 finished = false;
00334                                         }
00335                                 }
00336                                 usleep(10000);
00337                                 //feedback_ = 
00338                                 //as_.send feedback_
00339                         }
00340 
00341                         // set the action state to succeeded                    
00342                         ROS_INFO("%s: Succeeded", action_name_.c_str());
00343                         result_.result.data = "succesfully received new goal";
00344                         result_.success = 1;
00345                         as_.setSucceeded(result_);
00346                 }
00347 
00348                 // service callback functions
00349                 // function will be called when a service is querried
00350                 bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00351                                 cob_srvs::Trigger::Response &res )
00352                 {
00353 
00354                         if (isInitialized_ == false)
00355                         {
00356                                 //Init Hand connection  
00357                                 
00358                                 try
00359                                 {
00360                                         if(sdhdevicetype_.compare("RS232")==0)
00361                                         {
00362                                                 sdh_->OpenRS232( sdhdevicenum_, 115200, 1, sdhdevicestring_.c_str());
00363                                                 ROS_INFO("Initialized RS232 for SDH");
00364                                                 isInitialized_ = true;
00365                                         }
00366                                         if(sdhdevicetype_.compare("PEAK")==0)
00367                                         {
00368                                                 ROS_INFO("Starting initializing PEAKCAN");
00369                                                 sdh_->OpenCAN_PEAK(baudrate_, timeout_, id_read_, id_write_, sdhdevicestring_.c_str());
00370                                                 ROS_INFO("Initialized PEAK CAN for SDH");
00371                                                 isInitialized_ = true;
00372                                         }
00373                                         if(sdhdevicetype_.compare("ESD")==0)
00374                                         {
00375                                                 ROS_INFO("Starting init ESD");
00376                                                 sdh_->OpenCAN_ESD(0, baudrate_, timeout_, id_read_, id_write_ );
00377                                                 ROS_INFO("Initialized ESDCAN for SDH");
00378                                                 isInitialized_ = true;
00379                                         }
00380                                 }
00381                                 catch (SDH::cSDHLibraryException* e)
00382                                 {
00383                                         ROS_ERROR("An exception was caught: %s", e->what());
00384                                         delete e;
00385                                 }
00386                                 
00387                                 //Init tactile data
00388                     try
00389                                 {
00390                                 dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
00391                                         //dsa_->SetFramerate( 0, true, false );
00392                                         dsa_->SetFramerate( 1, true );
00393                             ROS_INFO("Initialized RS232 for DSA Tactile Sensors");
00394                                         // ROS_INFO("Set sensitivity to 1.0");
00395                                         // for(int i=0; i<6; i++)
00396                                         //      dsa_->SetMatrixSensitivity(i, 1.0);
00397                             isDSAInitialized_ = true;
00398                                 }
00399                                 catch (SDH::cSDHLibraryException* e)
00400                                 {
00401                             isDSAInitialized_ = false;
00402                                         ROS_ERROR("An exception was caught: %s", e->what());
00403                                         delete e;
00404                                 }
00405                                 
00406                         }
00407                         else
00408                         {
00409                                 ROS_ERROR("...sdh already initialized...");                     
00410                                 res.success = 1;
00411                                 res.errorMessage.data = "sdh already initialized";
00412                         }
00413                         
00414                         return true;
00415                 }
00416 
00417                 bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
00418                               cob_srvs::Trigger::Response &res )
00419         {
00420             ROS_INFO("Stopping sdh");
00421                                 
00422             // stopping all arm movements
00423                         try
00424                         {
00425                                 sdh_->Stop();
00426                         }
00427                         catch (SDH::cSDHLibraryException* e)
00428                         {
00429                                 ROS_ERROR("An exception was caught: %s", e->what());
00430                                 delete e;
00431                         }
00432 
00433                 ROS_INFO("Stopping sdh succesfull");
00434                 res.success = 0; // 0 = true, else = false
00435             return true;
00436         }
00437 
00438         bool srvCallback_SetOperationMode(cob_srvs::SetOperationMode::Request &req,
00439                                           cob_srvs::SetOperationMode::Response &res )
00440         {
00441                 ROS_INFO("Set operation mode to [%s]", req.operationMode.data.c_str());
00442             nh_.setParam("OperationMode", req.operationMode.data.c_str());
00443             res.success = 0; // 0 = true, else = false
00444             return true;
00445         }
00446         
00447         bool srvCallback_Updater( cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )        
00448            { 
00449                                                    
00450              boost::this_thread::sleep(boost::posix_time::milliseconds(200));
00451                res.success = 0;
00452              return true;
00453           }
00454 
00455  
00456         bool srvCallback_SetAxisTargetAcceleration( cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )
00457         
00458         {
00459          
00460          SDH::cSDH hand;
00461          sdh_->SetController( hand.eCT_VELOCITY_ACCELERATION );
00462          sdh_->SetAxisTargetAcceleration( hand.All, 100 );
00463          cdbg << "max=" << sdh_->GetAxisMaxVelocity( hand.all_real_axes ) << "\n";
00464          res.success = 0;
00465          return true;
00466         } 
00467           
00468         bool srvCallback_DemoInfo( cob_srvs::demoinfo::Request &req, cob_srvs::demoinfo::Response &res ) 
00469         {
00470           
00471            
00472            SDH::cSDH hand;
00473                    
00474            
00475            
00476            int i;
00477          std::vector<double> MinAngle = sdh_->GetAxisMinAngle(hand.all_real_axes);
00478          std::vector<double> MaxAngle = sdh_->GetAxisMaxAngle( hand.all_real_axes);
00479          std::vector<double> MaxVelocity = sdh_->GetAxisMaxVelocity( hand.all_real_axes);
00480          std::vector<double> TargetVelocity = sdh_->GetAxisTargetVelocity( hand.all_real_axes);
00481          std::vector<double> ActualAngle = sdh_->GetAxisActualAngle( hand.all_real_axes);
00482           
00483 /*         res.MinAngle.resize (7); 
00484          res.MaxAngle.resize (7);
00485          res.TargetVelocity.resize (7);
00486          res.TargetVelocity.resize (7);
00487          res.ActualAngle.resize (7);
00488 */         
00489            for (i =0; i < 7; i++)
00490             {
00491              (res.MinAngle)[i]=MinAngle[i];
00492               (res.MaxAngle)[i]=MaxAngle[i];
00493                 (res.MaxVelocity)[i]=MaxVelocity[i];
00494                  (res.TargetVelocity)[i]=TargetVelocity[i];
00495                   (res.ActualAngle)[i]=ActualAngle[i];
00496             }  
00497 
00498           res.Contact_info2_area = dsa_->GetContactInfo( 2 ).area;
00499           res.Contact_info3_area = dsa_->GetContactInfo( 3 ).area;
00500 
00501         res.success=0;
00502         return true;
00503        
00504        }
00505 
00506 
00507       bool srvCallback_CloseHand(cob_srvs::Trigger::Request &req,
00508                               cob_srvs::Trigger::Response &res )
00509         {
00510             ROS_INFO("Closing the hand");
00511         //SDH::cDSAUpdater dsa_updater(dsa_,8);                         
00512             // stop sensor:
00513         //dsa_updater.interrupt();
00514 
00515         dsa_->Close();
00516         cdbg << "Successfully disabled tactile sensor controller of SDH and closed connection\n";
00517 
00518         // Close the connection to the SDH and DSA
00519         sdh_->Close();
00520         cdbg << "Successfully disabled joint controllers of SDH and closed connection\n";
00521          res.success = 0; // 0 = true, else = false
00522          return true;
00523         }
00524         
00525        bool srvCallback_GetFingerXYZ( cob_srvs::GetFingerXYZ::Request &req, cob_srvs::GetFingerXYZ::Response &res )  
00526           { 
00527               std::vector<double> angle,result; 
00528               int i;            
00529              
00530                 for (i=0; i<(int)(sizeof(req.value)/sizeof(double));i++)
00531                   {
00532                      angle.push_back(req.value[i]);
00533                   } 
00534                     result = sdh_->GetFingerXYZ( req.number,angle);
00535                              
00536                 for (i=0; i<3;i++)   
00537                   {
00538                      res.data[i] = result[i];
00539                   } 
00540               
00541               res.success=0;  
00542               return true;
00543            }
00544        
00545        bool srvCallback_Force( cob_srvs::Force::Request &req, cob_srvs::Force::Response &res ) 
00546             {      
00547                SDH::cDSA::sContactInfo contact_info;
00548                int ic[] = {0,1,4,5};
00549                for (int i = 0; i< (int) (sizeof(ic)/sizeof(int));i++)
00550                    {                  
00551                       int im = ic[i];
00552                       contact_info = dsa_->GetContactInfo(im);
00553                       res.force_data[i] = contact_info.force;
00554                    }   
00555                      
00556                res.success=0;
00557                return true;         
00558             }
00559                         
00560       void updateSdh()
00561         {
00562                 ROS_DEBUG("updateJointState");
00563                 
00564                 if (isInitialized_ == true)
00565                         {
00566                                 if (hasNewGoal_ == true)
00567                                 {
00568                                         // stop sdh first when new goal arrived
00569                                         try
00570                                         {
00571                                                 sdh_->Stop();
00572                                         }
00573                                         catch (SDH::cSDHLibraryException* e)
00574                                         {
00575                                                 ROS_ERROR("An exception was caught: %s", e->what());
00576                                                 delete e;
00577                                         }
00578                         
00579                                         std::string operationMode;
00580                                         nh_.getParam("OperationMode", operationMode);
00581                                         if (operationMode == "position")
00582                                         {
00583                                                 ROS_DEBUG("moving sdh in position mode");
00584                                                         
00585                                                 try
00586                                                 {
00587                                                         sdh_->SetAxisTargetAngle( axes_, targetAngles_ );
00588                                                         sdh_->MoveHand(false);
00589                                                 }
00590                                                 catch (SDH::cSDHLibraryException* e)
00591                                                 {
00592                                                         ROS_ERROR("An exception was caught: %s", e->what());
00593                                                         delete e;
00594                                                 }
00595                                         }
00596                                         else if (operationMode == "velocity")
00597                                         {
00598                                                 ROS_DEBUG("moving sdh in velocity mode");
00599                                              try
00600                          {
00601                             sdh_->SetAxisTargetVelocity( axes_, targetVelocity_ );
00602                                                 sdh_->MoveHand(false);
00603                          }  
00604                                                 //sdh_->MoveVel(goal->trajectory.points[0].velocities);
00605                                                 //ROS_WARN("Moving in velocity mode currently disabled");
00606                          catch (SDH::cSDHLibraryException* e)
00607                                                 {
00608                                                         ROS_ERROR("An exception was caught: %s", e->what());
00609                                                         delete e;
00610                                                 } 
00611                                         }
00612                                         else if (operationMode == "effort")
00613                                         {
00614                                                 ROS_DEBUG("moving sdh in effort mode");
00615                                             //sdh_->MoveVel(goal->trajectory.points[0].velocities);
00616                                             ROS_WARN("Moving in effort mode currently disabled");
00617                                         }
00618                                         else
00619                                         {
00620                                             ROS_ERROR("sdh neither in position nor in velocity nor in effort mode. OperationMode = [%s]", operationMode.c_str());
00621                                         }
00622                                         
00623                                         hasNewGoal_ = false;
00624                                 }
00625                 
00626                         // read and publish joint angles
00627                                 std::vector<double> actualAngles;
00628                                 try
00629                                 {
00630                                         actualAngles = sdh_->GetAxisActualAngle( axes_ );
00631                                 }
00632                                 catch (SDH::cSDHLibraryException* e)
00633                                 {
00634                                         ROS_ERROR("An exception was caught: %s", e->what());
00635                                         delete e;
00636                                 }
00637                                 
00638                                 ROS_DEBUG("received %d angles from sdh",actualAngles.size());
00639                         
00640                         //Creat an instance of the hand
00641                         SDH::cSDH hand; 
00642                         // create joint_state message
00643                         sensor_msgs::JointState msg;
00644                                 msg.header.stamp = ros::Time::now();
00645                                 msg.name.resize(DOF_ROS_);
00646                                 msg.position.resize(DOF_ROS_);
00647                                 msg.velocity.resize(DOF_ROS_);
00648 
00649                                 // set joint names and map them to angles
00650                                 msg.name = JointNamesAll_;
00651                                 //std::cout << actualAngles[0] << " , " << actualAngles[1] << " , " << actualAngles[2] << " , " << actualAngles[3] << " , " << actualAngles[4] << " , " << actualAngles[5] << " , " << actualAngles[6] << std::endl;
00652                                 msg.position[0] = 0.0; // joint_palm_thumb1
00653                 msg.position[1] = actualAngles[3]*pi_/180.0; // joint_thumb1_thumb2
00654                                 msg.position[2] = actualAngles[4]*pi_/180.0; // joint_thumb2_thumb3
00655                                 msg.position[3] = actualAngles[0]*pi_/180.0; // joint_palm_finger11
00656                                 msg.position[4] = actualAngles[5]*pi_/180.0; // joint_finger11_finger12
00657                                 msg.position[5] = actualAngles[6]*pi_/180.0; // joint_finger12_finger13
00658                                 msg.position[6] = actualAngles[0]*pi_/180.0; // joint_palm_finger21
00659                                 msg.position[7] = actualAngles[1]*pi_/180.0; // joint_finger21_finger22
00660                                 msg.position[8] = actualAngles[2]*pi_/180.0; // joint_finger22_finger23
00661                         msg.velocity = sdh_->GetAxisTargetVelocity( hand.all_axes );
00662                         // publish message
00663                         topicPub_JointState_.publish(msg); 
00664                         
00665                                 // read sdh status
00666                         state_ = sdh_->GetAxisActualState(axes_);
00667                         }
00668                         else
00669                         {
00670                                 ROS_DEBUG("sdh not initialized");
00671                         }
00672                 }
00673                 
00674 
00675                 void readTactileData()
00676                 {
00677                         ROS_DEBUG("readTactileData");
00678                         if(isDSAInitialized_)
00679                         {
00680                                 try
00681                                 {
00682                                         //dsa_->SetFramerate( 0, true, true );
00683                                         dsa_->UpdateFrame();
00684                                 }
00685                                 catch (SDH::cSDHLibraryException* e)
00686                                 {
00687                                         ROS_ERROR("An exception was caught: %s", e->what());
00688                                         delete e;
00689                                 }
00690                         }
00691                 }
00692 
00693 
00694                 void updateTactileData()
00695                 {
00696                         ROS_DEBUG("updateTactileData"); //Create new Topic to publish Force
00697                         cob_msgs::TactileSensor msg;
00698                         //cob_msgs::TactileForce forcemsg;
00699             cob_msgs::ContactArea areamsg;
00700                         if(isDSAInitialized_)
00701                         {
00702                                 msg.header.stamp = ros::Time::now();
00703                                 //forcemsg.header.stamp = ros::Time::now();
00704                                 areamsg.header.stamp = ros::Time::now();
00705                                 //std::cerr << *dsa_;
00706                                 int m, x, y;
00707                                 
00708                                 msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
00709                                 //forcemsg.data.resize(dsa_->GetSensorInfo().nb_matrices);
00710                                 
00711                                 
00712                                 
00713                                 for ( m = 0; m < dsa_->GetSensorInfo().nb_matrices; m++ )
00714                                 {
00715                                         //double force = dsa_->GetContactInfo( m ).force;
00716                                         //forcemsg.data[m] = force;
00717                                          
00718                                         cob_msgs::TactileMatrix &tm = msg.tactile_matrix[m];
00719                                         tm.matrix_id = m;
00720                                         tm.cells_x = dsa_->GetMatrixInfo( m ).cells_x;
00721                                         tm.cells_y = dsa_->GetMatrixInfo( m ).cells_y;
00722                                         tm.tactile_array.resize(tm.cells_x * tm.cells_y);
00723                                         for ( y = 0; y < tm.cells_y; y++ )
00724                                         {
00725                                                 for ( x = 0; x < tm.cells_x; x++ )
00726                                                         tm.tactile_array[tm.cells_x*y + x] = dsa_->GetTexel( m, x, y );
00727                                         }
00728                                 }
00729                                 //publish matrix
00730                                 areamsg.data2 = dsa_->GetContactArea(2);
00731                                 areamsg.data3 = dsa_->GetContactArea(3); 
00732                                 
00733                                 topicPub_TactileSensor_.publish(msg);
00734                 //topicPub_Force_.publish(forcemsg);
00735                 topicPub_ContactArea_.publish(areamsg);
00736                                    
00737  
00738                         }
00739                 }
00740 
00741                  
00742 }; //SdhDriverNode
00743 
00744 //#######################
00745 //#### main programm ####
00746 int main(int argc, char** argv)
00747 {
00748         
00749         // initialize ROS, spezify name of node
00750         ros::init(argc, argv, "cob_sdh_driver");
00751 
00752         SdhDriverNode sdh_node("JointCommand");
00753         if (!sdh_node.init()) return 0;
00754         
00755         ROS_INFO("...sdh node running...");
00756 
00757         sleep(1);
00758         ros::Rate loop_rate(5); // Hz
00759         while(sdh_node.nh_.ok())
00760         {
00761                 for(int i=0; i<7; i++)
00762                 {
00763                         sdh_node.readTactileData();
00764                         /*
00765                         if(++n % 30 == 0)
00766                                 std::cout << n << std::endl;
00767                                 */
00768                 }
00769                 // publish JointState
00770                 sdh_node.updateSdh();
00771                 // publish TactileData
00772                 sdh_node.updateTactileData();
00773                 
00774                 // sleep and waiting for messages, callbacks    
00775                 ros::spinOnce();
00776                 usleep(200000);
00777                 //loop_rate.sleep();
00778         }
00779 
00780         return 0;
00781 }