sr_motor_robot_lib.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/sr_motor_robot_lib.hpp"
00029 #include <string>
00030 #include <boost/foreach.hpp>
00031 
00032 #include <sys/time.h>
00033 
00034 #include <ros/ros.h>
00035 
00036 #include <pr2_mechanism_msgs/ListControllers.h>
00037 
00038 #define SERIOUS_ERROR_FLAGS PALM_0200_EDC_SERIOUS_ERROR_FLAGS
00039 #define error_flag_names palm_0200_edc_error_flag_names
00040 
00041 namespace shadow_robot
00042 {
00043 
00044   template <class StatusType, class CommandType>
00045   SrMotorRobotLib<StatusType, CommandType>::SrMotorRobotLib(pr2_hardware_interface::HardwareInterface *hw)
00046     : SrRobotLib<StatusType, CommandType>(hw),
00047       motor_current_state(operation_mode::device_update_state::INITIALIZATION),
00048       config_index(MOTOR_CONFIG_FIRST_VALUE),
00049       control_type_changed_flag_(false)
00050   {
00051     lock_command_sending_ = boost::shared_ptr<boost::mutex>(new boost::mutex());
00052 
00053     //reading the parameters to check for a specified default control type
00054     // using FORCE control if no parameters are set
00055     control_type_.control_type = sr_robot_msgs::ControlType::FORCE;
00056     std::string default_control_mode;
00057     this->nh_tilde.template param<std::string>("default_control_mode", default_control_mode, "FORCE");
00058     if( default_control_mode.compare("PWM") == 0 )
00059     {
00060       ROS_INFO("Using PWM control.");
00061       control_type_.control_type = sr_robot_msgs::ControlType::PWM;
00062     }
00063     else
00064     {
00065       ROS_INFO("Using TORQUE control.");
00066     }
00067 
00068     //initialising the change control type service
00069     change_control_type_ = this->nh_tilde.advertiseService( "change_control_type", &SrMotorRobotLib::change_control_type_callback_, this);
00070 
00072     motor_system_control_server_ = this->nh_tilde.advertiseService( "change_motor_system_controls", &SrMotorRobotLib::motor_system_controls_callback_, this);
00073 
00074 #ifdef DEBUG_PUBLISHER
00075     this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const);
00076     for( int i = 0; i < this->nb_debug_publishers_const; ++i )
00077     {
00078       std::stringstream ss;
00079       ss << "srh/debug_" << i;
00080       this->debug_publishers.push_back(this->node_handle.template advertise<std_msgs::Int16>(ss.str().c_str(),100));
00081     }
00082 #endif
00083 
00084   }
00085 
00086   template <class StatusType, class CommandType>
00087   void SrMotorRobotLib<StatusType, CommandType>::update(StatusType* status_data)
00088   {
00089     //read the PIC idle time
00090     this->main_pic_idle_time = status_data->idle_time_us;
00091     if (status_data->idle_time_us < this->main_pic_idle_time_min)
00092       this->main_pic_idle_time_min = status_data->idle_time_us;
00093 
00094     //get the current timestamp
00095     struct timeval tv;
00096     double timestamp = 0.0;
00097     if (gettimeofday(&tv, NULL))
00098     {
00099       ROS_WARN("SrMotorRobotLib: Failed to get system time, timestamp in state will be zero");
00100     }
00101     else
00102     {
00103       timestamp = double(tv.tv_sec) + double(tv.tv_usec) / 1.0e+6;
00104     }
00105 
00106     //First we read the joints informations
00107     boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = this->joints_vector.begin();
00108     for (; joint_tmp != this->joints_vector.end(); ++joint_tmp)
00109     {
00110       sr_actuator::SrActuatorState* actuator_state = this->get_joint_actuator_state(joint_tmp);
00111 
00112       boost::shared_ptr<shadow_joints::MotorWrapper> motor_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint_tmp->actuator_wrapper);
00113 
00114       motor_index_full = motor_wrapper->motor_id;
00115       actuator_state->is_enabled_ = 1;
00116       actuator_state->device_id_ = motor_index_full;
00117       actuator_state->halted_ = false;
00118 
00119       //Fill in the tactiles.
00120       if( this->tactiles != NULL )
00121         actuator_state->tactiles_ = this->tactiles->get_tactile_data();
00122 
00123       this->process_position_sensor_data(joint_tmp, status_data, timestamp);
00124 
00125       sr_actuator::SrActuator* motor_actuator = static_cast<sr_actuator::SrActuator*>(joint_tmp->actuator_wrapper->actuator);
00126       //filter the effort
00127       std::pair<double, double> effort_and_effort_d = joint_tmp->effort_filter.compute(
00128           motor_actuator->state_.force_unfiltered_, timestamp);
00129       motor_actuator->state_.last_measured_effort_ = effort_and_effort_d.first;
00130 
00131       //if no motor is associated to this joint, then continue
00132       if ((motor_index_full == -1))
00133         continue;
00134 
00135       //get the remaining information.
00136       bool read_motor_info = false;
00137 
00138       if (status_data->which_motors == 0)
00139       {
00140         //We sampled the even motor numbers
00141         if (motor_index_full % 2 == 0)
00142           read_motor_info = true;
00143       }
00144       else
00145       {
00146         //we sampled the odd motor numbers
00147         if (motor_index_full % 2 == 1)
00148           read_motor_info = true;
00149       }
00150 
00151       //the position of the motor in the message
00152       // is different from the motor index:
00153       // the motor indexes range from 0 to 19
00154       // while the message contains information
00155       // for only 10 motors.
00156       index_motor_in_msg = motor_index_full / 2;
00157 
00158       //setting the position of the motor in the message,
00159       // we'll print that in the diagnostics.
00160       motor_wrapper->msg_motor_id = index_motor_in_msg;
00161 
00162       //ok now we read the info and add it to the actuator state
00163       if (read_motor_info)
00164         read_additional_data(joint_tmp, status_data);
00165     } //end for joint
00166 
00167     //then we read the tactile sensors information
00168     this->update_tactile_info(status_data);
00169   } //end update()
00170 
00171   template <class StatusType, class CommandType>
00172   void SrMotorRobotLib<StatusType, CommandType>::build_command(CommandType* command)
00173   {
00174     //Mutual exclusion with the change_control_type service. We have to wait until the control_type_ variable has been set.
00175     boost::mutex::scoped_lock l(*lock_command_sending_);
00176 
00177     if(control_type_changed_flag_)
00178     {
00179       if(!change_control_parameters(control_type_.control_type))
00180       {
00181         ROS_FATAL("Changing control parameters failed. Stopping real time loop to protect the robot.");
00182         exit(EXIT_FAILURE);
00183       }
00184 
00185       control_type_changed_flag_ = false;
00186     }
00187 
00188     if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
00189     {
00190       motor_current_state = motor_updater_->build_init_command(command);
00191     }
00192     else
00193     {
00194       //build the motor command
00195       motor_current_state = motor_updater_->build_command(command);
00196     }
00197 
00198     //Build the tactile sensors command
00199     this->build_tactile_command(command);
00200 
00202     // Now we chose the command to send to the motor
00203     // by default we send a torque demand (we're running
00204     // the force control on the motors), but if we have a waiting
00205     // configuration, a reset command, or a motor system control
00206     // request then we send the configuration
00207     // or the reset.
00208     if ( reconfig_queue.empty() && reset_motors_queue.empty()
00209          && motor_system_control_flags_.empty() )
00210     {
00211       //no config to send
00212       switch( control_type_.control_type )
00213       {
00214       case sr_robot_msgs::ControlType::FORCE:
00215         command->to_motor_data_type = MOTOR_DEMAND_TORQUE;
00216         break;
00217       case sr_robot_msgs::ControlType::PWM:
00218         command->to_motor_data_type = MOTOR_DEMAND_PWM;
00219         break;
00220 
00221       default:
00222         command->to_motor_data_type = MOTOR_DEMAND_TORQUE;
00223         break;
00224       }
00225 
00226       //loop on all the joints and update their motor: we're sending commands to all the motors.
00227       boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = this->joints_vector.begin();
00228       for (; joint_tmp != this->joints_vector.end(); ++joint_tmp)
00229       {
00230         boost::shared_ptr<shadow_joints::MotorWrapper> motor_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint_tmp->actuator_wrapper);
00231 
00232         if (joint_tmp->has_actuator)
00233         {
00234           if( !this->nullify_demand_ )
00235           {
00236             //We send the computed demand
00237             command->motor_data[motor_wrapper->motor_id] = motor_wrapper->actuator->command_.effort_;
00238           }
00239           else
00240           {
00241             //We want to send a demand of 0
00242             command->motor_data[motor_wrapper->motor_id] = 0;
00243           }
00244 /*
00245 #ifdef DEBUG_PUBLISHER
00246           //publish the debug values for the given motors.
00247           // NB: debug_motor_indexes_and_data is smaller
00248           //     than debug_publishers.
00249           int publisher_index = 0;
00250           boost::shared_ptr<std::pair<int,int> > debug_pair;
00251           if( this->debug_mutex.try_lock() )
00252           {
00253             BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data)
00254             {
00255               if( debug_pair != NULL )
00256               {
00257                 shadow_joints::MotorWrapper* actuator_wrapper = static_cast<shadow_joints::MotorWrapper*>(joint_tmp->actuator_wrapper.get());
00258                 //check if we want to publish some data for the current motor
00259                 if( debug_pair->first == actuator_wrapper->motor_id )
00260                 {
00261                   //check if it's the correct data
00262                   if( debug_pair->second == -1 )
00263                   {
00264                     this->msg_debug.data = joint_tmp->actuator_wrapper->actuator->command_.effort_;
00265                     this->debug_publishers[publisher_index].publish(this->msg_debug);
00266                   }
00267                 }
00268               }
00269               publisher_index ++;
00270             }
00271 
00272             this->debug_mutex.unlock();
00273           } //end try_lock
00274 #endif
00275 */
00276           joint_tmp->actuator_wrapper->actuator->state_.last_commanded_effort_ = joint_tmp->actuator_wrapper->actuator->command_.effort_;
00277         } //end if has_actuator
00278       } // end for each joint
00279     } //endif reconfig_queue.empty()
00280     else
00281     {
00282       if ( !motor_system_control_flags_.empty() )
00283       {
00284         //treat the first waiting system control and remove it from the queue
00285         std::vector<sr_robot_msgs::MotorSystemControls> system_controls_to_send;
00286         system_controls_to_send = motor_system_control_flags_.front();
00287         motor_system_control_flags_.pop();
00288 
00289         //set the correct type of command to send to the hand.
00290         command->to_motor_data_type = MOTOR_SYSTEM_CONTROLS;
00291 
00292         std::vector<sr_robot_msgs::MotorSystemControls>::iterator it;
00293         for( it = system_controls_to_send.begin(); it != system_controls_to_send.end(); ++it)
00294         {
00295           short combined_flags = 0;
00296           if( it->enable_backlash_compensation )
00297             combined_flags |= MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE;
00298           else
00299             combined_flags |= MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE;
00300 
00301           if( it->increase_sgl_tracking )
00302             combined_flags |= MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC;
00303           if( it->decrease_sgl_tracking )
00304             combined_flags |= MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC;
00305 
00306           if( it->increase_sgr_tracking )
00307             combined_flags |= MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC;
00308           if( it->decrease_sgr_tracking )
00309             combined_flags |= MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC;
00310 
00311           if( it->initiate_jiggling )
00312             combined_flags |= MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING;
00313 
00314           if( it->write_config_to_eeprom )
00315             combined_flags |= MOTOR_SYSTEM_CONTROL_EEPROM_WRITE;
00316 
00317           command->motor_data[ it->motor_id ] = combined_flags;
00318         }
00319       } //end if motor_system_control_flags_.empty
00320       else
00321       {
00322         if (!reset_motors_queue.empty())
00323         {
00324           //reset the CAN messages counters for the motor we're going to reset.
00325           short motor_id = reset_motors_queue.front();
00326           boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = this->joints_vector.begin();
00327           for (; joint_tmp != this->joints_vector.end(); ++joint_tmp)
00328           {
00329             boost::shared_ptr<shadow_joints::MotorWrapper> motor_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint_tmp->actuator_wrapper);
00330             sr_actuator::SrActuatorState* actuator_state = this->get_joint_actuator_state(joint_tmp);
00331 
00332             if( motor_wrapper->motor_id == motor_id )
00333             {
00334               actuator_state->can_msgs_transmitted_ = 0;
00335               actuator_state->can_msgs_received_ = 0;
00336             }
00337           }
00338 
00339           //we have some reset command waiting.
00340           // We'll send all of them
00341           command->to_motor_data_type = MOTOR_SYSTEM_RESET;
00342 
00343           while (!reset_motors_queue.empty())
00344           {
00345             motor_id = reset_motors_queue.front();
00346             reset_motors_queue.pop();
00347 
00348             // we send the MOTOR_RESET_SYSTEM_KEY
00349             // and the motor id (on the bus)
00350             crc_unions::union16 to_send;
00351             to_send.byte[1] = MOTOR_SYSTEM_RESET_KEY >> 8;
00352             if (motor_id > 9)
00353               to_send.byte[0] = motor_id - 10;
00354             else
00355               to_send.byte[0] = motor_id;
00356 
00357             command->motor_data[motor_id] = to_send.word;
00358           }
00359         } // end if reset queue not empty
00360         else
00361         {
00362           if (!reconfig_queue.empty())
00363           {
00364             //we have a waiting config:
00365             // we need to send all the config, finishing by the
00366             // CRC. We'll remove the config from the queue only
00367             // when the whole config has been sent
00368 
00369             // the motor data type correspond to the index
00370             // in the config array.
00371             command->to_motor_data_type = static_cast<TO_MOTOR_DATA_TYPE>(config_index);
00372 
00373             //convert the motor index to the index of the motor in the message
00374             int motor_index = reconfig_queue.front().first;
00375 
00376             //set the data we want to send to the given motor
00377             command->motor_data[motor_index] = reconfig_queue.front().second[config_index].word;
00378 
00379             //We're now sending the CRC. We need to send the correct CRC to
00380             // the motor we updated, and CRC=0 to all the other motors in its
00381             // group (odd/even) to tell them to ignore the new
00382             // configuration.
00383             // Once the config has been transmitted, pop the element
00384             // and reset the config_index to the beginning of the
00385             // config values
00386             if (config_index == static_cast<int>(MOTOR_CONFIG_CRC))
00387             {
00388               //loop on all the motors and send a CRC of 0
00389               // except for the motor we're reconfiguring
00390               for (int i = 0; i < NUM_MOTORS; ++i)
00391               {
00392                 if (i != motor_index)
00393                   command->motor_data[i] = 0;
00394               }
00395 
00396               //reset the config_index and remove the configuration
00397               // we just sent from the configurations queue
00398               reconfig_queue.pop();
00399               config_index = MOTOR_CONFIG_FIRST_VALUE;
00400             }
00401             else
00402               ++config_index;
00403           } //end if reconfig queue not empty
00404         } // end else reset_queue.empty
00405       } // end else motor_system_control_flags_.empty
00406     } //endelse reconfig_queue.empty() && reset_queue.empty()
00407   }
00408 
00409   template <class StatusType, class CommandType>
00410   void SrMotorRobotLib<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00411                                    diagnostic_updater::DiagnosticStatusWrapper &d)
00412   {
00413     boost::ptr_vector<shadow_joints::Joint>::iterator joint = this->joints_vector.begin();
00414     for (; joint != this->joints_vector.end(); ++joint)
00415     {
00416       std::stringstream name;
00417       name.str("");
00418       name << "SRDMotor " << joint->joint_name;
00419       d.name = name.str();
00420 
00421       boost::shared_ptr<shadow_joints::MotorWrapper> actuator_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint->actuator_wrapper);
00422 
00423       if (joint->has_actuator)
00424       {
00425         const sr_actuator::SrActuator* sr_actuator = static_cast<sr_actuator::SrActuator*>(actuator_wrapper->actuator);
00426         const sr_actuator::SrMotorActuatorState* state = &(sr_actuator->state_);
00427 
00428         if (actuator_wrapper->actuator_ok)
00429         {
00430           if (actuator_wrapper->bad_data)
00431           {
00432             d.summary(d.WARN, "WARNING, bad CAN data received");
00433 
00434             d.clear();
00435             d.addf("Motor ID", "%d", actuator_wrapper->motor_id);
00436           }
00437           else //the data is good
00438           {
00439             d.summary(d.OK, "OK");
00440 
00441             d.clear();
00442             d.addf("Motor ID", "%d", actuator_wrapper->motor_id);
00443             d.addf("Motor ID in message", "%d", actuator_wrapper->msg_motor_id);
00444             d.addf("Serial Number", "%d", state->serial_number );
00445             d.addf("Assembly date", "%d / %d / %d", state->assembly_data_day, state->assembly_data_month, state->assembly_data_year );
00446 
00447             d.addf("Strain Gauge Left", "%d", state->strain_gauge_left_);
00448             d.addf("Strain Gauge Right", "%d", state->strain_gauge_right_);
00449 
00450             //if some flags are set
00451             std::stringstream ss;
00452             if (state->flags_.size() > 0)
00453             {
00454               int flags_seriousness = d.OK;
00455               std::pair<std::string, bool> flag;
00456               BOOST_FOREACH(flag, state->flags_)
00457               {
00458                 //Serious error flag
00459                 if (flag.second)
00460                   flags_seriousness = d.ERROR;
00461 
00462                 if (flags_seriousness != d.ERROR)
00463                   flags_seriousness = d.WARN;
00464                 ss << flag.first << " | ";
00465               }
00466               d.summary(flags_seriousness, ss.str().c_str());
00467             }
00468             else
00469               ss << " None";
00470             d.addf("Motor Flags", "%s", ss.str().c_str());
00471 
00472             d.addf("Measured PWM", "%d", state->pwm_);
00473             d.addf("Measured Current", "%f", state->last_measured_current_);
00474             d.addf("Measured Voltage", "%f", state->motor_voltage_);
00475             d.addf("Measured Effort", "%f", state->last_measured_effort_);
00476             d.addf("Temperature", "%f", state->temperature_);
00477 
00478             d.addf("Unfiltered position", "%f", state->position_unfiltered_);
00479             d.addf("Unfiltered force", "%f", state->force_unfiltered_);
00480 
00481             d.addf("Gear Ratio", "%d", state->motor_gear_ratio);
00482 
00483             d.addf("Number of CAN messages received", "%lld", state->can_msgs_received_);
00484             d.addf("Number of CAN messages transmitted", "%lld", state->can_msgs_transmitted_);
00485 
00486             d.addf("Force control Pterm", "%d", state->force_control_pterm);
00487             d.addf("Force control Iterm", "%d", state->force_control_iterm);
00488             d.addf("Force control Dterm", "%d", state->force_control_dterm);
00489 
00490             d.addf("Force control F", "%d", state->force_control_f_);
00491             d.addf("Force control P", "%d", state->force_control_p_);
00492             d.addf("Force control I", "%d", state->force_control_i_);
00493             d.addf("Force control D", "%d", state->force_control_d_);
00494             d.addf("Force control Imax", "%d", state->force_control_imax_);
00495             d.addf("Force control Deadband", "%d", state->force_control_deadband_);
00496             d.addf("Force control Frequency", "%d", state->force_control_frequency_);
00497 
00498             if (state->force_control_sign_ == 0)
00499               d.addf("Force control Sign", "+");
00500             else
00501               d.addf("Force control Sign", "-");
00502 
00503             d.addf("Last Commanded Effort", "%f", state->last_commanded_effort_);
00504 
00505             d.addf("Encoder Position", "%f", state->position_);
00506 
00507             if (state->firmware_modified_)
00508               d.addf("Firmware svn revision (server / pic / modified)", "%d / %d / True",
00509                      state->server_firmware_svn_revision_, state->pic_firmware_svn_revision_);
00510             else
00511               d.addf("Firmware svn revision (server / pic / modified)", "%d / %d / False",
00512                      state->server_firmware_svn_revision_, state->pic_firmware_svn_revision_);
00513           }
00514         }
00515         else
00516         {
00517           d.summary(d.ERROR, "Motor error");
00518           d.clear();
00519           d.addf("Motor ID", "%d", actuator_wrapper->motor_id);
00520         }
00521       }
00522       else
00523       {
00524         d.summary(d.OK, "No motor associated to this joint");
00525         d.clear();
00526       }
00527       vec.push_back(d);
00528 
00529     } //end for each joints
00530 
00531   }
00532 
00533 
00534   template <class StatusType, class CommandType>
00535   void SrMotorRobotLib<StatusType, CommandType>::read_additional_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data)
00536   {
00537     //check the masks to see if the CAN messages arrived to the motors
00538     //the flag should be set to 1 for each motor
00539     joint_tmp->actuator_wrapper->actuator_ok = sr_math_utils::is_bit_mask_index_true(status_data->which_motor_data_arrived,
00540                                                                        motor_index_full);
00541 
00542     //check the masks to see if a bad CAN message arrived
00543     //the flag should be 0
00544     joint_tmp->actuator_wrapper->bad_data = sr_math_utils::is_bit_mask_index_true(status_data->which_motor_data_had_errors,
00545                                                                        index_motor_in_msg);
00546 
00547     crc_unions::union16 tmp_value;
00548 
00549     if (joint_tmp->actuator_wrapper->actuator_ok && !(joint_tmp->actuator_wrapper->bad_data))
00550     {
00551       sr_actuator::SrActuator* actuator = static_cast<sr_actuator::SrActuator*>(joint_tmp->actuator_wrapper->actuator);
00552       shadow_joints::MotorWrapper* actuator_wrapper = static_cast<shadow_joints::MotorWrapper*>(joint_tmp->actuator_wrapper.get());
00553 /*
00554 #ifdef DEBUG_PUBLISHER
00555       int publisher_index = 0;
00556       //publish the debug values for the given motors.
00557       // NB: debug_motor_indexes_and_data is smaller
00558       //     than debug_publishers.
00559       boost::shared_ptr<std::pair<int,int> > debug_pair;
00560 
00561       if( this->debug_mutex.try_lock() )
00562       {
00563         BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data)
00564         {
00565           if( debug_pair != NULL )
00566           {
00567             shadow_joints::MotorWrapper* actuator_wrapper = static_cast<shadow_joints::MotorWrapper*>(joint_tmp->actuator_wrapper.get());
00568             
00569             //check if we want to publish some data for the current motor
00570             if( debug_pair->first == actuator_wrapper->motor_id )
00571             {
00572               //if < 0, then we're not asking for a FROM_MOTOR_DATA_TYPE
00573               if( debug_pair->second > 0 )
00574               {
00575                 //check if it's the correct data
00576                 if( debug_pair->second == status_data->motor_data_type )
00577                 {
00578                   this->msg_debug.data = status_data->motor_data_packet[index_motor_in_msg].misc;
00579                   this->debug_publishers[publisher_index].publish(this->msg_debug);
00580                 }
00581               }
00582             }
00583           }
00584           publisher_index ++;
00585         }
00586 
00587         this->debug_mutex.unlock();
00588       } //end try_lock
00589 #endif
00590 */
00591       //we received the data and it was correct
00592       bool read_torque = true;
00593       switch (status_data->motor_data_type)
00594       {
00595       case MOTOR_DATA_SGL:
00596         actuator->state_.strain_gauge_left_ =
00597           static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].misc);
00598 
00599 #ifdef DEBUG_PUBLISHER
00600         if( actuator_wrapper->motor_id == 19 )
00601         {
00602           //ROS_ERROR_STREAM("SGL " <<actuator->state_.strain_gauge_left_);
00603           this->msg_debug.data = actuator->state_.strain_gauge_left_;
00604           this->debug_publishers[0].publish(this->msg_debug);
00605         }
00606 #endif
00607         break;
00608       case MOTOR_DATA_SGR:
00609         actuator->state_.strain_gauge_right_ =
00610           static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].misc);
00611 
00612 #ifdef DEBUG_PUBLISHER
00613         if( actuator_wrapper->motor_id == 19 )
00614         {
00615           //ROS_ERROR_STREAM("SGR " <<actuator->state_.strain_gauge_right_);
00616           this->msg_debug.data = actuator->state_.strain_gauge_right_;
00617           this->debug_publishers[1].publish(this->msg_debug);
00618         }
00619 #endif
00620         break;
00621       case MOTOR_DATA_PWM:
00622         actuator->state_.pwm_ =
00623           static_cast<int>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].misc));
00624 
00625 #ifdef DEBUG_PUBLISHER
00626         if( actuator_wrapper->motor_id == 19 )
00627         {
00628           //ROS_ERROR_STREAM("SGR " <<actuator->state_.strain_gauge_right_);
00629           this->msg_debug.data = actuator->state_.pwm_;
00630           this->debug_publishers[2].publish(this->msg_debug);
00631         }
00632 #endif
00633         break;
00634       case MOTOR_DATA_FLAGS:
00635         actuator->state_.flags_ = humanize_flags(status_data->motor_data_packet[index_motor_in_msg].misc);
00636         break;
00637       case MOTOR_DATA_CURRENT:
00638         //we're receiving the current in milli amps
00639         actuator->state_.last_measured_current_ =
00640           static_cast<double>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc))
00641           / 1000.0;
00642 
00643 #ifdef DEBUG_PUBLISHER
00644         if( actuator_wrapper->motor_id == 19 )
00645         {
00646           //ROS_ERROR_STREAM("Current " <<actuator->state_.last_measured_current_);
00647           this->msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
00648           this->debug_publishers[3].publish(this->msg_debug);
00649         }
00650 #endif
00651         break;
00652       case MOTOR_DATA_VOLTAGE:
00653         actuator->state_.motor_voltage_ =
00654           static_cast<double>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) / 256.0;
00655 
00656 #ifdef DEBUG_PUBLISHER
00657         if( actuator_wrapper->motor_id == 19 )
00658         {
00659           //ROS_ERROR_STREAM("Voltage " <<actuator->state_.motor_voltage_);
00660           this->msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
00661           this->debug_publishers[4].publish(this->msg_debug);
00662         }
00663 #endif
00664         break;
00665       case MOTOR_DATA_TEMPERATURE:
00666         actuator->state_.temperature_ =
00667           static_cast<double>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) / 256.0;
00668         break;
00669       case MOTOR_DATA_CAN_NUM_RECEIVED:
00670         // those are 16 bits values and will overflow -> we compute the real value.
00671         // This needs to be updated faster than the overflowing period (which should be roughly every 30s)
00672         actuator->state_.can_msgs_received_ = sr_math_utils::counter_with_overflow(
00673           actuator->state_.can_msgs_received_,
00674           static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00675         break;
00676       case MOTOR_DATA_CAN_NUM_TRANSMITTED:
00677         // those are 16 bits values and will overflow -> we compute the real value.
00678         // This needs to be updated faster than the overflowing period (which should be roughly every 30s)
00679         actuator->state_.can_msgs_transmitted_ = sr_math_utils::counter_with_overflow(
00680           actuator->state_.can_msgs_transmitted_,
00681           static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00682         break;
00683 
00684       case MOTOR_DATA_SLOW_MISC:
00685         //We received a slow data:
00686         // the slow data type is contained in .torque, while
00687         // the actual data is in .misc.
00688         // so we won't read torque information from .torque
00689         read_torque = false;
00690 
00691         switch (static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].torque))
00692         {
00693         case MOTOR_SLOW_DATA_SVN_REVISION:
00694           actuator->state_.pic_firmware_svn_revision_ =
00695             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00696           break;
00697         case MOTOR_SLOW_DATA_SVN_SERVER_REVISION:
00698           actuator->state_.server_firmware_svn_revision_ =
00699             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00700           break;
00701         case MOTOR_SLOW_DATA_SVN_MODIFIED:
00702           actuator->state_.firmware_modified_ =
00703             static_cast<bool>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00704           break;
00705         case MOTOR_SLOW_DATA_SERIAL_NUMBER_LOW:
00706           actuator->state_.set_serial_number_low (
00707             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) );
00708           break;
00709         case MOTOR_SLOW_DATA_SERIAL_NUMBER_HIGH:
00710           actuator->state_.set_serial_number_high (
00711             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)) );
00712           break;
00713         case MOTOR_SLOW_DATA_GEAR_RATIO:
00714           actuator->state_.motor_gear_ratio =
00715             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00716           break;
00717         case MOTOR_SLOW_DATA_ASSEMBLY_DATE_YYYY:
00718           actuator->state_.assembly_data_year =
00719             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00720           break;
00721         case MOTOR_SLOW_DATA_ASSEMBLY_DATE_MMDD:
00722           actuator->state_.assembly_data_month =
00723             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)
00724                                       >> 8);
00725           actuator->state_.assembly_data_day =
00726             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc)
00727                                       && 0x00FF);
00728           break;
00729         case MOTOR_SLOW_DATA_CONTROLLER_F:
00730           actuator->state_.force_control_f_ =
00731             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00732           break;
00733         case MOTOR_SLOW_DATA_CONTROLLER_P:
00734           actuator->state_.force_control_p_ =
00735             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00736           break;
00737         case MOTOR_SLOW_DATA_CONTROLLER_I:
00738           actuator->state_.force_control_i_ =
00739             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00740           break;
00741         case MOTOR_SLOW_DATA_CONTROLLER_D:
00742           actuator->state_.force_control_d_ =
00743             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00744           break;
00745         case MOTOR_SLOW_DATA_CONTROLLER_IMAX:
00746           actuator->state_.force_control_imax_ =
00747             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00748           break;
00749         case MOTOR_SLOW_DATA_CONTROLLER_DEADSIGN:
00750           tmp_value.word = status_data->motor_data_packet[index_motor_in_msg].misc;
00751           actuator->state_.force_control_deadband_ = static_cast<int>(tmp_value.byte[0]);
00752           actuator->state_.force_control_sign_ = static_cast<int>(tmp_value.byte[1]);
00753           break;
00754         case MOTOR_SLOW_DATA_CONTROLLER_FREQUENCY:
00755           actuator->state_.force_control_frequency_ =
00756             static_cast<unsigned int>(static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc));
00757           break;
00758 
00759         default:
00760           break;
00761         }
00762         break;
00763 
00764       case MOTOR_DATA_CAN_ERROR_COUNTERS:
00765         actuator->state_.can_error_counters =
00766           static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
00767         break;
00768       case MOTOR_DATA_PTERM:
00769         actuator->state_.force_control_pterm =
00770           static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
00771         break;
00772       case MOTOR_DATA_ITERM:
00773         actuator->state_.force_control_iterm =
00774           static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
00775         break;
00776       case MOTOR_DATA_DTERM:
00777         actuator->state_.force_control_dterm =
00778           static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
00779         break;
00780 
00781       default:
00782         break;
00783       }
00784 
00785       if (read_torque)
00786       {
00787         actuator->state_.force_unfiltered_ =
00788           static_cast<double>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque));
00789 
00790 #ifdef DEBUG_PUBLISHER
00791         if( actuator_wrapper->motor_id == 19 )
00792         {
00793           this->msg_debug.data = static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque);
00794           this->debug_publishers[5].publish(this->msg_debug);
00795         }
00796 #endif
00797       }
00798 
00799       //Check the message to see if everything has already been received
00800       if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
00801       {
00802         if (motor_data_checker->check_message(
00803               joint_tmp, status_data->motor_data_type,
00804               static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].torque)))
00805         {
00806           motor_updater_->update_state = operation_mode::device_update_state::OPERATION;
00807           motor_current_state = operation_mode::device_update_state::OPERATION;
00808 
00809           ROS_INFO("All motors were initialized.");
00810         }
00811       }
00812     }
00813   }
00814 
00815   template <class StatusType, class CommandType>
00816   std::vector<std::pair<std::string, bool> > SrMotorRobotLib<StatusType, CommandType>::humanize_flags(int flag)
00817   {
00818     std::vector<std::pair<std::string, bool> > flags;
00819 
00820     //16 is the number of flags
00821     for (unsigned int i = 0; i < 16; ++i)
00822     {
00823       std::pair<std::string, bool> new_flag;
00824       //if the flag is set add the name
00825       if (sr_math_utils::is_bit_mask_index_true(flag, i))
00826       {
00827         if (sr_math_utils::is_bit_mask_index_true(SERIOUS_ERROR_FLAGS, i))
00828           new_flag.second = true;
00829         else
00830           new_flag.second = false;
00831 
00832         new_flag.first = error_flag_names[i];
00833         flags.push_back(new_flag);
00834       }
00835     }
00836     return flags;
00837   }
00838 
00839   template <class StatusType, class CommandType>
00840   void SrMotorRobotLib<StatusType, CommandType>::generate_force_control_config(int motor_index, int max_pwm, int sg_left, int sg_right, int f, int p,
00841                                                  int i, int d, int imax, int deadband, int sign)
00842   {
00843     ROS_INFO_STREAM(
00844       "Setting new pid values for motor" << motor_index << ": max_pwm="<< max_pwm <<" sgleftref=" << sg_left << " sgrightref=" << sg_right << " f=" << f << " p=" << p << " i=" << i << " d="<< d << " imax=" << imax << " deadband="<< deadband << " sign=" << sign);
00845 
00846     //the vector is of the size of the TO_MOTOR_DATA_TYPE enum.
00847     //the value of the element at a given index is the value
00848     //for the given MOTOR_CONFIG.
00849     std::vector<crc_unions::union16> full_config(MOTOR_CONFIG_CRC + 1);
00850     crc_unions::union16 value;
00851 
00852     value.word = max_pwm;
00853     full_config.at(MOTOR_CONFIG_MAX_PWM) = value;
00854 
00855     value.byte[0] = sg_left;
00856     value.byte[1] = sg_right;
00857     full_config.at(MOTOR_CONFIG_SG_REFS) = value;
00858 
00859     value.word = f;
00860     full_config.at(MOTOR_CONFIG_F) = value;
00861 
00862     value.word = p;
00863     full_config.at(MOTOR_CONFIG_P) = value;
00864 
00865     value.word = i;
00866     full_config.at(MOTOR_CONFIG_I) = value;
00867 
00868     value.word = d;
00869     full_config.at(MOTOR_CONFIG_D) = value;
00870 
00871     value.word = imax;
00872     full_config.at(MOTOR_CONFIG_IMAX) = value;
00873 
00874     value.byte[0] = deadband;
00875     value.byte[1] = sign;
00876     full_config.at(MOTOR_CONFIG_DEADBAND_SIGN) = value;
00877     ROS_DEBUG_STREAM(
00878       "deadband: " << static_cast<int>(static_cast<int8u>(value.byte[0]) ) << " value: " << static_cast<int16u>(value.word));
00879 
00880     //compute crc
00881     crc_result = 0;
00882     for (unsigned int i = MOTOR_CONFIG_FIRST_VALUE; i <= MOTOR_CONFIG_LAST_VALUE; ++i)
00883     {
00884       crc_byte = full_config.at(i).byte[0];
00885       INSERT_CRC_CALCULATION_HERE;
00886 
00887       crc_byte = full_config.at(i).byte[1];
00888       INSERT_CRC_CALCULATION_HERE;
00889     }
00890 
00891     //never send a CRC of 0, send 1 if the
00892     // computed CRC is 0 (0 is a code for
00893     // ignoring the config)
00894     if (crc_result == 0)
00895       crc_result = 1;
00896     value.word = crc_result;
00897     full_config.at(MOTOR_CONFIG_CRC) = value;
00898 
00899     ForceConfig config;
00900     config.first = motor_index;
00901     config.second = full_config;
00902     //push the new config to the configuration queue
00903     reconfig_queue.push(config);
00904   }
00905 
00906   template <class StatusType, class CommandType>
00907   void SrMotorRobotLib<StatusType, CommandType>::reinitialize_motors()
00908   {
00909     //Create a new MotorUpdater object
00910     motor_updater_ = boost::shared_ptr<generic_updater::MotorUpdater<CommandType> >(new generic_updater::MotorUpdater<CommandType>(motor_update_rate_configs_vector, operation_mode::device_update_state::INITIALIZATION));
00911     motor_current_state = operation_mode::device_update_state::INITIALIZATION;
00912     //Initialize the motor data checker
00913     motor_data_checker = boost::shared_ptr<generic_updater::MotorDataChecker>(new generic_updater::MotorDataChecker(this->joints_vector, motor_updater_->initialization_configs_vector));
00914   }
00915 
00916   template <class StatusType, class CommandType>
00917   bool SrMotorRobotLib<StatusType, CommandType>::change_control_type_callback_( sr_robot_msgs::ChangeControlType::Request& request,
00918                                                   sr_robot_msgs::ChangeControlType::Response& response )
00919   {
00920     //querying which we're control type we're using currently.
00921     if( request.control_type.control_type == sr_robot_msgs::ControlType::QUERY )
00922     {
00923       response.result = control_type_;
00924       return true;
00925     }
00926 
00927     //We're not querying the control type
00928     if( (request.control_type.control_type != sr_robot_msgs::ControlType::PWM) &&
00929         (request.control_type.control_type != sr_robot_msgs::ControlType::FORCE) )
00930     {
00931       std::string ctrl_type_text = "";
00932       if(control_type_.control_type == sr_robot_msgs::ControlType::FORCE)
00933         ctrl_type_text = "FORCE";
00934       else
00935         ctrl_type_text = "PWM";
00936 
00937       ROS_ERROR_STREAM(" The value you specified for the control type (" << request.control_type
00938                        << ") is incorrect. Using " << ctrl_type_text << " control.");
00939 
00940       response.result = control_type_;
00941       return false;
00942     }
00943 
00944     if(control_type_.control_type != request.control_type.control_type)
00945     {
00946       //Mutual exclusion with the build_command() function. We have to wait until the current motor command has been built.
00947       boost::mutex::scoped_lock l(*lock_command_sending_);
00948 
00949       ROS_WARN("Changing control type");
00950 
00951       control_type_ = request.control_type;
00952       //Flag to signal that there has been a change in the value of control_type_ and certain actions are required.
00953       //The flag is set in the callback function of the change_control_type_ service.
00954       //The flag is checked in build_command() and the necessary actions are taken there.
00955       //These actions involve calling services in the controller manager and all the active controllers. This is the
00956       //reason why we don't do it directly in the callback function. As we use a single thread to serve the callbacks,
00957       //doing so would cause a deadlock, thus we do it in the realtime loop thread instead.
00958       control_type_changed_flag_ = true;
00959     }
00960 
00961     response.result = control_type_;
00962     return true;
00963   }
00964 
00965   template <class StatusType, class CommandType>
00966   bool SrMotorRobotLib<StatusType, CommandType>::change_control_parameters(int16_t control_type)
00967   {
00968     bool success = true;
00969     std::string env_variable;
00970     std::string param_value;
00971     ros::NodeHandle nh;
00972 
00973     if( control_type == sr_robot_msgs::ControlType::PWM)
00974     {
00975       env_variable = "PWM_CONTROL=1";
00976       param_value = "PWM";
00977     }
00978     else
00979     {
00980       env_variable = "PWM_CONTROL=0";
00981       param_value = "FORCE";
00982     }
00983 
00984     // Read the namespace of the node.
00985     // The ns will be passed as an argument to the sr_edc_default_controllers.launch
00986     // so that the parameters in it will be loaded inside the correct namespace
00987     std::string ns = ros::this_node::getNamespace();
00988     std::string arguments = "";
00989 
00990     if(ns.compare("/") == 0)
00991     {
00992       ROS_DEBUG("Using base namespace: %s", ns.c_str());
00993     }
00994     else if (ns.find("//") == 0)
00995     {
00996       ns.erase(0,2);
00997       ROS_DEBUG("Node namespace: %s", ns.c_str());
00998       arguments += " set_namespace:=1 namespace:=" + ns;
00999     }
01000     else
01001     {
01002       ROS_ERROR("Node namespace: %s", ns.c_str());
01003     }
01004     arguments = " set_namespace:=0";
01005     // Read the config_dir prefix from the parameter server
01006     // The config_dir will be passed as an argument to the sr_edc_default_controllers.launch
01007     // so that the parameters in it will be read from the correct files
01008     std::string config_dir = "";
01009     nh.template param<std::string>("config_dir", config_dir, "");
01010     ROS_DEBUG("config_dir: %s", config_dir.c_str());
01011     arguments += " config_dir:=" + config_dir;
01012     ROS_INFO("arguments: %s", arguments.c_str());
01013 
01014     int result = system((env_variable + " roslaunch sr_ethercat_hand_config sr_edc_default_controllers.launch" + arguments).c_str());
01015 
01016     if(result == 0)
01017     {
01018       ROS_WARN("New parameters loaded successfully on Parameter Server");
01019 
01020       this->nh_tilde.setParam("default_control_mode", param_value);
01021 
01022       ros::ServiceClient list_ctrl_client = nh.template serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");
01023       pr2_mechanism_msgs::ListControllers controllers_list;
01024 
01025       if (list_ctrl_client.call(controllers_list))
01026       {
01027         for(unsigned int i=0; i < controllers_list.response.controllers.size(); ++i)
01028         {
01029           ros::ServiceClient reset_gains_client = nh.template serviceClient<std_srvs::Empty>(controllers_list.response.controllers.at(i) + "/reset_gains");
01030           std_srvs::Empty empty_message;
01031           if (!reset_gains_client.call(empty_message))
01032           {
01033             ROS_ERROR_STREAM("Failed to reset gains for controller: " << controllers_list.response.controllers.at(i));
01034             return false;
01035           }
01036         }
01037       }
01038       else
01039       {
01040         return false;
01041       }
01042     }
01043     else
01044     {
01045       return false;
01046     }
01047 
01048     return success;
01049   }
01050 
01051   template <class StatusType, class CommandType>
01052   bool SrMotorRobotLib<StatusType, CommandType>::motor_system_controls_callback_( sr_robot_msgs::ChangeMotorSystemControls::Request& request,
01053                                                     sr_robot_msgs::ChangeMotorSystemControls::Response& response )
01054   {
01055     std::vector<sr_robot_msgs::MotorSystemControls> tmp_motor_controls;
01056 
01057     response.result = sr_robot_msgs::ChangeMotorSystemControls::Response::SUCCESS;
01058     bool no_motor_id_out_of_range = true;
01059 
01060     for( unsigned int i=0; i < request.motor_system_controls.size(); ++i)
01061     {
01062       if( request.motor_system_controls[i].motor_id >= NUM_MOTORS ||
01063           request.motor_system_controls[i].motor_id < 0)
01064       {
01065         response.result = sr_robot_msgs::ChangeMotorSystemControls::Response::MOTOR_ID_OUT_OF_RANGE;
01066         no_motor_id_out_of_range = false;
01067       }
01068       else
01069       {
01070         //only pushes the demands with a correct motor_id
01071         tmp_motor_controls.push_back( request.motor_system_controls[i] );
01072       }
01073     }
01074 
01075     //add the request to the queue if it's not empty
01076     if( tmp_motor_controls.size() > 0 )
01077       motor_system_control_flags_.push( tmp_motor_controls );
01078 
01079     return no_motor_id_out_of_range;
01080   }
01081 
01082   //Only to ensure that the template class is compiled for the types we are interested in
01083   template class SrMotorRobotLib<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
01084   template class SrMotorRobotLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
01085 } //end namespace
01086 
01087 /* For the emacs weenies in the crowd.
01088  Local Variables:
01089  c-basic-offset: 2
01090  End:
01091  */


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50