schunk_sdh: schunk_sdh: sdh.cpp Source File

00001 
00060 //##################
00061 //#### includes ####
00062 
00063 // standard includes
00064 #include <unistd.h>
00065 
00066 // ROS includes
00067 #include <ros/ros.h>
00068 #include <urdf/model.h>
00069 #include <actionlib/server/simple_action_server.h>
00070 
00071 // ROS message includes
00072 #include <std_msgs/Float32MultiArray.h>
00073 #include <trajectory_msgs/JointTrajectory.h>
00074 #include <sensor_msgs/JointState.h>
00075 //#include <pr2_controllers_msgs/JointTrajectoryAction.h>
00076 #include <control_msgs/FollowJointTrajectoryAction.h>
00077 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00078 #include <schunk_sdh/TactileSensor.h>
00079 #include <schunk_sdh/TactileMatrix.h>
00080 #include <brics_actuator/JointVelocities.h>
00081 #include <brics_actuator/JointValue.h>
00082 
00083 // ROS service includes
00084 #include <cob_srvs/Trigger.h>
00085 #include <cob_srvs/SetOperationMode.h>
00086 
00087 // ROS diagnostic msgs
00088 #include <diagnostic_msgs/DiagnosticArray.h>
00089 
00090 // external includes
00091 #include <schunk_sdh/sdh.h>
00092 #include <schunk_sdh/dsa.h>
00093 
00099 class SdhNode
00100 {
00101         public:
00103                 ros::NodeHandle nh_;
00104         private:
00105                 // declaration of topics to publish
00106                 ros::Publisher topicPub_JointState_;
00107                 ros::Publisher topicPub_ControllerState_;
00108                 ros::Publisher topicPub_TactileSensor_;
00109                 ros::Publisher topicPub_Diagnostics_;
00110                 
00111                 // topic subscribers
00112                 ros::Subscriber subSetVelocitiesRaw_;
00113                 ros::Subscriber subSetVelocities_;
00114 
00115                 // service servers
00116                 ros::ServiceServer srvServer_Init_;
00117                 ros::ServiceServer srvServer_Stop_;
00118                 ros::ServiceServer srvServer_Recover_;
00119                 ros::ServiceServer srvServer_SetOperationMode_;
00120 
00121                 // actionlib server
00122                 //actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> as_;
00123                 actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00124                 std::string action_name_;
00125 
00126                 // service clients
00127                 //--
00128 
00129                 // other variables
00130                 SDH::cSDH *sdh_;
00131                 SDH::cDSA *dsa_;
00132                 std::vector<SDH::cSDH::eAxisState> state_;
00133 
00134                 std::string sdhdevicetype_;
00135                 std::string sdhdevicestring_;
00136                 int sdhdevicenum_;
00137                 std::string dsadevicestring_;
00138                 int dsadevicenum_;
00139                 int baudrate_, id_read_, id_write_;
00140                 double timeout_;
00141 
00142                 bool isInitialized_;
00143                 bool isDSAInitialized_;
00144                 bool isError_;
00145                 int DOF_;
00146                 double pi_;
00147                 
00148                 trajectory_msgs::JointTrajectory traj_;
00149                 
00150                 std::vector<std::string> joint_names_;
00151                 std::vector<int> axes_;
00152                 std::vector<double> targetAngles_; // in degrees
00153                 std::vector<double> velocities_; // in rad/s
00154                 bool hasNewGoal_;
00155                 std::string operationMode_; 
00156                 
00157         public:
00163                 SdhNode(std::string name):
00164                         as_(nh_, name, boost::bind(&SdhNode::executeCB, this, _1),true),
00165                         action_name_(name)
00166                 {
00167                         pi_ = 3.1415926;
00168                         
00169                         nh_ = ros::NodeHandle ("~");
00170                         isError_ = false;
00171                         // diagnostics
00172                         topicPub_Diagnostics_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00173 
00174                 }
00175 
00179                 ~SdhNode() 
00180                 {
00181                         if(isDSAInitialized_)
00182                                 dsa_->Close();
00183                         if(isInitialized_)
00184                                 sdh_->Close();
00185                         delete sdh_;
00186                 }
00187 
00188 
00192                 bool init()
00193                 {
00194                         // initialize member variables
00195                         isInitialized_ = false;
00196                         isDSAInitialized_ = false;
00197                         hasNewGoal_ = false;
00198 
00199                         // implementation of topics to publish
00200                         topicPub_JointState_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
00201                         topicPub_ControllerState_ = nh_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
00202                         topicPub_TactileSensor_ = nh_.advertise<schunk_sdh::TactileSensor>("tactile_data", 1);
00203 
00204                         // pointer to sdh
00205                         sdh_ = new SDH::cSDH(false, false, 0); //(_use_radians=false, bool _use_fahrenheit=false, int _debug_level=0)
00206 
00207                         // implementation of service servers
00208                         srvServer_Init_ = nh_.advertiseService("init", &SdhNode::srvCallback_Init, this);
00209                         srvServer_Stop_ = nh_.advertiseService("stop", &SdhNode::srvCallback_Stop, this);
00210                         srvServer_Recover_ = nh_.advertiseService("recover", &SdhNode::srvCallback_Init, this); //HACK: There is no recover implemented yet, so we execute a init
00211                         srvServer_SetOperationMode_ = nh_.advertiseService("set_operation_mode", &SdhNode::srvCallback_SetOperationMode, this);
00212                         
00213                         subSetVelocitiesRaw_ = nh_.subscribe("set_velocities_raw", 1, &SdhNode::topicCallback_setVelocitiesRaw, this);
00214                         subSetVelocities_ = nh_.subscribe("set_velocities", 1, &SdhNode::topicCallback_setVelocities, this);
00215 
00216                         // getting hardware parameters from parameter server
00217                         nh_.param("sdhdevicetype", sdhdevicetype_, std::string("PCAN"));
00218                         nh_.param("sdhdevicestring", sdhdevicestring_, std::string("/dev/pcan0"));
00219                         nh_.param("sdhdevicenum", sdhdevicenum_, 0);
00220                         
00221                         nh_.param("dsadevicestring", dsadevicestring_, std::string(""));
00222                         nh_.param("dsadevicenum", dsadevicenum_, 0);
00223                         
00224                         nh_.param("baudrate", baudrate_, 1000000);
00225                         nh_.param("timeout", timeout_, (double)0.04);
00226                         nh_.param("id_read", id_read_, 43);
00227                         nh_.param("id_write", id_write_, 42);
00228 
00229                         // get joint_names from parameter server
00230                         ROS_INFO("getting joint_names from parameter server");
00231                         XmlRpc::XmlRpcValue joint_names_param;
00232                         if (nh_.hasParam("joint_names"))
00233                         {
00234                                 nh_.getParam("joint_names", joint_names_param);
00235                         }
00236                         else
00237                         {
00238                                 ROS_ERROR("Parameter joint_names not set, shutting down node...");
00239                                 nh_.shutdown();
00240                                 return false;
00241                         }
00242                         DOF_ = joint_names_param.size();
00243                         joint_names_.resize(DOF_);
00244                         for (int i = 0; i<DOF_; i++ )
00245                         {
00246                                 joint_names_[i] = (std::string)joint_names_param[i];
00247                         }
00248                         std::cout << "joint_names = " << joint_names_param << std::endl;
00249                         
00250                         // define axes to send to sdh
00251                         axes_.resize(DOF_);
00252                         velocities_.resize(DOF_);
00253                         for(int i=0; i<DOF_; i++)
00254                         {
00255                                 axes_[i] = i;
00256                         }
00257                         ROS_INFO("DOF = %d",DOF_);
00258                         
00259                         state_.resize(axes_.size());
00260 
00261                         nh_.param("OperationMode", operationMode_, std::string("position"));
00262                         return true;
00263                 }
00264 
00271                 //void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
00272                 void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
00273                 {                       
00274                         ROS_INFO("sdh: executeCB");
00275                         if (operationMode_ != "position")
00276                         {
00277                                 ROS_ERROR("%s: Rejected, sdh not in position mode", action_name_.c_str());
00278                                 as_.setAborted();
00279                                 return;
00280                         }
00281                         if (!isInitialized_)
00282                         {
00283                                 ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
00284                                 as_.setAborted();
00285                                 return;
00286                         }
00287 
00288                         if (goal->trajectory.points.empty() || goal->trajectory.points[0].positions.size() != size_t(DOF_))
00289                         {
00290                                 ROS_ERROR("%s: Rejected, malformed FollowJointTrajectoryGoal", action_name_.c_str());
00291                                 as_.setAborted();
00292                                 return;
00293                         }
00294                         while (hasNewGoal_ == true ) usleep(10000);
00295 
00296                         // \todo TODO: use joint_names for assigning values
00297                         targetAngles_.resize(DOF_);
00298                         targetAngles_[0] = goal->trajectory.points[0].positions[0]*180.0/pi_; // sdh_knuckle_joint
00299                         targetAngles_[1] = goal->trajectory.points[0].positions[5]*180.0/pi_; // sdh_finger22_joint
00300                         targetAngles_[2] = goal->trajectory.points[0].positions[6]*180.0/pi_; // sdh_finger23_joint
00301                         targetAngles_[3] = goal->trajectory.points[0].positions[1]*180.0/pi_; // sdh_thumb2_joint
00302                         targetAngles_[4] = goal->trajectory.points[0].positions[2]*180.0/pi_; // sdh_thumb3_joint
00303                         targetAngles_[5] = goal->trajectory.points[0].positions[3]*180.0/pi_; // sdh_finger12_joint
00304                         targetAngles_[6] = goal->trajectory.points[0].positions[4]*180.0/pi_; // sdh_finger13_joint
00305                         ROS_INFO("received new position goal: [['sdh_knuckle_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']] = [%f,%f,%f,%f,%f,%f,%f]",goal->trajectory.points[0].positions[0],goal->trajectory.points[0].positions[1],goal->trajectory.points[0].positions[2],goal->trajectory.points[0].positions[3],goal->trajectory.points[0].positions[4],goal->trajectory.points[0].positions[5],goal->trajectory.points[0].positions[6]);
00306                 
00307                         hasNewGoal_ = true;
00308                         
00309                         usleep(500000); // needed sleep until sdh starts to change status from idle to moving
00310                         
00311                         bool finished = false;
00312                         while(finished == false)
00313                         {
00314                                 if (as_.isNewGoalAvailable())
00315                                 {
00316                                         ROS_WARN("%s: Aborted", action_name_.c_str());
00317                                         as_.setAborted();
00318                                         return;
00319                                 }
00320                                 for ( unsigned int i = 0; i < state_.size(); i++ )
00321                                 {
00322                                         ROS_DEBUG("state[%d] = %d",i,state_[i]);
00323                                         if (state_[i] == 0)
00324                                         {
00325                                                 finished = true;
00326                                         }
00327                                         else
00328                                         {       
00329                                                 finished = false;
00330                                         }
00331                                 }
00332                                 usleep(10000);
00333                                 //feedback_ = 
00334                                 //as_.send feedback_
00335                         }
00336 
00337                         // set the action state to succeeded                    
00338                         ROS_INFO("%s: Succeeded", action_name_.c_str());
00339                         //result_.result.data = "succesfully received new goal";
00340                         //result_.success = 1;
00341                         //as_.setSucceeded(result_);
00342                         as_.setSucceeded();
00343                 }
00344 
00345                 void topicCallback_setVelocitiesRaw(const std_msgs::Float32MultiArrayPtr& velocities)
00346                 {
00347                         if (!isInitialized_)
00348                         {
00349                                 ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
00350                                 return;
00351                         }
00352                         if(velocities->data.size() != velocities_.size()){
00353                                 ROS_ERROR("Velocity array dimension mismatch");
00354                                 return;
00355                         }
00356                         if (operationMode_ != "velocity")
00357                         {
00358                                 ROS_ERROR("%s: Rejected, sdh not in velocity mode", action_name_.c_str());
00359                                 return;
00360                         }
00361 
00362                         // TODO: write proper lock!
00363                         while (hasNewGoal_ == true ) usleep(10000);
00364 
00365                         velocities_[0] = velocities->data[0] * 180.0 / pi_; // sdh_knuckle_joint
00366                         velocities_[1] = velocities->data[5] * 180.0 / pi_; // sdh_finger22_joint
00367                         velocities_[2] = velocities->data[6] * 180.0 / pi_; // sdh_finger23_joint
00368                         velocities_[3] = velocities->data[1] * 180.0 / pi_; // sdh_thumb2_joint
00369                         velocities_[4] = velocities->data[2] * 180.0 / pi_; // sdh_thumb3_joint
00370                         velocities_[5] = velocities->data[3] * 180.0 / pi_; // sdh_finger12_joint
00371                         velocities_[6] = velocities->data[4] * 180.0 / pi_; // sdh_finger13_joint
00372 
00373                         hasNewGoal_ = true;
00374                 }
00375                 bool parseDegFromJointValue(const brics_actuator::JointValue& val, double &deg_val){
00376                     if (val.unit == "rad/s"){
00377                         deg_val = val.value  * 180.0 / pi_;
00378                         return true;
00379                     }else if (val.unit == "deg/s"){
00380                         deg_val = val.value;
00381                         return true;
00382                     }else {
00383                         ROS_ERROR_STREAM("Rejected message, unit '" << val.unit << "' not supported");
00384                         return false;
00385                     }
00386                 }
00387                 void topicCallback_setVelocities(const brics_actuator::JointVelocities::ConstPtr& msg)
00388                 {
00389                         if (!isInitialized_)
00390                         {
00391                                 ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
00392                                 return;
00393                         }
00394                         if(msg->velocities.size() != velocities_.size()){
00395                                 ROS_ERROR("Velocity array dimension mismatch");
00396                                 return;
00397                         }
00398                         if (operationMode_ != "velocity")
00399                         {
00400                                 ROS_ERROR("%s: Rejected, sdh not in velocity mode", action_name_.c_str());
00401                                 return;
00402                         }
00403 
00404                         // TODO: write proper lock!
00405                         while (hasNewGoal_ == true ) usleep(10000);
00406                         bool valid = true;
00407 
00408                         valid = valid && parseDegFromJointValue(msg->velocities[0], velocities_[0]); // sdh_knuckle_joint
00409                         valid = valid && parseDegFromJointValue(msg->velocities[5], velocities_[1]); // sdh_finger22_joint
00410                         valid = valid && parseDegFromJointValue(msg->velocities[6], velocities_[2]); // sdh_finger23_joint
00411                         valid = valid && parseDegFromJointValue(msg->velocities[1], velocities_[3]); // sdh_thumb2_joint
00412                         valid = valid && parseDegFromJointValue(msg->velocities[2], velocities_[4]); // sdh_thumb3_joint
00413                         valid = valid && parseDegFromJointValue(msg->velocities[3], velocities_[5]); // sdh_finger12_joint
00414                         valid = valid && parseDegFromJointValue(msg->velocities[4], velocities_[6]); // sdh_finger13_joint
00415 
00416                         if (valid) hasNewGoal_ = true;
00417                 }               
00425                 bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00426                                                         cob_srvs::Trigger::Response &res )
00427                 {
00428 
00429                         if (isInitialized_ == false)
00430                         {
00431                                 //Init Hand connection  
00432                                 
00433                                 try
00434                                 {
00435                                         if(sdhdevicetype_.compare("RS232")==0)
00436                                         {
00437                                                 sdh_->OpenRS232( sdhdevicenum_, 115200, 1, sdhdevicestring_.c_str());
00438                                                 ROS_INFO("Initialized RS232 for SDH");
00439                                                 isInitialized_ = true;
00440                                         }
00441                                         if(sdhdevicetype_.compare("PCAN")==0)
00442                                         {
00443                                                 ROS_INFO("Starting initializing PEAKCAN");
00444                                                 sdh_->OpenCAN_PEAK(baudrate_, timeout_, id_read_, id_write_, sdhdevicestring_.c_str());
00445                                                 ROS_INFO("Initialized PEAK CAN for SDH");
00446                                                 isInitialized_ = true;
00447                                         }
00448                                         if(sdhdevicetype_.compare("ESD")==0)
00449                                         {
00450                                                 ROS_INFO("Starting initializing ESD");
00451                                                 if(strcmp(sdhdevicestring_.c_str(), "/dev/can0") == 0)
00452                                                 {
00453                                                         ROS_INFO("Initializing ESD on device %s",sdhdevicestring_.c_str());
00454                                                         sdh_->OpenCAN_ESD(0, baudrate_, timeout_, id_read_, id_write_ );
00455                                                 }
00456                                                 else if(strcmp(sdhdevicestring_.c_str(), "/dev/can1") == 0)
00457                                                 {
00458                                                         ROS_INFO("Initializin ESD on device %s",sdhdevicestring_.c_str());
00459                                                         sdh_->OpenCAN_ESD(1, baudrate_, timeout_, id_read_, id_write_ );
00460                                                 }
00461                                                 else
00462                                                 {
00463                                                         ROS_ERROR("Currently only support for /dev/can0 and /dev/can1");
00464                                                         res.success.data = false;
00465                                                         res.error_message.data = "Currently only support for /dev/can0 and /dev/can1";
00466                                                         return true;
00467                                                 }
00468                                                 ROS_INFO("Initialized ESDCAN for SDH"); 
00469                                                 isInitialized_ = true;
00470                                         }
00471                                 }
00472                                 catch (SDH::cSDHLibraryException* e)
00473                                 {
00474                                         ROS_ERROR("An exception was caught: %s", e->what());
00475                                         res.success.data = false;
00476                                         res.error_message.data = e->what();
00477                                         delete e;
00478                                         return true;
00479                                 }
00480                                 
00481                                 //Init tactile data
00482                                 if(!dsadevicestring_.empty())  {
00483                                         try
00484                                         {
00485                                                 dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
00486                                                 //dsa_->SetFramerate( 0, true, false );
00487                                                 dsa_->SetFramerate( 1, true );
00488                                                 ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str());
00489                                                 // ROS_INFO("Set sensitivity to 1.0");
00490                                                 // for(int i=0; i<6; i++)
00491                                                 //      dsa_->SetMatrixSensitivity(i, 1.0);
00492                                                 isDSAInitialized_ = true;
00493                                         }
00494                                         catch (SDH::cSDHLibraryException* e)
00495                                         {
00496                                                 isDSAInitialized_ = false;
00497                                                 ROS_ERROR("An exception was caught: %s", e->what());
00498                                                 res.success.data = false;
00499                                                 res.error_message.data = e->what();
00500                                                 delete e;
00501                                                 return true;
00502                                         }
00503                                 }
00504                         }
00505                         else
00506                         {
00507                                 ROS_WARN("...sdh already initialized...");
00508                                 res.success.data = true;
00509                                 res.error_message.data = "sdh already initialized";
00510                         }
00511                         
00512                         res.success.data = true;
00513                         return true;
00514                 }
00515 
00523                 bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
00524                                                         cob_srvs::Trigger::Response &res )
00525                 {
00526                         ROS_INFO("Stopping sdh");
00527 
00528                         // stopping all arm movements
00529                         try
00530                         {
00531                                 sdh_->Stop();
00532                         }
00533                         catch (SDH::cSDHLibraryException* e)
00534                         {
00535                                 ROS_ERROR("An exception was caught: %s", e->what());
00536                                 delete e;
00537                         }
00538 
00539                 ROS_INFO("Stopping sdh succesfull");
00540                 res.success.data = true;
00541                 return true;
00542         }
00543 
00551         bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
00552                                                         cob_srvs::Trigger::Response &res )
00553         {
00554                 ROS_WARN("Service recover not implemented yet");
00555                 res.success.data = true;
00556                 res.error_message.data = "Service recover not implemented yet";
00557                 return true;
00558         }
00559         
00567         bool srvCallback_SetOperationMode(cob_srvs::SetOperationMode::Request &req,
00568                                                                         cob_srvs::SetOperationMode::Response &res )
00569         {
00570                 hasNewGoal_ = false;
00571                 sdh_->Stop();
00572                 ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
00573                 operationMode_ = req.operation_mode.data;
00574                 res.success.data = true;
00575                 if( operationMode_ == "position"){
00576                         sdh_->SetController(SDH::cSDH::eCT_POSE);
00577                 }else if( operationMode_ == "velocity"){
00578                         try{
00579                                 sdh_->SetController(SDH::cSDH::eCT_VELOCITY);
00580                                 sdh_->SetAxisEnable(sdh_->All, 1.0);
00581                         }
00582                         catch (SDH::cSDHLibraryException* e)
00583                         {
00584                                 ROS_ERROR("An exception was caught: %s", e->what());
00585                                 delete e;
00586                         }
00587                 }else{
00588                         ROS_ERROR_STREAM("Operation mode '" << req.operation_mode.data << "'  not supported");
00589                 }
00590                 return true;
00591         }
00592 
00598         void updateSdh()
00599         {
00600                 ROS_DEBUG("updateJointState");
00601                 if (isInitialized_ == true)
00602                 {
00603                         if (hasNewGoal_ == true)
00604                         {
00605                                 // stop sdh first when new goal arrived
00606                                 try
00607                                 {
00608                                         sdh_->Stop();
00609                                 }
00610                                 catch (SDH::cSDHLibraryException* e)
00611                                 {
00612                                         ROS_ERROR("An exception was caught: %s", e->what());
00613                                         delete e;
00614                                 }
00615                 
00616                                 if (operationMode_ == "position")
00617                                 {
00618                                         ROS_DEBUG("moving sdh in position mode");
00619 
00620                                         try
00621                                         {
00622                                                 sdh_->SetAxisTargetAngle( axes_, targetAngles_ );
00623                                                 sdh_->MoveHand(false);
00624                                         }
00625                                         catch (SDH::cSDHLibraryException* e)
00626                                         {
00627                                                 ROS_ERROR("An exception was caught: %s", e->what());
00628                                                 delete e;
00629                                         }
00630                                 }
00631                                 else if (operationMode_ == "velocity")
00632                                 {
00633                                         ROS_DEBUG("moving sdh in velocity mode");
00634                                         try
00635                                         {
00636                                                 sdh_->SetAxisTargetVelocity(axes_,velocities_);
00637                                                 // ROS_DEBUG_STREAM("velocities: " << velocities_[0] << " "<< velocities_[1] << " "<< velocities_[2] << " "<< velocities_[3] << " "<< velocities_[4] << " "<< velocities_[5] << " "<< velocities_[6]);
00638                                         }
00639                                         catch (SDH::cSDHLibraryException* e)
00640                                         {
00641                                                 ROS_ERROR("An exception was caught: %s", e->what());
00642                                                 delete e;
00643                                         }
00644                                 }
00645                                 else if (operationMode_ == "effort")
00646                                 {
00647                                         ROS_DEBUG("moving sdh in effort mode");
00648                                         //sdh_->MoveVel(goal->trajectory.points[0].velocities);
00649                                         ROS_WARN("Moving in effort mode currently disabled");
00650                                 }
00651                                 else
00652                                 {
00653                                         ROS_ERROR("sdh neither in position nor in velocity nor in effort mode. OperationMode = [%s]", operationMode_.c_str());
00654                                 }
00655                                 
00656                                 hasNewGoal_ = false;
00657                         }
00658 
00659                         // read and publish joint angles and velocities
00660                         std::vector<double> actualAngles;
00661                         try
00662                         {
00663                                 actualAngles = sdh_->GetAxisActualAngle( axes_ );
00664                         }
00665                         catch (SDH::cSDHLibraryException* e)
00666                         {
00667                                 ROS_ERROR("An exception was caught: %s", e->what());
00668                                 delete e;
00669                         }
00670                         std::vector<double> actualVelocities;
00671                         try
00672                         {
00673                                 actualVelocities = sdh_->GetAxisActualVelocity( axes_ );
00674                         }
00675                         catch (SDH::cSDHLibraryException* e)
00676                         {
00677                                 ROS_ERROR("An exception was caught: %s", e->what());
00678                                 delete e;
00679                         }
00680                         
00681                         ROS_DEBUG("received %d angles from sdh",(int)actualAngles.size());
00682                         
00683                         ros::Time time = ros::Time::now();
00684                         
00685                         // create joint_state message
00686                         sensor_msgs::JointState msg;
00687                         msg.header.stamp = time;
00688                         msg.name.resize(DOF_);
00689                         msg.position.resize(DOF_);
00690                         msg.velocity.resize(DOF_);
00691                         msg.effort.resize(DOF_);
00692                         // set joint names and map them to angles
00693                         msg.name = joint_names_;
00694                         //['sdh_knuckle_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']
00695                         // pos
00696                         msg.position[0] = actualAngles[0]*pi_/180.0; // sdh_knuckle_joint
00697                         msg.position[1] = actualAngles[3]*pi_/180.0; // sdh_thumb_2_joint
00698                         msg.position[2] = actualAngles[4]*pi_/180.0; // sdh_thumb_3_joint
00699                         msg.position[3] = actualAngles[5]*pi_/180.0; // sdh_finger_12_joint
00700                         msg.position[4] = actualAngles[6]*pi_/180.0; // sdh_finger_13_joint
00701                         msg.position[5] = actualAngles[1]*pi_/180.0; // sdh_finger_22_joint
00702                         msg.position[6] = actualAngles[2]*pi_/180.0; // sdh_finger_23_joint
00703                         // vel                  
00704                         msg.velocity[0] = actualVelocities[0]*pi_/180.0; // sdh_knuckle_joint
00705                         msg.velocity[1] = actualVelocities[3]*pi_/180.0; // sdh_thumb_2_joint
00706                         msg.velocity[2] = actualVelocities[4]*pi_/180.0; // sdh_thumb_3_joint
00707                         msg.velocity[3] = actualVelocities[5]*pi_/180.0; // sdh_finger_12_joint
00708                         msg.velocity[4] = actualVelocities[6]*pi_/180.0; // sdh_finger_13_joint
00709                         msg.velocity[5] = actualVelocities[1]*pi_/180.0; // sdh_finger_22_joint
00710                         msg.velocity[6] = actualVelocities[2]*pi_/180.0; // sdh_finger_23_joint
00711                         // publish message
00712                         topicPub_JointState_.publish(msg);
00713                         
00714                         
00715                         // because the robot_state_publisher doen't know about the mimic joint, we have to publish the coupled joint separately
00716                         sensor_msgs::JointState  mimicjointmsg;
00717                         mimicjointmsg.header.stamp = time;
00718                         mimicjointmsg.name.resize(1);
00719                         mimicjointmsg.position.resize(1);
00720                         mimicjointmsg.velocity.resize(1);
00721                         mimicjointmsg.name[0] = "sdh_finger_21_joint";
00722                         mimicjointmsg.position[0] = msg.position[0]; // sdh_knuckle_joint = sdh_finger_21_joint
00723                         mimicjointmsg.velocity[0] = msg.velocity[0]; // sdh_knuckle_joint = sdh_finger_21_joint
00724                         topicPub_JointState_.publish(mimicjointmsg);
00725                         
00726                         
00727                         // publish controller state message
00728                         pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
00729                         controllermsg.header.stamp = time;
00730                         controllermsg.joint_names.resize(DOF_);
00731                         controllermsg.desired.positions.resize(DOF_);
00732                         controllermsg.desired.velocities.resize(DOF_);
00733                         controllermsg.actual.positions.resize(DOF_);
00734                         controllermsg.actual.velocities.resize(DOF_);
00735                         controllermsg.error.positions.resize(DOF_);
00736                         controllermsg.error.velocities.resize(DOF_);
00737                         // set joint names and map them to angles
00738                         controllermsg.joint_names = joint_names_;
00739                         //['sdh_knuckle_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']
00740                         // desired pos
00741                         if (targetAngles_.size() != 0)
00742                         {
00743                                 controllermsg.desired.positions[0] = targetAngles_[0]*pi_/180.0; // sdh_knuckle_joint
00744                                 controllermsg.desired.positions[1] = targetAngles_[3]*pi_/180.0; // sdh_thumb_2_joint
00745                                 controllermsg.desired.positions[2] = targetAngles_[4]*pi_/180.0; // sdh_thumb_3_joint
00746                                 controllermsg.desired.positions[3] = targetAngles_[5]*pi_/180.0; // sdh_finger_12_joint
00747                                 controllermsg.desired.positions[4] = targetAngles_[6]*pi_/180.0; // sdh_finger_13_joint
00748                                 controllermsg.desired.positions[5] = targetAngles_[1]*pi_/180.0; // sdh_finger_22_joint
00749                                 controllermsg.desired.positions[6] = targetAngles_[2]*pi_/180.0; // sdh_finger_23_joint
00750                         }
00751                         // desired vel
00752                                 // they are all zero
00753                         // actual pos                   
00754                         controllermsg.actual.positions = msg.position;
00755                         // actual vel
00756                         controllermsg.actual.velocities = msg.velocity;
00757                         // error, calculated out of desired and actual values
00758                         for (int i = 0; i<DOF_; i++ )
00759                         {
00760                                 controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
00761                                 controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
00762                         }
00763                         // publish controller message
00764                         topicPub_ControllerState_.publish(controllermsg);
00765 
00766                         // read sdh status
00767                         state_ = sdh_->GetAxisActualState(axes_);
00768                 }
00769                 else
00770                 {
00771                         ROS_DEBUG("sdh not initialized");
00772                 }
00773                 // publishing diagnotic messages
00774             diagnostic_msgs::DiagnosticArray diagnostics;
00775             diagnostics.status.resize(1);
00776             // set data to diagnostics
00777             if(isError_)
00778             {
00779               diagnostics.status[0].level = 2;
00780               diagnostics.status[0].name = "schunk_powercube_chain";
00781               diagnostics.status[0].message = "one or more drives are in Error mode";
00782             }
00783             else
00784             {
00785               if (isInitialized_)
00786               {
00787                 diagnostics.status[0].level = 0;
00788                 diagnostics.status[0].name = nh_.getNamespace(); //"schunk_powercube_chain";
00789                 if(isDSAInitialized_)
00790                         diagnostics.status[0].message = "sdh with tactile sensing initialized and running";
00791                 else
00792                         diagnostics.status[0].message = "sdh initialized and running, tactile sensors not connected";   
00793               }
00794               else
00795               {
00796                 diagnostics.status[0].level = 1;
00797                 diagnostics.status[0].name = nh_.getNamespace(); //"schunk_powercube_chain";
00798                 diagnostics.status[0].message = "sdh not initialized";
00799               }
00800             }
00801             // publish diagnostic message
00802             topicPub_Diagnostics_.publish(diagnostics);
00803 
00804         }
00805 
00811         void updateDsa()
00812         {
00813                 static const int dsa_reorder[6] = { 2 ,3, 4, 5, 0 , 1 }; // t1,t2,f11,f12,f21,f22
00814                 ROS_DEBUG("updateTactileData");
00815 
00816                 if(isDSAInitialized_)
00817                 {
00818                         // read tactile data
00819                         for(int i=0; i<7; i++)
00820                         {
00821                                 try
00822                                 {
00823                                         //dsa_->SetFramerate( 0, true, true );
00824                                         dsa_->UpdateFrame();
00825                                 }
00826                                 catch (SDH::cSDHLibraryException* e)
00827                                 {
00828                                         ROS_ERROR("An exception was caught: %s", e->what());
00829                                         delete e;
00830                                 }
00831                         }
00832 
00833                         schunk_sdh::TactileSensor msg;
00834                         msg.header.stamp = ros::Time::now();
00835                         int m, x, y;
00836                         msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
00837                         for ( int i = 0; i < dsa_->GetSensorInfo().nb_matrices; i++ )
00838                         {
00839                                 m = dsa_reorder[i];                                  
00840                                 schunk_sdh::TactileMatrix &tm = msg.tactile_matrix[i];
00841                                 tm.matrix_id = i;
00842                                 tm.cells_x = dsa_->GetMatrixInfo( m ).cells_x;
00843                                 tm.cells_y = dsa_->GetMatrixInfo( m ).cells_y;
00844                                 tm.tactile_array.resize(tm.cells_x * tm.cells_y);
00845                                 for ( y = 0; y < tm.cells_y; y++ )
00846                                 {
00847                                         for ( x = 0; x < tm.cells_x; x++ )
00848                                                 tm.tactile_array[tm.cells_x*y + x] = dsa_->GetTexel( m, x, y );
00849                                 }
00850                         }
00851                         //publish matrix
00852                         topicPub_TactileSensor_.publish(msg);
00853                 }
00854         }        
00855 }; //SdhNode
00856 
00862 int main(int argc, char** argv)
00863 {
00864         // initialize ROS, spezify name of node
00865         ros::init(argc, argv, "schunk_sdh");
00866 
00867         //SdhNode sdh_node(ros::this_node::getName() + "/joint_trajectory_action");
00868         SdhNode sdh_node(ros::this_node::getName() + "/follow_joint_trajectory");
00869         if (!sdh_node.init()) return 0;
00870         
00871         ROS_INFO("...sdh node running...");
00872 
00873         double frequency;
00874         if (sdh_node.nh_.hasParam("frequency"))
00875         {
00876                 sdh_node.nh_.getParam("frequency", frequency);
00877         }
00878         else
00879         {
00880                 frequency = 5; //Hz
00881                 ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);
00882         }
00883 
00884         //sleep(1);
00885         ros::Rate loop_rate(frequency); // Hz
00886         while(sdh_node.nh_.ok())
00887         {
00888                 // publish JointState
00889                 sdh_node.updateSdh();
00890                 
00891                 // publish TactileData
00892                 sdh_node.updateDsa();
00893                 
00894                 // sleep and waiting for messages, callbacks
00895                 ros::spinOnce();
00896                 loop_rate.sleep();
00897         }
00898 
00899         return 0;
00900 }
00901