riq_hand_ethercat_hardware: riq_hand_ethercat_hardware: riq

00001 
00002 #include "riq_hand_ethercat_hardware/riq_hand.h"
00003 #include "riq_hand_ethercat_hardware/riq_hand_state.h"
00004 #include "riq_hand_ethercat_hardware/riq_hand_command.h"
00005 
00006 #include <iomanip>
00007 
00008 #include <boost/static_assert.hpp>
00009 
00010 #include <ros/console.h>
00011 
00012 #include <math.h>
00013 #include <algorithm>
00014 
00015 // Product code = 11
00016 PLUGINLIB_REGISTER_CLASS(11, riq_hand_ethercat_hardware::RIQHand, EthercatDevice);
00017 
00018 namespace riq_hand_ethercat_hardware
00019 {
00020 
00021 RIQHandDiagnostics::RIQHandDiagnostics() :
00022   al_status_good_(false)
00023 {
00024   // empty
00025 }
00026 
00027 void RIQHand::sizeAssert()
00028 {
00029   BOOST_STATIC_ASSERT(sizeof(RIQHandStateEcat) == STATUS_SIZE);
00030   BOOST_STATIC_ASSERT(sizeof(RIQHandCommandEcat) == COMMAND_SIZE);
00031 }
00032 
00033 RIQHand::RIQHand() :
00034   state_cycle_count_(0),
00035   should_publish_(true),
00036   needs_watchdog_reset_(true),
00037   needs_reset_(false),
00038   needs_reset_countdown_(0),
00039   latched_fault_code_(RIQHandStateEcat::NO_FAULT),
00040   after_reset_countdown_(0),
00041   after_watchdog_reset_countdown_(0),
00042   halted_(false)
00043 {
00044   //empty
00045 }
00046  
00047 RIQHand::~RIQHand()
00048 {
00049   delete sh_->get_fmmu_config();
00050   delete sh_->get_pd_config();
00051 }
00052 
00053 void RIQHand::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00054 {
00055   EthercatDevice::construct(sh, start_address);
00056 
00057   command_size_ = COMMAND_SIZE;
00058   status_size_  = STATUS_SIZE;
00059 
00060   EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00061   (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
00062                        command_size_, // Logical length
00063                        0x00,  // Logical StartBit
00064                        0x07,  // Logical EndBit
00065                        COMMAND_PHY_ADDR, // Physical Start address
00066                        0x00,   // Physical StartBit
00067                        false,  // Read Enable
00068                        true,   // Write Enable
00069                        true);  // Enable  
00070 
00071   start_address += command_size_;
00072 
00073   (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
00074                        status_size_,  // Logical length
00075                        0x00,  // Logical StartBit
00076                        0x07,  // Logical EndBit
00077                        STATUS_PHY_ADDR, // Physical Start address
00078                        0x00,  // Physical StartBit
00079                        true,  // Read Enable
00080                        false, // Write Enable
00081                        true); // Enable
00082 
00083   start_address += status_size_;
00084 
00085   sh->set_fmmu_config(fmmu);
00086 
00087 
00088   EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(2);
00089 
00090   // Sync manager for command data - SyncMan Control Register
00091   // <Sm MinSize="0" MaxSize="1024" DefaultSize="200" StartAddress="#x1100" ControlByte="#x74" Enable="1">Outputs</Sm>
00092   (*pd)[0] =
00093     EC_SyncMan (
00094                 COMMAND_PHY_ADDR,         // 0-1   Physical Addr    -
00095                 command_size_,            // 2-3   Length           - 
00096                 EC_BUFFERED,              // 4.1:0 Buffer type      -  Operation Mode (Buffered/Mailbox)
00097                 EC_WRITTEN_FROM_MASTER,   // 4.3:2 Direction        -  
00098                                           //          Read: ECAT read, PDI write  or  
00099                                           //          Write: ECAT write, PDI read
00100                 true,                     // 4.5   AL event enable  -  Interrupt ECAT Event Request
00101                 true,                     // 4.6   Watchdog enable  -  Watchdog Trigger Enable
00102                 false,                    // 5.0   Write event      -  Interrupt Write
00103                 false,                    // 5.1   Read event       -  Interrupt Read
00104                 false,                    // 5.2   Watchdog trigger -  ?? Reserved ??
00105                 false,                    // 5.5:3 Queued state     -  Mailbox empty/full  
00106                 EC_FIRST_BUFFER,          // 5.5:4 BufferedState    -  Last written buffer
00107                 true);                    // 7.0   Channel Enable   -  
00108   (*pd)[0].ECATEventEnable = 1;
00109 
00110 
00111   // Sync manager for status data - SyncMan Control Register = 0x20
00112   // <Sm MinSize="0" MaxSize="1024" DefaultSize="200" StartAddress="#x1D00" ControlByte="#x30" Enable="1">Inputs</Sm>
00113   (*pd)[1] =
00114     EC_SyncMan (
00115                 STATUS_PHY_ADDR,          // 0-1   Physical Addr    -
00116                 status_size_,             // 2-3   Length           - 
00117                 EC_BUFFERED,              // 4.1:0 Buffer type      -  Operation Mode (Buffered/Mailbox)
00118                 EC_READ_FROM_MASTER,      // 4.3:2 Direction        -  
00119                                           //          Read: ECAT read, PDI write  or  
00120                                           //          Write: ECAT write, PDI read
00121                 true,                     // 4.5   AL event enable  -  Interrupt ECAT Event Request
00122                 false,                    // 4.6   Watchdog enable  -  Watchdog Trigger Enable
00123                 false,                    // 5.0   Write event      -  Interrupt Write
00124                 false,                    // 5.1   Read event       -  Interrupt Read
00125                 false,                    // 5.2   Watchdog trigger -  ?? Reserved ??
00126                 false,                    // 5.5:3 Queued state     -  Mailbox empty/full  
00127                 EC_FIRST_BUFFER,          // 5.5:4 BufferedState    -  Last written buffer
00128                 true);                    // 7.0   Channel Enable   -  
00129   (*pd)[1].ECATEventEnable = 1;
00130 
00131   sh->set_pd_config(pd);
00132 
00133 
00134   EtherCAT_MbxConfig *mbx = new EtherCAT_MbxConfig;
00135 
00136   // Sync manager for Master->Slave Mailbox - SyncMan Control Register = 0x26
00137   mbx->SM0 =
00138     EC_SyncMan(
00139                MBX_COMMAND_PHY_ADDR,
00140                MBX_COMMAND_SIZE,
00141                EC_QUEUED,                //
00142                EC_WRITTEN_FROM_MASTER,     //
00143                true,                       // Trigger AL interupt (to uC/FPGA slave) when buffer is writen by master
00144                false,                      // no watchdog
00145                false,                      // (ro)
00146                false,                      // (ro)
00147                false,                      // (ro)
00148                false,                      // (ro)
00149                EC_FIRST_BUFFER,            // (ro) which buffer was last written
00150                true);                      // enable syncmanager
00151   mbx->SM0.ECATEventEnable = 1;
00152 
00153   // Sync manager for Slave->Master Mailbox - SyncMan Control Register = 0x22
00154   mbx->SM1 =
00155     EC_SyncMan(
00156                MBX_STATUS_PHY_ADDR,
00157                MBX_STATUS_SIZE,
00158                EC_QUEUED,                  //
00159                EC_READ_FROM_MASTER,        //
00160                true,                       // Don't AL interupt (to uC/FPGA slave) when buffer is read by master
00161                false,                      // no watchdog
00162                false,                      // (ro)
00163                false,                      // (ro)
00164                false,                      // (ro)
00165                false,                      // (ro)
00166                EC_FIRST_BUFFER,            // which buffer was last written (ro)
00167                true);
00168   //mbx->SM1.ECATEventEnable = 1;
00169 
00170   sh->set_mbx_config(mbx);
00171 
00172   // Disable watchdog as early as possible (may not be needed any more)
00173   // disableWatchdog(sh);
00174 }
00175 
00176 int RIQHand::initialize(pr2_hardware_interface::HardwareInterface *, bool)
00177 {
00178   ROS_DEBUG("Device #%02d: RIQ Hand (%#08x)", sh_->get_ring_position(), sh_->get_product_code());
00179 
00180   if (use_ros_)
00181   {
00182     ros::NodeHandle nh;
00183 
00184     // Register ROS subscriber for hand command
00185     commandSubscriber_ = nh.subscribe("riq_hand_command", 2, &RIQHand::commandCallback, this);
00186     
00187     // Intialize realtime publisher
00188     publisher_.init(nh, "riq_hand_state", 1);
00189   }
00190 
00191   return 0;
00192 }
00193 
00194 void RIQHand::commandCallback(const riq_msgs::RIQHandCommand::ConstPtr& msg)
00195 {
00196   command_mutex_.lock();
00197   new_command_ = *msg;    
00198   command_mutex_.unlock();
00199 }
00200 
00201 
00202 unsigned RIQHand::scaleAndSaturate255(double value)
00203 {
00204   value = double(value)*255.0;
00205   value = roundf(value);
00206   value = std::max(0.0, std::min( 255.0, value ) );
00207   return unsigned(value); 
00208 }  
00209 
00210 void RIQHand::packCommand(unsigned char *buffer, bool halt, bool reset)
00211 {
00212   // Get newest command data (if possible)
00213   if (command_mutex_.try_lock())
00214   {
00215     command_ = new_command_;
00216     command_mutex_.unlock();
00217   } 
00218 
00219   // Request watch reset (by needs to be done by non-realtime thread)
00220   if (reset)
00221   {
00222     needs_watchdog_reset_ = true;
00223     latched_fault_code_ = 0;
00224     if (needs_reset_)
00225     {
00226       // When device need reset (re initialization) keep reset bit low for a couple of cycles
00227       needs_reset_countdown_ = 10;
00228       needs_reset_ = false;
00229     }
00230   }    
00231 
00232   RIQHandCommandEcat *command = (RIQHandCommandEcat *)(buffer);  
00233   RIQActionRequest &action_request(command->action_request_);
00234 
00235   command->zero();
00236 
00237   if (needs_reset_countdown_ > 0)
00238   {
00239     --needs_reset_countdown_;
00240     action_request.initialize_ = 0;
00241   }
00242   else 
00243   {
00244     action_request.initialize_ = 1;
00245   }  
00246 
00247   switch(command_.action)
00248   {
00249   case riq_msgs::RIQHandCommand::OPEN :
00250     action_request.grip_ = RIQActionRequest::GRIP_OPEN;
00251     break;
00252   case riq_msgs::RIQHandCommand::CLOSE :
00253     action_request.grip_ = RIQActionRequest::GRIP_CLOSE;
00254     break;
00255   case riq_msgs::RIQHandCommand::STOP :
00256     action_request.grip_ = RIQActionRequest::GRIP_STOP;
00257     break;
00258   default:
00259     action_request.grip_ = RIQActionRequest::GRIP_STOP;
00260   }
00261 
00262   switch(command_.mode)
00263   {
00264   case riq_msgs::RIQHandCommand::CYLINDRICAL:
00265     action_request.mode_ = RIQActionRequest::CYLINDRICAL_MODE;
00266     break;
00267   case riq_msgs::RIQHandCommand::PINCH:
00268     action_request.mode_ = RIQActionRequest::PINCH_MODE;
00269     break;
00270   case riq_msgs::RIQHandCommand::SPHERIOD:
00271     action_request.mode_ = RIQActionRequest::SPHERIOD_MODE;
00272     break;
00273   case riq_msgs::RIQHandCommand::SCISSORS:
00274     action_request.mode_ = RIQActionRequest::SCISSORS_MODE;
00275     break;
00276   }
00277   
00278   command->force_    = scaleAndSaturate255(command_.force);
00279   command->velocity_ = scaleAndSaturate255(command_.velocity);
00280 
00281   if (halt)
00282   {
00283     action_request.grip_ = RIQActionRequest::GRIP_STOP;
00284   }
00285   halted_ = halt;
00286 
00287   command->safety_shutdown_.timeout_ = RIQSafetyShutdown::SHUTDOWN_TIMEOUT_10240ms;
00288 }
00289 
00290 
00291 bool RIQHand::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00292 {
00293   // Nothing for now
00294   const RIQHandStateEcat *state = (const RIQHandStateEcat *)(this_buffer + command_size_);
00295   const RIQHandStateEcat *prev_state = (const RIQHandStateEcat *)(prev_buffer + command_size_);
00296   const RIQGripperStatus &gripper_status(state->gripper_status_);
00297   const RIQObjectStatus  &object_status(state->object_status_);
00298   
00299   if (++state_cycle_count_ >= STATE_PUBLISH_PERIOD)
00300   {
00301     state_cycle_count_ = 0;
00302     should_publish_ = true;
00303   }
00304 
00305   if (should_publish_ && use_ros_ && publisher_.trylock())
00306   {
00307     should_publish_ = false;
00308 
00309     riq_msgs::RIQHandState     &msg(publisher_.msg_);
00310     riq_msgs::RIQActuatorState &thumb(msg.thumb);
00311     riq_msgs::RIQActuatorState &right(msg.right_finger);
00312     riq_msgs::RIQActuatorState &left(msg.left_finger);
00313     riq_msgs::RIQActuatorState &scissors(msg.scissors);
00314     
00315     msg.operational = 
00316       (gripper_status.initialization_ == 1) &&
00317       (state->fault_status_ == RIQHandStateEcat::NO_FAULT) && 
00318       !halted_;
00319 
00320     // status, mode, and object field constants are defined to be the same as EtherCAT interface values
00321     msg.status = gripper_status.status_;
00322     msg.mode   = gripper_status.mode_;
00323     msg.grip   = gripper_status.grip_;
00324     msg.object = object_status.detected_;
00325   
00326     thumb.object_detected     = object_status.thumb_detected_;
00327     right.object_detected     = object_status.right_finger_detected_;
00328     left.object_detected      = object_status.left_finger_detected_;
00329     scissors.object_detected  = object_status.scissors_detected_;
00330 
00331     thumb.position    = RIQHandStateEcat::convertPosition(state->thumb_position_);
00332     right.position    = RIQHandStateEcat::convertPosition(state->right_finger_position_);
00333     left.position     = RIQHandStateEcat::convertPosition(state->left_finger_position_);
00334     scissors.position = RIQHandStateEcat::convertPosition(state->scissors_position_);
00335 
00336     thumb.current     = RIQHandStateEcat::convertCurrent(state->thumb_current_);
00337     right.current     = RIQHandStateEcat::convertCurrent(state->right_finger_current_);
00338     left.current      = RIQHandStateEcat::convertCurrent(state->left_finger_current_);
00339     scissors.current  = RIQHandStateEcat::convertCurrent(state->scissors_current_);
00340 
00341     publisher_.unlockAndPublish();
00342   }
00343 
00344   bool rv = true;
00345 
00346   if (needs_watchdog_reset_)
00347   {
00348     after_watchdog_reset_countdown_  = 100;
00349   }
00350   else if (after_watchdog_reset_countdown_ > 0)
00351   {
00352     --after_watchdog_reset_countdown_;
00353     if ((prev_state->fault_status_ == RIQHandStateEcat::FAULT_COMMUNICATION_TIMEOUT) && 
00354         (state->fault_status_      == RIQHandStateEcat::NO_FAULT))
00355     {
00356       ROS_INFO("Communication fault cleared at %d", after_watchdog_reset_countdown_);
00357     }
00358   }    
00359   
00360   if (needs_reset_countdown_ > 0)
00361   {
00362     after_reset_countdown_ = 100;
00363   }
00364   else if (after_reset_countdown_ > 0)
00365   {
00366     --after_reset_countdown_;
00367     bool was_major_fault = 
00368       (prev_state->fault_status_ == RIQHandStateEcat::FAULT_MAJOR_SCISSORS_DISPLACEMENT) || 
00369       (prev_state->fault_status_ == RIQHandStateEcat::FAULT_MAJOR_SCISSORS_DISPLACEMENT);
00370     if (was_major_fault && (state->fault_status_ == RIQHandStateEcat::NO_FAULT))
00371     {
00372       ROS_INFO("Major fault cleared at %d", after_reset_countdown_);
00373     }
00374   }
00375 
00376   // When certain faults occur, halt motors and save fault code
00377   if (!halted_  &&  (state->fault_status_ != RIQHandStateEcat::NO_FAULT))
00378   {
00379     switch (state->fault_status_)
00380     {
00381     case RIQHandStateEcat::FAULT_COMMUNICATION_TIMEOUT:
00382       // Ignore this error, for a few cycles after has been cleared
00383       if (after_watchdog_reset_countdown_ == 0)
00384       {
00385         rv = false;
00386         latched_fault_code_ = state->fault_status_;
00387         ROS_ERROR("Watchdog");
00388       }
00389       break;
00390     case RIQHandStateEcat::FAULT_INSUFFICIENT_SUPPLY_VOLTAGE:
00391       rv = false;
00392       latched_fault_code_ = state->fault_status_;
00393       break;
00394     case RIQHandStateEcat::FAULT_MAJOR_SCISSORS_DISPLACEMENT:
00395     case RIQHandStateEcat::FAULT_MAJOR_FINGER_DISPLACEMENT:
00396       // Ignore these errors for a while after resetting
00397       if (after_reset_countdown_ == 0)
00398       {
00399         needs_reset_ = true;
00400         rv = false;
00401         latched_fault_code_ = state->fault_status_;
00402       }
00403       break;
00404     default:
00405       // Do nothing for other fault codes
00406       break;
00407     }
00408   }
00409 
00410   return rv;
00411 }
00412 
00413 
00414 
00415 void RIQHand::disableWatchdog(EtherCAT_SlaveHandler *sh)
00416 {
00417   // The ethercat process data watchdog timeout on set to 100ms after reset.
00418   // This disables watchdog by change timeout to special value of 0.
00419   EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00420   static const unsigned WATCHDOG_TIME_PDI_ADDR = 0x410;
00421   static const unsigned WATCHDOG_TIME_PROCESS_DATA_ADDR = 0x420;  
00422   uint16_t timeout = 0;
00423   if (writeData(&com, sh, WATCHDOG_TIME_PDI_ADDR, &timeout, 2, FIXED_ADDR) != 0)
00424   {
00425     ROS_ERROR("Error disabling PDI watchdog\n");
00426   }
00427   timeout = 0;
00428   if (writeData(&com, sh, WATCHDOG_TIME_PROCESS_DATA_ADDR, &timeout, 2, FIXED_ADDR) != 0)
00429   {
00430     ROS_ERROR("Error disabling process data watchdog\n");
00431   }
00432 }
00433 
00434 
00435 void RIQHand::collectDiagnostics(EthercatCom *com)
00436 {
00437   EthercatDevice::collectDiagnostics(com);
00438   
00439   ECatALStatusAll al_status;
00440   bool al_status_good = al_status.readData(com,sh_);
00441 
00442   { boost::unique_lock<boost::mutex> lock(diagnostics_mutex_);
00443     collect_diagnostics_.al_status_good_ = al_status_good;
00444     collect_diagnostics_.al_status_ = al_status;
00445   }
00446 
00447   // Use diagnostics collection thread to clear watchdog timeout error (when requested by reset_motors)
00448   // When the EtherCAT master does not update slave process data every 100ms, the watchdog times out.
00449   // When this occcurs the device automatically goes into the SAFE-OP state.
00450   // To get device operational again, the AL error is acknowledged, 
00451   // and the device is put back into the OPerational state
00452   if (needs_watchdog_reset_)
00453   {
00454     if (!al_status_good)
00455     {
00456       // Without status data, it is not possible to determine if
00457       // watchdog error does indeed need to cleared.
00458       //ROS_WARN("Cannot reset watchdog, no status data");
00459     }
00460     else if (al_status.status_code_.error_code_ == ECatALStatusCode::SYNC_MANAGER_WATCHDOG)
00461     {
00462       // reset error code and move back into operational state
00463       ECatALControl al_control;
00464       al_control.raw_ = 0;
00465       al_control.ack_error_ = 1;
00466       al_control.state_ = ECatStates::OP;
00467       if (al_control.writeData(com, sh_))
00468       {
00469         // watchdog error has been cleared
00470         //ROS_WARN("Watchdog reset");
00471         needs_watchdog_reset_ = false;        
00472       }
00473       else
00474       {
00475         //ROS_WARN("Watchdog reset failed");
00476       }
00477     }
00478     else
00479     {
00480       // No watchdog error to clear
00481       //ROS_WARN("Watchdog reset uneeded");
00482       needs_watchdog_reset_ = false;      
00483     }
00484   }
00485 }
00486 
00487 
00488 void RIQHand::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00489 {
00490   const RIQHandStateEcat *status = (const RIQHandStateEcat *)(buffer + command_size_);
00491   const RIQHandCommandEcat *command = (const RIQHandCommandEcat *)(buffer);  
00492 
00493   // Get copy of diagnostics data collected with another thread
00494   if (diagnostics_mutex_.try_lock())
00495   { 
00496     publish_diagnostics_ = collect_diagnostics_;
00497     diagnostics_mutex_.unlock();
00498   }
00499 
00500   stringstream str;
00501   str << "EtherCAT Device #" << setw(2) << setfill('0') << sh_->get_ring_position() << " (RIQHand)";
00502   d.name = str.str();
00503   d.summary(d.OK, "OK");
00504   char serial[32];
00505   snprintf(serial, sizeof(serial), "%05d-%05d", sh_->get_product_code(), sh_->get_serial());
00506   d.hardware_id = serial;
00507 
00508   d.clear();
00509   d.addf("Product code", "RIQ Hand (%d), Revision %d",
00510          sh_->get_product_code(), sh_->get_revision());
00511   d.addf("Serial Number", "%s", serial);
00512 
00513   const RIQGripperStatus &gripper_status(status->gripper_status_);
00514   d.addf("Initialization", "%s (%d)", (gripper_status.initialization_ ? "Initialized" : "Reset"),
00515          int(gripper_status.initialization_) );
00516   d.addf("Gripper Mode", "%s (%d)", gripper_status.modeString(), gripper_status.mode_);
00517   d.addf("Gripper Status", "%s (%d)", gripper_status.statusString(), gripper_status.status_);
00518   bool in_scissors_mode = gripper_status.inScissorsMode();
00519 
00520   const RIQObjectStatus &object_status(status->object_status_);
00521   d.addf("Object Detected", "%s (%d)", 
00522          object_status.detectedString(in_scissors_mode), 
00523          object_status.detected_);
00524 
00525   d.addf("Fault", "%s (%d)", status->faultString(), int(status->fault_status_));
00526 
00527   if (latched_fault_code_ != 0)
00528   {
00529     // If fault was latched, it is error that requires motor reset
00530     // Examples : communication timeout, undervoltage
00531     d.mergeSummary(d.ERROR, RIQHandStateEcat::faultString(latched_fault_code_));
00532   }
00533   else if (status->fault_status_ != status->NO_FAULT)
00534   {
00535     // If fault was not latched
00536     d.mergeSummary(d.WARN, status->faultString());
00537   }
00538 
00539   d.addf("Thumb position", "%f (%u)", 
00540          status->convertPosition(status->thumb_position_), 
00541          unsigned(status->thumb_position_));
00542   d.addf("Thumb position", "%fmA (%u)", 
00543          1000.0 * status->convertCurrent(status->thumb_current_),
00544          unsigned(status->thumb_current_));
00545 
00546   d.addf("Right finger position", "%f (%u)", 
00547          status->convertPosition(status->right_finger_position_), 
00548          unsigned(status->right_finger_position_));
00549   d.addf("Right finger current", "%fmA (%u)", 
00550          1000.0 * status->convertCurrent(status->right_finger_current_),
00551          unsigned(status->right_finger_current_));
00552 
00553   d.addf("Left finger position", "%f (%u)", 
00554          status->convertPosition(status->left_finger_position_), 
00555          unsigned(status->left_finger_position_));
00556   d.addf("Left finger current", "%fmA (%u)", 
00557          1000.0 * status->convertCurrent(status->left_finger_current_),
00558          unsigned(status->left_finger_current_));
00559 
00560   d.addf("Scissors position", "%f (%u)", 
00561          status->convertPosition(status->scissors_position_), 
00562          unsigned(status->scissors_position_));
00563   d.addf("Scissors current", "%fmA (%u)", 
00564          1000.0 * status->convertCurrent(status->scissors_current_),
00565          unsigned(status->scissors_current_));
00566 
00567 
00568   const RIQActionRequest &action_request(command->action_request_);
00569 
00570   d.addf("Initialization request", "%s (%d)", 
00571          (action_request.initialize_ ? "Initialize" : "Reset"),
00572          unsigned(action_request.initialize_));
00573   d.addf("Mode request", "%s (%d)", action_request.modeString(),
00574          unsigned(action_request.mode_));
00575   d.addf("Grip request", "%s (%d)", action_request.gripString(),
00576          unsigned(action_request.grip_));
00577   d.addf("Force", "%f (%d)", (double(command->force_)/255.0), 
00578          unsigned(command->force_));
00579   d.addf("Velocity", "%f (%d)", (double(command->velocity_)/255.0),
00580          unsigned(command->velocity_));
00581 
00582     
00583   if (publish_diagnostics_.al_status_good_)
00584   {
00585     const ECatALStatusAll &al_status(publish_diagnostics_.al_status_);
00586     d.addf("AL Status", "%s : %s", 
00587            (al_status.status_.error_?"Error":"Good"),
00588            al_status.status_code_.errorCodeString());
00589     d.addf("AL State", "%s (%d)", 
00590            al_status.status_.stateString(),
00591            al_status.status_.state_);
00592   }
00593   else 
00594   {
00595     d.addf("AL Status", "INVALID"); 
00596   }  
00597 
00598   // TODO : RIQHand RX error counters do not seem to work. 
00599   //  Get EtherCAT support document to figure out what slave device registers are supported
00600   EthercatDevice::ethercatDiagnostics(d, 1);   // RIQHand has only a single port
00601 }
00602 
00603 
00604 }; //namespace riq_hand_ethercat_hardware
riq_hand.cpp
