schunk_powercube_chain: schunk_powercube_chain: PowerCubeCtrl.cpp Source File

00001 
00060 // ROS includes
00061 #include <ros/ros.h>
00062 
00063 // own includes
00064 #include <schunk_powercube_chain/PowerCubeCtrl.h>
00065 
00066 #define PCTRL_CHECK_INITIALIZED()                               \
00067   if ( isInitialized()==false )                                 \
00068     {                                                           \
00069       m_ErrorMessage.assign("Manipulator not initialized.");    \
00070       return false;                                             \
00071     }
00072 
00073 #define Ready4MoveStep 4638
00074 
00075 /*
00076  * \brief Constructor
00077  */
00078 PowerCubeCtrl::PowerCubeCtrl(PowerCubeCtrlParams * params)
00079 {
00080   m_mutex = PTHREAD_MUTEX_INITIALIZER;
00081 
00082   m_CANDeviceOpened = false;
00083   m_Initialized = false;
00084 
00085   m_params = params;
00086 
00087   m_horizon = 0.01; // sec
00088 
00089   m_last_time_pub = ros::Time::now();
00090 
00091         m_pc_status = PC_CTRL_OK; 
00092 }
00093 
00094 /*
00095  * \brief Destructor
00096  */
00097 PowerCubeCtrl::~PowerCubeCtrl()
00098 {               
00099   // stop all components
00100   Stop(); 
00101         
00102   // close CAN device
00103   if (m_CANDeviceOpened)
00104     {
00105       pthread_mutex_lock(&m_mutex);
00106       PCube_closeDevice(m_DeviceHandle);
00107       pthread_mutex_unlock(&m_mutex);
00108     }
00109 }
00110 
00112 
00117 bool PowerCubeCtrl::Init(PowerCubeCtrlParams * params)
00118 {
00119   int ret = 0;
00120   int DOF = m_params->GetDOF();
00121   std::string CanModule = m_params->GetCanModule();
00122   std::string CanDevice = m_params->GetCanDevice();
00123   std::vector<int> ModulIDs = m_params->GetModuleIDs();
00124   int CanBaudrate = m_params->GetBaudrate();
00125   std::vector<double> MaxVel = m_params->GetMaxVel();
00126   std::vector<double> MaxAcc = m_params->GetMaxAcc();
00127   std::vector<double> Offsets = m_params->GetOffsets();
00128   std::vector<double> LowerLimits = m_params->GetLowerLimits();
00129   std::vector<double> UpperLimits = m_params->GetUpperLimits();
00130 
00132   std::cout << " D  O  F  :" << DOF << std::endl;
00133   m_status.resize(DOF);
00134   m_ModuleTypes.resize(DOF);
00135   m_version.resize(DOF); 
00136   m_dios.resize(DOF);
00137   m_positions.resize(DOF);
00138   m_velocities.resize(DOF);
00139 
00140   std::cout << "=========================================================================== " << std::endl;
00141   std::cout << "PowerCubeCtrl:Init: Trying to initialize with the following parameters: " << std::endl;
00142   std::cout << "DOF: " << DOF << std::endl;
00143   std::cout << "CanModule: " << CanModule << std::endl;
00144   std::cout << "CanDevice: " << CanDevice << std::endl;
00145   std::cout << "CanBaudrate: " << CanBaudrate << std::endl;
00146   std::cout << "ModulIDs: ";
00147   for (int i = 0; i < DOF; i++)
00148     {
00149       std::cout << ModulIDs[i] << " ";
00150     }
00151   std::cout << std::endl;
00152 
00153   std::cout << std::endl << "maxVel: ";
00154   for (int i = 0; i < DOF; i++)
00155     {
00156       std::cout << MaxVel[i] << " ";
00157     }
00158 
00159   std::cout << std::endl << "maxAcc: ";
00160   for (int i = 0; i < DOF; i++)
00161     {
00162       std::cout << MaxAcc[i] << " ";
00163     }
00164 
00165   std::cout << std::endl << "upperLimits: ";
00166   for (int i = 0; i < DOF; i++)
00167     {
00168       std::cout << UpperLimits[i] << " ";
00169     }
00170 
00171   std::cout << std::endl << "lowerLimits: ";
00172   for (int i = 0; i < DOF; i++)
00173     {
00174       std::cout << LowerLimits[i] << " ";
00175     }
00176 
00177   std::cout << std::endl << "offsets: ";
00178   for (int i = 0; i < DOF; i++)
00179     {
00180       std::cout << Offsets[i] << " ";
00181     }
00182 
00183   std::cout << std::endl << "=========================================================================== " << std::endl;
00184   std::ostringstream InitStr;
00185   InitStr << CanModule << ":" << CanDevice << "," << CanBaudrate;
00186   std::cout << "initstring = " << InitStr.str().c_str() << std::endl;
00187 
00189   pthread_mutex_lock(&m_mutex);
00190   ret = PCube_openDevice(&m_DeviceHandle, InitStr.str().c_str());
00191   pthread_mutex_unlock(&m_mutex);
00192   if (ret != 0)
00193     {
00194       std::ostringstream errorMsg;
00195       errorMsg << "Could not open device " << CanDevice << ", m5api error code: " << ret;
00196       m_ErrorMessage = errorMsg.str();
00197       return false;
00198     }
00199 
00200   m_CANDeviceOpened = true;
00201 
00203         int max_tries = 3; 
00204         for (int i = 0; i < DOF; i++)
00205         {  
00206                 for (int reset_try = 0; reset_try < max_tries; i++)
00207                 {
00208                         pthread_mutex_lock(&m_mutex);
00209                         ret =  PCube_resetModule(m_DeviceHandle, ModulIDs.at(i));
00210                         pthread_mutex_unlock(&m_mutex);
00211                         
00212                         if (ret == 0)
00213                         {       
00214                                 break; 
00215                         }
00216                         else if ((ret != 0) && (reset_try == (max_tries-1)))
00217                         {
00218                                 std::ostringstream errorMsg;
00219         errorMsg << "Could not reset module " << i << " during init. Try to init once more.";
00220         m_ErrorMessage = errorMsg.str();
00221                                 return false;
00222                         }
00223                         else
00224                         {
00225                                 // little break
00226                                 usleep(1000000); 
00227                         }
00228                         
00229                 }
00230         }
00231 
00233   ModulIDs.clear();
00234 
00236   pthread_mutex_lock(&m_mutex);
00237   int number_of_modules = PCube_getModuleCount(m_DeviceHandle);
00238   pthread_mutex_unlock(&m_mutex);
00239   std::cout << "found " << number_of_modules << " modules." << std::endl;
00240 
00242   for (int i = 0; i < DOF; i++)
00243   {
00244       unsigned long serNo;
00245       unsigned short verNo;
00246       unsigned long defConfig;
00247       std::vector<std::string> Module_Types; 
00248                 
00250       pthread_mutex_lock(&m_mutex);
00251       ret = PCube_getModuleSerialNo(m_DeviceHandle, ModulIDs[i], &serNo);
00252       pthread_mutex_unlock(&m_mutex);
00253       if (ret != 0)
00254                         {
00255                                 std::ostringstream errorMsg;
00256                                 errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00257                                 m_ErrorMessage = errorMsg.str();        
00258                                 return false;
00259                         }
00260                 
00262       pthread_mutex_lock(&m_mutex);
00263       ret = PCube_getModuleVersion(m_DeviceHandle, ModulIDs[i], &verNo);
00264       pthread_mutex_unlock(&m_mutex);
00265       if (ret != 0)
00266                         {
00267                                 std::ostringstream errorMsg;
00268                                 errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00269                                 m_ErrorMessage = errorMsg.str();        
00270                                 return false;
00271                         }
00272                         else
00273                         {
00274                           m_version[i] = verNo; 
00275                         }       
00276                         
00278                         float gear_ratio;       
00279       pthread_mutex_lock(&m_mutex);
00280       ret = PCube_getDefGearRatio(m_DeviceHandle, ModulIDs[i], &gear_ratio);
00281       pthread_mutex_unlock(&m_mutex);
00282       if (ret != 0)
00283                         {
00284                                 std::ostringstream errorMsg;
00285                                 errorMsg << "Could not get Module type with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00286                                 m_ErrorMessage = errorMsg.str();        
00287                                 return false;
00288                         }
00289                         else
00290                         {
00291                                 if (true)
00292                                 {
00293                                 std::cout << "gear ratio: " << gear_ratio << std::endl; 
00294                                         //return false; 
00295                                 } 
00296                         }       
00297                         
00299                         unsigned char type;     
00300       pthread_mutex_lock(&m_mutex);
00301       ret = PCube_getModuleType(m_DeviceHandle, ModulIDs[i], &type);
00302       pthread_mutex_unlock(&m_mutex);
00303       if (ret != 0)
00304                         {
00305                                 std::ostringstream errorMsg;
00306                                 errorMsg << "Could not get Module type with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00307                                 m_ErrorMessage = errorMsg.str();        
00308                                 return false;
00309                         }
00310                         else
00311                         {
00312                                 if (type != TYPEID_MOD_ROTARY)
00313                                 {
00314                                 std::cout << "wrong module type configured. Type must be rotary axis. Use Windows configuration software to change type." << std::endl; 
00315                                         return false; 
00316                                 } 
00317                         }       
00318 
00320                         // the typ -if PW or PRL- can be distinguished by the typ of encoder. 
00321                         pthread_mutex_lock(&m_mutex);
00322                         ret = PCube_getDefSetup(m_DeviceHandle, ModulIDs[i], &defConfig);
00323                         pthread_mutex_unlock(&m_mutex);
00324 
00325                         ROS_DEBUG("module type check: %li (std::dec)",defConfig); 
00326                         
00327                 if (ret != 0)
00328                         {
00329                         std::ostringstream errorMsg;
00330                                 errorMsg << "Error on communication with module " << ModulIDs[i] << ", m5api error code: " << ret;
00331                                 m_ErrorMessage = errorMsg.str();        
00332                                 return false;
00333                         }       
00334                 
00335                         // Firmware version 4634 of PRL modules replies ABSOULTE_FEEDBACK, firmware 4638 replies RESOLVER_FEEDBACK. 
00336                         // Both means the same: Module is PRL. PW modules have encoders (ENCODER_FEEDBACK, s.M5API), but this bit is not set is DefConfig word.
00337                         // For new firmware versions this needs to be evaluated. 
00338                         if (((defConfig & CONFIG_ABSOLUTE_FEEDBACK)==CONFIG_ABSOLUTE_FEEDBACK) || ((defConfig & CONFIG_RESOLVER_FEEDBACK)==CONFIG_RESOLVER_FEEDBACK))
00339                         {
00340                                 m_ModuleTypes[i] = "PRL"; 
00341                                 ROS_DEBUG("Module %i is from type: PRL", i);
00342                         }
00343                 else
00344                         {
00345                                 m_ModuleTypes[i] = "PW"; 
00346                                 ROS_DEBUG("Module %i is from type: PW", i);
00347                         }
00348 
00350                         std::cout << "Found module " << std::dec << ModulIDs[i] << " Serial: " << serNo << " Version: " << std::hex << verNo << std::endl;
00351 
00352                 }
00353         
00354   // modules should be initialized now
00355   m_pc_status = PC_CTRL_OK;
00356   m_Initialized = true; 
00357 
00359   std::vector<std::string> errorMessages;
00360   PC_CTRL_STATUS status;
00361 
00362   // update status variables
00363   updateStates();
00364 
00365   // grep updated status 
00366   getStatus(status, errorMessages);
00367         
00368   // homing dependant on moduletype and if already homed
00369   bool successful = false;
00370   successful = doHoming();
00371   if (!successful)
00372     {
00373       std::cout << "PowerCubeCtrl:Init: homing not successful, aborting ...\n";
00374       return false;
00375     }
00376 
00378   for (int i = 0; i < DOF; i++)
00379   {
00380     pthread_mutex_lock(&m_mutex);
00381     ret = PCube_setHomeOffset(m_DeviceHandle, ModulIDs[i], Offsets[i]);
00382     pthread_mutex_unlock(&m_mutex);
00383 
00384                 if (ret!=0) 
00385                 {               // 2. chance
00386                                 pthread_mutex_lock(&m_mutex);
00387                 ret = PCube_setHomeOffset(m_DeviceHandle, ModulIDs[i], Offsets[i]);
00388                 pthread_mutex_unlock(&m_mutex);
00389                                 if (ret!=0)
00390                                 {return false;}
00391                 }
00392   }
00393 
00395         //TODO: add safty limit for hardware, that modules don't reach limits
00396   for (int i = 0; i < DOF; i++)
00397   {
00398     pthread_mutex_lock(&m_mutex);
00399     //ret = PCube_setMinPos(m_DeviceHandle, ModulIDs[i], LowerLimits[i]);
00400     pthread_mutex_unlock(&m_mutex);
00401                 if (ret!=0)
00402                 {return false;}
00403 
00404     pthread_mutex_lock(&m_mutex);
00405     //ret = PCube_setMaxPos(m_DeviceHandle, ModulIDs[i], UpperLimits[i]);
00406     pthread_mutex_unlock(&m_mutex);
00407                 if (ret!=0)
00408                 {return false;}
00409   }
00410 
00412   setMaxVelocity(MaxVel);
00413         
00415         setMaxAcceleration(MaxAcc);
00416 
00418   setSyncMotion();
00419 
00420   // All modules initialized successfully
00421   m_pc_status = PC_CTRL_OK;
00422 
00423   return true;
00424 }
00425 
00429 bool PowerCubeCtrl::Close()
00430 {
00431   if (m_CANDeviceOpened)
00432     {
00433       m_Initialized = false;
00434       m_CANDeviceOpened = false;
00435 
00436       pthread_mutex_lock(&m_mutex);
00437       PCube_closeDevice(m_DeviceHandle);
00438       pthread_mutex_unlock(&m_mutex);
00439 
00440       return true;
00441     }
00442 
00443   else
00444     {
00445       return false;
00446     }
00447 }
00448 
00450 
00455 bool PowerCubeCtrl::MoveJointSpaceSync(const std::vector<double>& target)
00456 {
00457   PCTRL_CHECK_INITIALIZED();
00458   unsigned int DOF = m_params->GetDOF();
00459 
00460   std::vector<std::string> errorMessages;
00461   PC_CTRL_STATUS status;
00462   getStatus(status, errorMessages);
00463   if ((status != PC_CTRL_OK))
00464     {
00465       m_ErrorMessage.assign("");
00466       for (unsigned int i = 0; i < DOF; i++)
00467         {
00468           m_ErrorMessage.append(errorMessages[i]);
00469         }
00470       return false;
00471     }
00472 
00473   std::vector<double> vel(DOF);
00474   std::vector<double> acc(DOF);
00475 
00476   double TG = 0;
00477 
00478   try
00479     {
00481       std::vector<double> times(DOF);
00482       for (unsigned int i = 0; i < DOF; i++)
00483         {
00484           RampCommand rm(m_positions[i], m_velocities[i], target[i], m_params->GetMaxAcc()[i],
00485                          m_params->GetMaxVel()[i]);
00486           times[i] = rm.getTotalTime();
00487         }
00488 
00490       int furthest = 0;
00491       double max = times[0];
00492       for (unsigned int i = 1; i < DOF; i++)
00493         {
00494           if (times[i] > max)
00495             {
00496               max = times[i];
00497               furthest = i;
00498             }
00499         }
00500 
00501       RampCommand rm_furthest(m_positions[furthest], m_velocities[furthest], target[furthest],
00502                               m_params->GetMaxAcc()[furthest], m_params->GetMaxVel()[furthest]);
00503 
00504       double T1 = rm_furthest.T1();
00505       double T2 = rm_furthest.T2();
00506       double T3 = rm_furthest.T3();
00507 
00509       TG = T1 + T2 + T3;
00510 
00512       acc[furthest] = m_params->GetMaxAcc()[furthest];
00513       vel[furthest] = m_params->GetMaxVel()[furthest];
00514       for (unsigned int i = 0; i < DOF; i++)
00515         {
00516           if (int(i) != furthest)
00517             {
00518               double a;
00519               double v;
00520               RampCommand::calculateAV(m_positions[i], m_velocities[i], target[i], TG, T3, m_params->GetMaxAcc()[i],
00521                                        m_params->GetMaxVel()[i], a, v);
00522 
00523               acc[i] = a;
00524               vel[i] = v;
00525             }
00526         }
00527     }
00528   catch (...)
00529     {
00530       return false;
00531     }
00532 
00534   for (unsigned int i = 0; i < DOF; i++)
00535     {
00536       pthread_mutex_lock(&m_mutex);
00537       PCube_moveRamp(m_DeviceHandle, m_params->GetModuleIDs()[i], target[i], fabs(vel[i]), fabs(acc[i]));
00538       pthread_mutex_unlock(&m_mutex);
00539     }
00540 
00541   pthread_mutex_lock(&m_mutex);
00542   PCube_startMotionAll(m_DeviceHandle);
00543   pthread_mutex_unlock(&m_mutex);
00544 
00545   return true;
00546 }
00547 
00548 /*
00549  * \brief Move joints with calculated velocities
00550  *
00551  * Calculating positions and times by desired value of the cob_trajectory_controller
00552  * \param velocities Vector
00553  */
00554 bool PowerCubeCtrl::MoveVel(const std::vector<double>& vel)
00555 {
00556   PCTRL_CHECK_INITIALIZED();
00557   
00558   //== init var ==================================================
00559 
00561         unsigned int DOF = m_params->GetDOF();
00562         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00563         
00564         std::vector<double> velocities; 
00565         velocities.resize(DOF); 
00566         velocities = vel;                               // temp var for velocity because of const reference of the function
00567         
00568         std::ostringstream errorMsg;                    // temp error msg for being copied to m_errorMessage
00569         
00570         std::vector<std::string> errorMessages;         // temp error msg for call of getStatus()
00571         PC_CTRL_STATUS status;                                                                          // PowerCube Ctrl status variable
00572 
00573         std::vector<float> delta_pos;                   // traviling distance for next command
00574         std::vector<float> delta_pos_horizon;
00575         delta_pos.resize(DOF);
00576         delta_pos_horizon.resize(DOF);
00577         
00578         std::vector<float> target_pos;          // absolute target postion that is desired with this command
00579         std::vector<float> target_pos_horizon;  
00580         
00581         target_pos.resize(DOF); 
00582         target_pos_horizon.resize(DOF);
00583         float target_time;      // time in milliseconds
00584         float target_time_horizon = 0;
00585   
00586         float delta_t;                  // time from the last moveVel cmd to now
00587 
00589         std::vector<double> LowerLimits = m_params->GetLowerLimits();
00590         std::vector<double> UpperLimits = m_params->GetUpperLimits();
00591         std::vector<double> maxVels = m_params->GetMaxVel();
00592         
00593         int ret;                // temp return value holder
00594         float pos;      // temp position variable for PCube_move.. cmds 
00595 
00596         //== calculate destination position ============================
00597         // needed for limit handling and MoveStep command
00598         
00599   delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
00600         
00601   m_last_time_pub = ros::Time::now();  
00602 
00603   std::vector<double> pos_temp;
00604   pos_temp.resize(DOF);
00605 
00606   for (unsigned int i = 0; i < DOF; i++)
00607   {
00608     // limit step time to 50msec
00609     //TODO: set value 0.05 as parameter
00610         if (delta_t >= 0.050)
00611           {
00612             target_time = 0.050; //sec
00613           }
00614         else
00615           {
00616             target_time = delta_t;      //sec
00617           }
00618         
00619         //add horizon to time before calculation of target position, to influence both time and position at the same time
00620         target_time_horizon = target_time + (float)m_horizon; //sec
00621         // calculate travel distance
00622         delta_pos_horizon[i] = target_time_horizon * velocities[i];
00623         delta_pos[i] = target_time * velocities[i];
00624         
00625                 ROS_DEBUG("delta_pos[%i]: %f target_time: %f velocitiy[%i]: %f",i ,delta_pos[i], target_time, i, velocities[i]);
00626  
00627                 // calculate target position   
00628                 target_pos_horizon[i] = m_positions[i] + delta_pos_horizon[i];
00629                 ROS_DEBUG("target_pos[%i]: %f m_position[%i]: %f",i ,target_pos[i], i, m_positions[i]);
00630         }
00631 
00632         //== check input parameter =====================================
00633   
00635   if (velocities.size() != DOF)
00636     {
00637       m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
00638       return false;
00639     }
00640 
00641   for (unsigned int i = 0; i < DOF; i++)
00642     {
00644       if(velocities[i] > maxVels[i])
00645                         { 
00646                                 // set velocities command to max value
00647                                 velocities[i] = maxVels[i];
00648                 
00649                                 //TODO: add ros_warn    
00650 
00651                                 ROS_INFO("Velocity %f exceeds limit %f for axis %i. moving with max velocity %f instead", velocities[i], maxVels[i], i, maxVels[i]);
00652                         }
00653 
00655                         // TODO: add second limit "safty limit" 
00656                         // if target position is outer limits and the command velocity is in in direction away from working range, skip command
00657       if ((target_pos[i] < LowerLimits[i]) && (velocities[i] < 0))
00658                         {       
00659                                 ROS_INFO("Skipping command: %f Target position exceeds lower limit (%f).", target_pos[i], LowerLimits[i]);              
00660                                 // target position is set to actual position and velocity to Null. So only movement in the non limit direction is possible.
00661         
00662                                 pthread_mutex_lock(&m_mutex);
00663                                 PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
00664                                 pthread_mutex_unlock(&m_mutex);
00665                                 
00666                                 return true; 
00667                         } 
00668                         
00669                         // if target position is outer limits and the command velocity is in in direction away from working range, skip command
00670                         if ((target_pos[i] > UpperLimits[i]) && (velocities[i] > 0))
00671                         {       
00672                                 ROS_INFO("Skipping command: %f Target position exceeds upper limit (%f).", target_pos[i], UpperLimits[i]);              
00673                                 // target position is set to actual position. So only movement in the non limit direction is possible.
00674                                 
00675                                 pthread_mutex_lock(&m_mutex);
00676                                 PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
00677                                 pthread_mutex_unlock(&m_mutex);
00678 
00679                                 return true; 
00680                         } 
00681                 }
00682 
00683   //== check system status ====================================== 
00684   
00685   getStatus(status, errorMessages);
00686         
00687   if ((status != PC_CTRL_OK))
00688   {
00689     m_ErrorMessage.assign("");
00690     for (unsigned int i = 0; i < DOF; i++)
00691                 {
00692                 m_ErrorMessage.append(errorMessages[i]);
00693                 }
00694                 ROS_INFO("Error during movement. Status: %i     \n", status);
00695     return false;
00696   }
00697 
00698   //== send velocity cmd to modules ============================== 
00699 
00700         //convert the time to int in [ms]
00701         unsigned short time4motion = (unsigned short)((target_time_horizon)*1000.0);
00702 
00703         for (unsigned int i = 0; i < DOF; i++)
00704         {
00705                 // check module type sending command (PRL-Modules can be driven with moveStepExtended(), PW-Modules can only be driven with less safe moveVelExtended())
00706                 if ((m_ModuleTypes.at(i) == "PRL") && (m_version[i] >= Ready4MoveStep) && !m_params->GetUseMoveVel())
00707                 {
00708                         //ROS_DEBUG("Modul_id = %i, ModuleType: %s, step=%f, time=%f", m_params->GetModuleID(i), m_ModuleTypes[i].c_str(), target_pos[i], target_time);
00709 
00710                         //ROS_INFO("Modul_id = %i, ModuleType: %s, step=%f, time4motion=%i [ms], target_time=%f, horizon: %f", m_params->GetModuleID(i), m_ModuleTypes[i].c_str(), delta_pos[i], time4motion, target_time, m_horizon);
00711                         
00712                         pthread_mutex_lock(&m_mutex);
00713                         ret = PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), target_pos_horizon[i], time4motion, &m_status[i], &m_dios[i], &pos);
00714                         pthread_mutex_unlock(&m_mutex);
00715                 }
00716                 else    
00717                 {               
00718                         pthread_mutex_lock(&m_mutex);
00719                         ret = PCube_moveVelExtended(m_DeviceHandle, m_params->GetModuleID(i), velocities[i], &m_status[i], &m_dios[i], &pos);
00720                         pthread_mutex_unlock(&m_mutex);
00721                 }
00722 
00724                 if (ret != 0)
00725                 {
00726                         ROS_DEBUG("Com Error: %i", ret);                  
00727                         pos = m_positions[i];
00728                         //m_pc_status = PC_CTRL_ERR;
00729                         //TODO: add error msg for diagnostics if error occours often
00730                 }
00731                 
00732                 // !!! Position in pos is position before moveStep movement, to get the expected position after the movement (required as input to the next moveStep command) we add the delta position (cmd_pos) !!!
00733                 m_positions[i] = (double)pos + delta_pos[i];
00734                 
00735                 pos_temp[i] = (double)pos;
00736                 //ROS_INFO("After cmd (%X) :m_positions[%i] %f = pos: %f + delta_pos[%i]: %f",m_status[i], i, m_positions[i], pos, i, delta_pos[i]);
00737         }
00738 
00739         updateVelocities(pos_temp, delta_t);
00740         
00741         //std::cout << "vel_com: " << velocities[1] << " vel_hori: " << delta_pos_horizon[1]/   target_time_horizon << " vel_real[1]: " << m_velocities.at(1) << std::endl;
00742 
00743         pthread_mutex_lock(&m_mutex);
00744         PCube_startMotionAll(m_DeviceHandle);
00745         pthread_mutex_unlock(&m_mutex);
00746         
00747         return true;
00748 }
00749 
00750 // Calculation of velocities based on vel = 1/(6*dt) * (-pos(t-3) - 3*pos(t-2) + 3*pos(t-1) + pos(t))
00751 void PowerCubeCtrl::updateVelocities(std::vector<double> pos_temp, double delta_t)
00752 {
00753   unsigned int DOF = m_params->GetDOF();
00754 
00755         if (m_cached_pos.empty())
00756         {       
00757                 std::vector<double> null; 
00758                 for (unsigned int i=0;i<DOF;i++){       null.push_back(0); }  
00759 
00760                 m_cached_pos.push_back(null);
00761                 m_cached_pos.push_back(null);
00762                 m_cached_pos.push_back(null);
00763                 m_cached_pos.push_back(null);
00764         }
00765 
00766         m_cached_pos.push_back(pos_temp);
00767         m_cached_pos.pop_front();
00768 
00769         std::vector<double> last_pos = m_cached_pos.front();
00770 
00771     for(unsigned int i = 0; i < DOF; i++)
00772         {
00773                 m_velocities[i] = 1/(6*delta_t) * (-m_cached_pos[0][i]-(3*m_cached_pos[1][i])+(3*m_cached_pos[2][i])+m_cached_pos[3][i]);
00774                 //m_velocities[i] = (m_cached_pos[3][i] - m_cached_pos[2][i])/delta_t;          
00775                 //m_velocities[i] = (pos_temp.at(i)-last_pos.at(i))/delta_t; 
00776         }   
00777 }
00778 
00779 
00780 
00784 bool PowerCubeCtrl::Stop()
00785 {       
00787   unsigned int DOF = m_params->GetDOF();
00788         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00789 
00791 
00792 
00793         for (unsigned int i = 0; i < DOF; i++)
00794   {  
00795                 pthread_mutex_lock(&m_mutex);
00796                 int ret =  PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
00797                 pthread_mutex_unlock(&m_mutex);
00798                 if (ret != 0)
00799                   {
00800                     std::ostringstream errorMsg;
00801                     errorMsg << "Could not reset all modules, m5api error code: " << ret;
00802                     m_ErrorMessage = errorMsg.str();
00803                     return false;
00804                   }
00805         }
00806 
00808   usleep(500000);
00809 
00811         for (unsigned int i = 0; i < DOF; i++)
00812   {  
00813                 pthread_mutex_lock(&m_mutex);
00814                 int ret =  PCube_resetModule(m_DeviceHandle, ModulIDs.at(i));
00815                 pthread_mutex_unlock(&m_mutex);
00816                 if (ret != 0)
00817                   {
00818                     std::ostringstream errorMsg;
00819                     errorMsg << "Could not reset all modules, m5api error code: " << ret;
00820                     m_ErrorMessage = errorMsg.str();
00821                     return false;
00822                   }
00823         }
00824   return true;
00825 }
00826 
00830 bool PowerCubeCtrl::Recover()
00831 {       
00832   unsigned int DOF = m_params->GetDOF();
00833         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00834   std::vector<double> MaxVel = m_params->GetMaxVel();
00835   std::vector<double> MaxAcc = m_params->GetMaxAcc();
00836   std::vector<double> Offsets = m_params->GetOffsets();
00837   
00838   std::vector<std::string> errorMessages;
00839   PC_CTRL_STATUS status;
00840         
00841   unsigned long state = PC_CTRL_OK;
00842   unsigned char dio;
00843   float position;
00844   int ret = 0;
00845         
00846   // check for each module if reset is necessary
00847   for (unsigned int i = 0; i < DOF; i++)
00848   {     
00849     pthread_mutex_lock(&m_mutex);
00850     ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
00851     pthread_mutex_unlock(&m_mutex);
00852     if (ret != 0)
00853                 {
00854                   m_pc_status = PC_CTRL_ERR;
00855                   std::cout << "State: Error com with Module: " << i << " Time: " << ros::Time::now() << std::endl;     
00856                   return false;                 
00857                 }
00858                 
00859       // if module is in error state --> reset
00860     if (state & STATEID_MOD_ERROR)
00861                 {       
00862                   pthread_mutex_lock(&m_mutex);
00863                   ret = PCube_resetModule(m_DeviceHandle, m_params->GetModuleID(i));
00864                   pthread_mutex_unlock(&m_mutex);
00865                   if (ret != 0)
00866             {
00867               m_pc_status = PC_CTRL_ERR;
00868               std::cout << "State: Error com with Module: " << i << " Time: " << ros::Time::now() << std::endl;         
00869               return false;             
00870             }
00871                 }
00872   }
00873         
00874   // time for reboot
00875   usleep(500000); 
00876 
00877   // check is everything is ok now
00878   updateStates(); 
00879 
00880   if (m_pc_status == PC_CTRL_NOT_HOMED)
00881   {
00882     if (!doHoming())
00883                 {
00884                   return false;
00885                 }
00886   }
00887         
00888   usleep(500000);
00889 
00891   for (int i = 0; i < DOF; i++)
00892   {
00893     pthread_mutex_lock(&m_mutex);
00894     ret = PCube_setHomeOffset(m_DeviceHandle, ModulIDs[i], Offsets[i]);
00895     pthread_mutex_unlock(&m_mutex);
00896 
00897                 if (ret!=0) 
00898                 {               // 2. chance
00899                                 pthread_mutex_lock(&m_mutex);
00900                 ret = PCube_setHomeOffset(m_DeviceHandle, ModulIDs[i], Offsets[i]);
00901                 pthread_mutex_unlock(&m_mutex);
00902                                 if (ret!=0)
00903                                 {return false;}
00904                 }
00905   }
00906 
00908   setMaxVelocity(MaxVel);
00909         
00911         setMaxAcceleration(MaxAcc);
00912         
00914   setSyncMotion();
00915 
00916   // modules should be recovered now
00917   m_pc_status = PC_CTRL_OK;     
00918         
00919   updateStates(); 
00920   // check if modules are really back to normal state
00921   getStatus(status, errorMessages);
00922 
00923   if ((status != PC_CTRL_OK))
00924   {
00925           m_ErrorMessage.assign("");
00926 
00927     for (int i = 0; i < m_params->GetDOF(); i++)
00928                 {
00929                   m_ErrorMessage.append(errorMessages[i]);
00930                 }
00931     return false;
00932   }
00933 
00935   m_pc_status = PC_CTRL_OK;
00936   return true;
00937 }
00938 
00944 bool PowerCubeCtrl::setMaxVelocity(double maxVelocity)
00945 {
00946         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00947 
00948   PCTRL_CHECK_INITIALIZED();
00949   for (int i = 0; i < m_params->GetDOF(); i++)
00950     {
00951       pthread_mutex_lock(&m_mutex);
00952       int ret = PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocity);
00953       pthread_mutex_unlock(&m_mutex);
00954                         if (ret!=0)
00955                         {       
00956                                 std::ostringstream errorMsg;
00957                                 errorMsg << "Could not set MaxVelocity in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00958                                 m_ErrorMessage = errorMsg.str();
00959                                 return false;
00960                         }
00961 
00962       std::vector<double> maxVelocities(maxVelocity);
00963       m_params->SetMaxVel(maxVelocities);
00964     }
00965 
00966   return true;
00967 }
00968 
00972 bool PowerCubeCtrl::setMaxVelocity(const std::vector<double>& maxVelocities)
00973 {       
00974   PCTRL_CHECK_INITIALIZED();
00975         
00976         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00977         
00978   for (int i = 0; i < m_params->GetDOF(); i++)
00979     {
00980       pthread_mutex_lock(&m_mutex);
00981       //std::cout << "------------------------------> PCube_setMaxVel()" << std::endl;
00982       int ret = PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocities[i]);
00983       pthread_mutex_unlock(&m_mutex);
00984                         if (ret!=0)
00985                         {       
00986                                 std::ostringstream errorMsg;
00987                                 errorMsg << "Could not set MaxVelocity in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00988                                 m_ErrorMessage = errorMsg.str();
00989                                 return false;
00990                         }
00991 
00992       m_params->SetMaxVel(maxVelocities);
00993     }
00994 
00995   return true;
00996 }
00997 
01003 bool PowerCubeCtrl::setMaxAcceleration(double maxAcceleration)
01004 {
01005   PCTRL_CHECK_INITIALIZED();
01006         
01007         std::vector<int> ModulIDs = m_params->GetModuleIDs();
01008 
01009   for (int i = 0; i < m_params->GetDOF(); i++)
01010     {
01011       pthread_mutex_lock(&m_mutex);
01012       //std::cout << "------------------------------> PCube_setMaxAcc()" << std::endl;
01013       int ret = PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAcceleration);
01014       pthread_mutex_unlock(&m_mutex);
01015                         if (ret!=0)
01016                         {       
01017                                 std::ostringstream errorMsg;
01018                                 errorMsg << "Could not set MaxAcceleration in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
01019                                 m_ErrorMessage = errorMsg.str();
01020                                 return false;
01021                         }
01022 
01023       std::vector<double> maxAccelerations(maxAcceleration);
01024       m_params->SetMaxAcc(maxAccelerations);
01025     }
01026 
01027   return true;
01028 }
01029 
01033 bool PowerCubeCtrl::setMaxAcceleration(const std::vector<double>& maxAccelerations)
01034 {
01035   PCTRL_CHECK_INITIALIZED();
01036         
01037         std::vector<int> ModulIDs = m_params->GetModuleIDs();
01038 
01039   for (int i = 0; i < m_params->GetDOF(); i++)
01040     {
01041       pthread_mutex_lock(&m_mutex);
01042       int ret = PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAccelerations[i]);
01043       pthread_mutex_unlock(&m_mutex);
01044                         if (ret!=0)
01045                         {       
01046                                 std::ostringstream errorMsg;
01047                                 errorMsg << "Could not set MaxAcceleration in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
01048                                 m_ErrorMessage = errorMsg.str();
01049                                 return false;
01050                         }
01051 
01052       m_params->SetMaxAcc(maxAccelerations);
01053     }
01054 
01055   return true;
01056 }
01057 
01064 bool PowerCubeCtrl::setHorizon(double horizon)
01065 {
01066   m_horizon = horizon;
01067 
01068   return true;
01069 }
01070 
01077 double PowerCubeCtrl::getHorizon()
01078 {
01079   return m_horizon;
01080 }
01081 
01087 bool PowerCubeCtrl::setSyncMotion()
01088 {
01089         std::vector<int> ModulIDs = m_params->GetModuleIDs();
01090 
01091   if (m_CANDeviceOpened)
01092   {
01093                 for (int i = 0; i < m_params->GetDOF(); i++)
01094                 {
01095                 unsigned long confword;
01096 
01098                 pthread_mutex_lock(&m_mutex);
01099                 PCube_getConfig(m_DeviceHandle, m_params->GetModuleID(i), &confword);
01100                 pthread_mutex_unlock(&m_mutex);
01101 
01103                 pthread_mutex_lock(&m_mutex);
01104                 int ret = PCube_setConfig(m_DeviceHandle, m_params->GetModuleID(i), confword | CONFIGID_MOD_SYNC_MOTION);
01105                 pthread_mutex_unlock(&m_mutex);
01106                         if (ret!=0)
01107                         {       
01108                                 std::ostringstream errorMsg;
01109                                 errorMsg << "Could not set SyncMotion in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
01110                                 m_ErrorMessage = errorMsg.str();
01111                                 return false;
01112                         }
01113                 }
01114       return true;
01115     }
01116   else
01117     {
01118       return false;
01119     }
01120 }
01126 bool PowerCubeCtrl::setASyncMotion()
01127 {
01128   if (m_CANDeviceOpened)
01129     {
01130       for (int i = 0; i < m_params->GetDOF(); i++)
01131         {
01132           unsigned long confword;
01133 
01135           pthread_mutex_lock(&m_mutex);
01136           PCube_getConfig(m_DeviceHandle, m_params->GetModuleID(i), &confword);
01137           pthread_mutex_unlock(&m_mutex);
01138 
01140           pthread_mutex_lock(&m_mutex);
01141           PCube_setConfig(m_DeviceHandle, m_params->GetModuleID(i), confword & (~CONFIGID_MOD_SYNC_MOTION));
01142           pthread_mutex_unlock(&m_mutex);
01143         }
01144       return true;
01145     }
01146 
01147   else
01148     {
01149       return false;
01150     }
01151 }
01155 bool PowerCubeCtrl::updateStates()
01156 {       
01157   PCTRL_CHECK_INITIALIZED();
01158 
01159   unsigned int DOF = m_params->GetDOF();
01160   unsigned long state;
01161   PC_CTRL_STATUS pc_status = PC_CTRL_ERR; 
01162   std::vector<std::string> ErrorMessages;
01163   std::ostringstream errorMsg;
01164 
01165   unsigned char dio;
01166   float position;
01167   int ret = 0;
01168 
01169   for (unsigned int i = 0; i < DOF; i++)
01170     {   
01171       state = m_status[i]; 
01172       pthread_mutex_lock(&m_mutex);
01173       ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
01174       pthread_mutex_unlock(&m_mutex);
01175                 
01176       if (ret != 0)
01177         {
01178           //m_pc_status = PC_CTRL_ERR;
01179           ROS_DEBUG("Error on com in UpdateStates");
01180           return true;
01181           //errorMsg << "State: Error com with Module [" <<  i << "]";
01182           //ErrorMessages[i] = errorMsg.str();                  
01183         }
01184       else
01185         {       
01186           ROS_DEBUG("Module %i, State: %li, Time: %f",i, state, ros::Time::now().toSec());
01187           
01188           m_status[i] = state;          
01189           m_dios[i] = dio;
01190           m_positions[i] = position;
01191         }       
01192       
01193     }
01194 
01195         /*
01196   double delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
01197   m_last_time_pub = ros::Time::now();  
01198 
01199   updateVelocities(m_positions, delta_t);
01200         */
01201 
01202   // evaluate state for translation for diagnostics msgs
01203   getStatus(pc_status, ErrorMessages);
01204 
01205   for (unsigned int i=0; i<ErrorMessages.size(); i++)
01206   {     
01207                 m_ErrorMessage.clear();
01208                 m_ErrorMessage.assign("");
01209     m_ErrorMessage.append(ErrorMessages[i]);
01210   }
01211 
01212   return true;
01213 }
01214 
01218 bool PowerCubeCtrl::getStatus(PC_CTRL_STATUS& status, std::vector<std::string>& errorMessages)
01219 {       
01220   unsigned int DOF = m_params->GetDOF();
01221   std::vector<int> ModuleIDs = m_params->GetModuleIDs();
01222         
01223         std::vector<PC_CTRL_STATUS> StatusArray;
01224         StatusArray.resize(DOF);        
01225 
01226   errorMessages.clear();
01227   errorMessages.resize(DOF);
01228 
01229   status = PC_CTRL_OK;
01230 
01231   for (unsigned int i = 0; i < DOF; i++)
01232   {
01233                 std::ostringstream errorMsg;
01234                 
01235                 if (m_status[i] & STATEID_MOD_POWERFAULT)
01236                 {       
01237                         if (m_status[i] & STATEID_MOD_POW_VOLT_ERR)
01238                         {
01239                                 errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01240                                 errorMsg << "Motor voltage below minimum value! Check Emergency Stop!";
01241                                 errorMessages[i] = errorMsg.str();
01242                         }
01243                         else if (m_status[i] & STATEID_MOD_POW_FET_TEMP)
01244                         {
01245                                 errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01246                                 errorMsg << "Overheated power transitors! Power must be switched of to reset this error.";
01247                                 errorMessages[i] = errorMsg.str();
01248                         }
01249                         else if (m_status[i] & STATEID_MOD_POW_INTEGRALERR)
01250                         {
01251                                 errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01252                                 errorMsg << "The drive has been overloaded and the servo loop has been disabled. Power must be switched off to reset this error.";
01253                                 errorMessages[i] = errorMsg.str();
01254                         }
01255                         StatusArray[i] = PC_CTRL_POW_VOLT_ERR;
01256                 }
01257     else if (m_status[i] & STATEID_MOD_TOW_ERROR)
01258                 {
01259                         errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01260                         errorMsg << "The servo loop was not able to follow the target position!";
01261                         errorMessages[i] = errorMsg.str();
01262                         StatusArray[i] = PC_CTRL_ERR;
01263                 }
01264     else if (m_status[i] & STATEID_MOD_ERROR)
01265                 {       
01266                         // STOP the motion for each module
01267                         pthread_mutex_lock(&m_mutex);
01268                         PCube_haltModule(m_DeviceHandle, m_params->GetModuleID(i));
01269                         pthread_mutex_unlock(&m_mutex);
01270                          
01271                         errorMsg << "Error in  Module " << ModuleIDs[i];
01272                         errorMsg << " : Status code: " << std::hex << m_status[i];
01273                         errorMessages[i] = errorMsg.str();
01274                         StatusArray[i] = PC_CTRL_ERR;
01275                 }
01276     else if (m_pc_status & PC_CTRL_ERR)
01277                 {       
01278                         Stop(); // stop all motion
01279                         errorMsg << "PowerCubeCtrl is in global error state";
01280                         errorMessages[i] = errorMsg.str();
01281                         StatusArray[i] =  PC_CTRL_ERR;
01282                 }
01283     else
01284                 {
01285                         errorMsg << "Module with Id " << ModuleIDs[i];
01286                         errorMsg << ": Status OK.";
01287                         errorMessages[i] = errorMsg.str();
01288                         StatusArray[i] = PC_CTRL_OK;
01289                 }
01290         }
01291         
01292         // search for worst status
01293         for (unsigned int i = 0; i < DOF; i++)
01294   {     
01295                 if ((int)StatusArray[i] <= (int)status)
01296                 {
01297                         status = StatusArray[i]; 
01298                 }       
01299         }
01300         
01301         m_pc_status = status;
01302 
01303   return true;
01304 }
01305 
01309 std::vector<unsigned long> PowerCubeCtrl::getVersion()
01310 {
01311   return m_version; 
01312 }
01316 bool PowerCubeCtrl::statusMoving()
01317 {
01318   PCTRL_CHECK_INITIALIZED();
01319 
01320   for (int i = 0; i < m_params->GetDOF(); i++)
01321     {
01322       if (m_status[i] & STATEID_MOD_MOTION)
01323         return true;
01324     }
01325   return false;
01326 }
01327 
01331 std::vector<double> PowerCubeCtrl::getPositions()
01332 {
01333   return m_positions;
01334 }
01335 
01339 std::vector<double> PowerCubeCtrl::getVelocities()
01340 {
01342   return m_velocities;
01343 }
01344 
01348 std::vector<double> PowerCubeCtrl::getAccelerations()
01349 {
01351   return m_accelerations;
01352 }
01353 
01359 bool PowerCubeCtrl::doHoming()
01360 {
01361         unsigned int DOF = m_params->GetDOF();
01362         std::vector<int> ModuleIDs = m_params->GetModuleIDs();
01363         std::vector<double> LowerLimits = m_params->GetLowerLimits();
01364         std::vector<double> UpperLimits = m_params->GetUpperLimits();
01365 
01367   double max_homing_time = 15.0; // seconds   
01368   double homing_time = 999.0;   // set to 0 if any module is homed
01369   double intervall = 0.1;
01370         
01371   unsigned long state = PC_CTRL_ERR;
01372   unsigned char dio;
01373   float position;
01374   int ret = 0;
01375         
01377   for (unsigned int i = 0; i < DOF; i++)
01378     {   
01379         pthread_mutex_lock(&m_mutex);
01380         ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
01381         pthread_mutex_unlock(&m_mutex);
01382                 
01383         // check and init m_position variable for trajectory controller
01384         if ((position > UpperLimits[i]) || (position < LowerLimits[i]))
01385         {       
01386                 std::ostringstream errorMsg;
01387                 errorMsg << "Module " << ModuleIDs[i] << " has position " << position << " that is outside limits (" << UpperLimits[i] << " <-> " << LowerLimits[i] << std::endl; 
01388                 if ((m_ModuleTypes.at(i)=="PW") || (m_ModuleTypes.at(i) == "other"))
01389                 {       std::cout << "Position error for PW-Module. Init is aborted. Try to reboot the robot." << std::endl;
01390                         m_ErrorMessage = errorMsg.str(); 
01391                         m_pc_status = PC_CTRL_ERR;
01392                         return false;
01393                 }
01394                 else if (m_ModuleTypes.at(i)=="PRL") 
01395                 {       
01396                         ROS_INFO("Position of Module %i is outside limits. Module can only be moved in opposite direction.",i );
01397                         ROS_INFO("Homing for Module: %i not necessary", m_params->GetModuleID(i));
01398                 }       
01399                 else 
01400                 {       
01401                         ROS_INFO("Module type incorrect. (in func. PowerCubeCtrl::doHoming();)");
01402                         return false;
01403                 }
01404         }
01405         else
01406         {
01407                 m_positions[i] = position; 
01408         }
01409 
01410         // check module type before homing (PRL-Modules need not to be homed by ROS)
01411         if ( (m_ModuleTypes.at(i) == "PW") || (m_ModuleTypes.at(i) == "other") )
01412         {       
01413                 pthread_mutex_lock(&m_mutex);
01414                 ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
01415                 pthread_mutex_unlock(&m_mutex);
01416         
01417                 if (ret != 0)
01418                 {       
01419                         ROS_INFO("Error on communication with Module: %i.", m_params->GetModuleID(i)); 
01420                         m_pc_status = PC_CTRL_ERR;
01421                         return false;
01422                 }
01423                         
01424                 // only do homing when necessary 
01425                 if (!(state & STATEID_MOD_HOME))
01426                 {
01427                         // homing timer
01428                         homing_time = 0.0;
01429 
01430                           pthread_mutex_lock(&m_mutex);
01431                           ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
01432                           pthread_mutex_unlock(&m_mutex);
01433 
01434                           ROS_INFO("Homing started at: %f", ros::Time::now().toSec()); 
01435 
01436                           if (ret != 0)
01437                         {        
01438                           ROS_INFO("Error while sending homing command to Module: %i. I try to reset the module.", i); 
01439 
01440                           // reset module with the hope that homing works afterwards
01441                           pthread_mutex_lock(&m_mutex);
01442                           ret = PCube_resetModule(m_DeviceHandle, ModuleIDs[i]);
01443                           pthread_mutex_unlock(&m_mutex);
01444                           if (ret != 0)
01445                                 {       
01446                                   std::ostringstream errorMsg;
01447                                   errorMsg << "Can't reset module after homing error" << ModuleIDs[i] << ", m5api error code: " << ret;
01448                                   m_ErrorMessage = errorMsg.str();
01449                                 }
01450 
01451                           // little break for reboot
01452                           usleep(200000); 
01453 
01454                           pthread_mutex_lock(&m_mutex);
01455                           ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
01456                           pthread_mutex_unlock(&m_mutex);
01457                           if (ret != 0)
01458                                 {       
01459                                   std::ostringstream errorMsg;
01460                                   errorMsg << "Can't start homing for module " << ModuleIDs[i] << ", tried reset with no success, m5api error code: " << ret;
01461                                   m_ErrorMessage = errorMsg.str();
01462                                   return false;                         
01463                                 }
01464 
01465                         }
01466             }else
01467             {
01468               ROS_INFO("Homing for Module: %i not necessary", m_params->GetModuleID(i));
01469             } 
01470         }else
01471         {
01472           ROS_INFO("Homing for PRL-Module %i not necessary", m_params->GetModuleID(i));
01473         } 
01474     }
01475 
01476   for (unsigned int i = 0; i < DOF; i++)
01477     {   
01478       unsigned long int help; 
01479       do
01480                         {
01481                                 pthread_mutex_lock(&m_mutex);
01482                                 PCube_getModuleState(m_DeviceHandle, ModuleIDs[i], &help);
01483                                 pthread_mutex_unlock(&m_mutex);
01484                                 ROS_DEBUG("Homing active for Module: %i State: %li", ModuleIDs.at(i), help);
01485 
01487                                 usleep(intervall * 1000000);    // convert sec to usec
01488                                 homing_time += intervall; 
01489                                 if (homing_time >= max_homing_time) {Stop(); break;} 
01490 
01491                         } while ((help & STATEID_MOD_HOME) == 0);
01492      
01493                         m_status[i] = help;
01494       ROS_DEBUG("State of Module %i : %li", ModuleIDs.at(i), help);
01495     }
01496 
01497   for (unsigned int i = 0; i < DOF; i++)
01498     {   
01500       if (!(m_status[i] & STATEID_MOD_HOME) || (m_status[i] & STATEID_MOD_ERROR) )
01501                         {
01502                                 std::cout << "Homing failed: Error in  Module " << ModuleIDs[i] << std::endl;
01503                                 m_pc_status = PC_CTRL_NOT_HOMED;
01504                                 return false;
01505                         }
01506                 
01507       ROS_INFO("Homing for Modul %i done.", ModuleIDs.at(i)); 
01508     }
01509 
01510   // modules successfully homed
01511   m_pc_status = PC_CTRL_OK;
01512   return true;
01513 }
01514 
01518 bool m_TranslateError2Diagnosics(std::ostringstream* errorMsg)
01519 {
01520   return true;
01521 }