sr_muscle_robot_lib.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/sr_muscle_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_0300_EDC_SERIOUS_ERROR_FLAGS
00039 #define error_flag_names palm_0300_edc_error_flag_names
00040 
00041 
00042 namespace shadow_robot
00043 {
00044   template <class StatusType, class CommandType>
00045   const double SrMuscleRobotLib<StatusType, CommandType>::timeout = 5.0;
00046 
00047   template <class StatusType, class CommandType>
00048   SrMuscleRobotLib<StatusType, CommandType>::SrMuscleRobotLib(pr2_hardware_interface::HardwareInterface *hw)
00049     : SrRobotLib<StatusType, CommandType>(hw),
00050       muscle_current_state(operation_mode::device_update_state::INITIALIZATION), init_max_duration(timeout)
00051   {
00052 #ifdef DEBUG_PUBLISHER
00053     this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const);
00054     for( int i = 0; i < this->nb_debug_publishers_const; ++i )
00055     {
00056       std::stringstream ss;
00057       ss << "srh/debug_" << i;
00058       this->debug_publishers.push_back(this->node_handle.template advertise<std_msgs::Int16>(ss.str().c_str(),100));
00059     }
00060 #endif
00061     lock_init_timeout_ = boost::shared_ptr<boost::mutex>(new boost::mutex());
00062     //Create a one-shot timer
00063     check_init_timeout_timer = this->nh_tilde.createTimer(init_max_duration,
00064                                                boost::bind(&SrMuscleRobotLib<StatusType, CommandType>::init_timer_callback, this, _1), true);
00065 
00066     this->pressure_calibration_map_ = this->read_pressure_calibration();
00067   }
00068 
00069   template <class StatusType, class CommandType>
00070   shadow_joints::CalibrationMap SrMuscleRobotLib<StatusType, CommandType>::read_pressure_calibration()
00071   {
00072     ROS_INFO("Reading pressure calibration");
00073     shadow_joints::CalibrationMap pressure_calibration;
00074     std::string param_name = "sr_pressure_calibrations";
00075 
00076     XmlRpc::XmlRpcValue calib;
00077     this->nodehandle_.getParam(param_name, calib);
00078     ROS_ASSERT(calib.getType() == XmlRpc::XmlRpcValue::TypeArray);
00079     //iterate on all the joints
00080     for(int32_t index_cal = 0; index_cal < calib.size(); ++index_cal)
00081     {
00082       //check the calibration is well formatted:
00083       // first joint name, then calibration table
00084       ROS_ASSERT(calib[index_cal][0].getType() == XmlRpc::XmlRpcValue::TypeString);
00085       ROS_ASSERT(calib[index_cal][1].getType() == XmlRpc::XmlRpcValue::TypeArray);
00086 
00087       std::string joint_name = static_cast<std::string> (calib[index_cal][0]);
00088       std::vector<joint_calibration::Point> calib_table_tmp;
00089 
00090       //now iterates on the calibration table for the current joint
00091       for(int32_t index_table=0; index_table < calib[index_cal][1].size(); ++index_table)
00092       {
00093         ROS_ASSERT(calib[index_cal][1][index_table].getType() == XmlRpc::XmlRpcValue::TypeArray);
00094         //only 2 values per calibration point: raw and calibrated (doubles)
00095         ROS_ASSERT(calib[index_cal][1][index_table].size() == 2);
00096         ROS_ASSERT(calib[index_cal][1][index_table][0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00097         ROS_ASSERT(calib[index_cal][1][index_table][1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00098 
00099 
00100         joint_calibration::Point point_tmp;
00101         point_tmp.raw_value = static_cast<double> (calib[index_cal][1][index_table][0]);
00102         point_tmp.calibrated_value = static_cast<double> (calib[index_cal][1][index_table][1]);
00103         calib_table_tmp.push_back(point_tmp);
00104       }
00105 
00106       pressure_calibration.insert(joint_name, boost::shared_ptr<shadow_robot::JointCalibration>(new shadow_robot::JointCalibration(calib_table_tmp)) );
00107     }
00108 
00109     return pressure_calibration;
00110   }
00111 
00112   template <class StatusType, class CommandType>
00113   void SrMuscleRobotLib<StatusType, CommandType>::update(StatusType* status_data)
00114   {
00115     //read the PIC idle time
00116     this->main_pic_idle_time = status_data->idle_time_us;
00117     if (status_data->idle_time_us < this->main_pic_idle_time_min)
00118       this->main_pic_idle_time_min = status_data->idle_time_us;
00119 
00120     //get the current timestamp
00121     struct timeval tv;
00122     double timestamp = 0.0;
00123     if (gettimeofday(&tv, NULL))
00124     {
00125       ROS_WARN("SrMuscleRobotLib: Failed to get system time, timestamp in state will be zero");
00126     }
00127     else
00128     {
00129       timestamp = double(tv.tv_sec) + double(tv.tv_usec) / 1.0e+6;
00130     }
00131 
00132     //First we read the tactile sensors information
00133     this->update_tactile_info(status_data);
00134 
00135     //then we read the muscle drivers information
00136     boost::ptr_vector<shadow_joints::MuscleDriver>::iterator muscle_driver_tmp = this->muscle_drivers_vector_.begin();
00137     for (; muscle_driver_tmp != this->muscle_drivers_vector_.end(); ++muscle_driver_tmp)
00138     {
00139       read_muscle_driver_data(muscle_driver_tmp, status_data);
00140     }
00141 
00142     //then we read the joints informations
00143     boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = this->joints_vector.begin();
00144     for (; joint_tmp != this->joints_vector.end(); ++joint_tmp)
00145     {
00146       sr_actuator::SrActuatorState* actuator_state = this->get_joint_actuator_state(joint_tmp);
00147 
00148       boost::shared_ptr<shadow_joints::MuscleWrapper> muscle_wrapper = boost::static_pointer_cast<shadow_joints::MuscleWrapper>(joint_tmp->actuator_wrapper);
00149 
00150       actuator_state->is_enabled_ = 1;
00151       //actuator_state->device_id_ = muscle_wrapper->muscle_id[0];
00152       actuator_state->halted_ = false;
00153 
00154       //Fill in the tactiles.
00155       if( this->tactiles != NULL )
00156         actuator_state->tactiles_ = this->tactiles->get_tactile_data();
00157 
00158       this->process_position_sensor_data(joint_tmp, status_data, timestamp);
00159 
00160       //if no muscle is associated to this joint, then continue
00161       if ((muscle_wrapper->muscle_driver_id[0] == -1))
00162         continue;
00163 
00164       read_additional_muscle_data(joint_tmp, status_data);
00165     } //end for joint
00166   } //end update()
00167 
00168   template <class StatusType, class CommandType>
00169   void SrMuscleRobotLib<StatusType, CommandType>::build_command(CommandType* command)
00170   {
00171     if (muscle_current_state == operation_mode::device_update_state::INITIALIZATION)
00172     {
00173       muscle_current_state = muscle_updater_->build_init_command(command);
00174     }
00175     else
00176     {
00177       //build the motor command
00178       muscle_current_state = muscle_updater_->build_command(command);
00179     }
00180 
00181     //Build the tactile sensors command
00182     this->build_tactile_command(command);
00183 
00185     // Now we chose the command to send to the muscle
00186     // by default we send a valve demand, but if we have a reset command,
00187     // then we send the reset.
00188     if ( reset_muscle_driver_queue.empty() )
00189     {
00190       command->to_muscle_data_type = MUSCLE_DEMAND_VALVES;
00191 
00192       //loop on all the joints and update their motor: we're sending commands to all the motors.
00193       boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = this->joints_vector.begin();
00194       for (; joint_tmp != this->joints_vector.end(); ++joint_tmp)
00195       {
00196         boost::shared_ptr<shadow_joints::MuscleWrapper> muscle_wrapper = boost::static_pointer_cast<shadow_joints::MuscleWrapper>(joint_tmp->actuator_wrapper);
00197         sr_actuator::SrMuscleActuator* muscle_actuator = static_cast<sr_actuator::SrMuscleActuator*>(muscle_wrapper->actuator);
00198 
00199         unsigned int muscle_driver_id_0 = muscle_wrapper->muscle_driver_id[0];
00200         unsigned int muscle_driver_id_1 = muscle_wrapper->muscle_driver_id[1];
00201         unsigned int muscle_id_0 = muscle_wrapper->muscle_id[0];
00202         unsigned int muscle_id_1 = muscle_wrapper->muscle_id[1];
00203 
00204         if (joint_tmp->has_actuator)
00205         {
00206           if( !this->nullify_demand_ )
00207           {
00208             set_valve_demand(&(command->muscle_data[(muscle_driver_id_0 * 10 + muscle_id_0) / 2]), muscle_actuator->command_.valve_[0], ((uint8_t)muscle_id_0) & 0x01);
00209             set_valve_demand(&(command->muscle_data[(muscle_driver_id_1 * 10 + muscle_id_1) / 2]), muscle_actuator->command_.valve_[1], ((uint8_t)muscle_id_1) & 0x01);
00210 
00211             muscle_actuator->state_.last_commanded_valve_[0] = muscle_actuator->command_.valve_[0];
00212             muscle_actuator->state_.last_commanded_valve_[1] = muscle_actuator->command_.valve_[1];
00213           }
00214           else
00215           {
00216             //We want to send a demand of 0
00217             set_valve_demand(&(command->muscle_data[(muscle_driver_id_0 * 10 + muscle_id_0) / 2]), 0, ((uint8_t)muscle_id_0) & 0x01);
00218             set_valve_demand(&(command->muscle_data[(muscle_driver_id_1 * 10 + muscle_id_1) / 2]), 0, ((uint8_t)muscle_id_1) & 0x01);
00219 
00220             muscle_actuator->state_.last_commanded_valve_[0] = 0;
00221             muscle_actuator->state_.last_commanded_valve_[1] = 0;
00222           }
00223 /*
00224 #ifdef DEBUG_PUBLISHER
00225           //publish the debug values for the given motors.
00226           // NB: debug_motor_indexes_and_data is smaller
00227           //     than debug_publishers.
00228           int publisher_index = 0;
00229           boost::shared_ptr<std::pair<int,int> > debug_pair;
00230           if( this->debug_mutex.try_lock() )
00231           {
00232             BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data)
00233             {
00234               if( debug_pair != NULL )
00235               {
00236                 shadow_joints::MuscleWrapper* actuator_wrapper = static_cast<shadow_joints::MuscleWrapper*>(joint_tmp->actuator_wrapper.get());
00237                 //check if we want to publish some data for the current motor
00238                 if( debug_pair->first == actuator_wrapper->muscle_id[0] )
00239                 {
00240                   //check if it's the correct data
00241                   if( debug_pair->second == -1 )
00242                   {
00243                     this->msg_debug.data = joint_tmp->actuator_wrapper->actuator->command_.effort_;
00244                     this->debug_publishers[publisher_index].publish(this->msg_debug);
00245                   }
00246                 }
00247               }
00248               publisher_index ++;
00249             }
00250 
00251             this->debug_mutex.unlock();
00252           } //end try_lock
00253 #endif
00254 */
00255         } //end if has_actuator
00256       } // end for each joint
00257     } //endif
00258     else
00259     {
00260       //we have some reset command waiting.
00261       // We'll send all of them
00262       command->to_muscle_data_type = MUSCLE_SYSTEM_RESET;
00263 
00264       while (!reset_muscle_driver_queue.empty())
00265       {
00266         short muscle_driver_id = reset_muscle_driver_queue.front();
00267         reset_muscle_driver_queue.pop();
00268 
00269         //reset the CAN messages counters for the muscle driver we're going to reset.
00270         boost::ptr_vector<shadow_joints::MuscleDriver>::iterator driver = this->muscle_drivers_vector_.begin();
00271         for (; driver != this->muscle_drivers_vector_.end(); ++driver)
00272         {
00273           if( driver->muscle_driver_id == muscle_driver_id)
00274           {
00275             driver->can_msgs_transmitted_ = 0;
00276             driver->can_msgs_received_ = 0;
00277           }
00278         }
00279 
00280         // we send the MUSCLE_SYSTEM_RESET_KEY
00281         // and the muscle_driver_id (on the bus)
00282         crc_unions::union16 to_send;
00283         to_send.byte[1] = MUSCLE_SYSTEM_RESET_KEY >> 8;
00284         if (muscle_driver_id > 1)
00285           to_send.byte[0] = muscle_driver_id - 2;
00286         else
00287           to_send.byte[0] = muscle_driver_id;
00288 
00289         command->muscle_data[muscle_driver_id * 5] = to_send.byte[0];
00290         command->muscle_data[muscle_driver_id * 5 + 1] = to_send.byte[1];
00291       }
00292     } // end if reset queue not empty
00293   }
00294 
00295   template <class StatusType, class CommandType>
00296   inline void SrMuscleRobotLib<StatusType, CommandType>::set_valve_demand(uint8_t *muscle_data_byte_to_set, int8_t valve_value, uint8_t shift)
00297   {
00298     uint8_t tmp_valve = 0;
00299 
00300     //The encoding we want for the negative integers is represented in two's complement, but based on a 4 bit data size instead of 8 bit, so
00301     // we'll have to do it manually
00302     if (valve_value < 0)
00303     {
00304       //Take the value as positive
00305       tmp_valve = -valve_value;
00306       //Do a bitwise inversion on the 4 lowest bytes only
00307       tmp_valve = (~tmp_valve) & 0x0F;
00308       //Add one
00309       tmp_valve = tmp_valve + 1;
00310     }
00311     else //Positive representation is straightforward
00312     {
00313       tmp_valve = valve_value & 0x0F;
00314     }
00315 
00316     //A shift of 0 means that we want to write the value on the 4 least significant bits
00317     // shift of 1 means that we want to write the value on the 4 most significant bits
00318 
00319     //We zero the 4 bits that we want to write our valve value on
00320     *muscle_data_byte_to_set &= (0xF0 >> (shift * 4) );
00321     //We write our valve value on those 4 bits
00322     *muscle_data_byte_to_set |= (tmp_valve << (shift * 4) );
00323   }
00324 
00325   template <class StatusType, class CommandType>
00326   void SrMuscleRobotLib<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00327                                    diagnostic_updater::DiagnosticStatusWrapper &d)
00328   {
00329     boost::ptr_vector<shadow_joints::Joint>::iterator joint = this->joints_vector.begin();
00330     for (; joint != this->joints_vector.end(); ++joint)
00331     {
00332       std::stringstream name;
00333       name.str("");
00334       name << "SRDMotor " << joint->joint_name;
00335       d.name = name.str();
00336 
00337       boost::shared_ptr<shadow_joints::MuscleWrapper> actuator_wrapper = boost::static_pointer_cast<shadow_joints::MuscleWrapper>(joint->actuator_wrapper);
00338 
00339       if (joint->has_actuator)
00340       {
00341         const sr_actuator::SrMuscleActuator* sr_muscle_actuator = static_cast<sr_actuator::SrMuscleActuator*>(actuator_wrapper->actuator);
00342         const sr_actuator::SrMuscleActuatorState* state = &(sr_muscle_actuator->state_);
00343 
00344         if (actuator_wrapper->actuator_ok)
00345         {
00346           if (actuator_wrapper->bad_data)
00347           {
00348             d.summary(d.WARN, "WARNING, bad CAN data received");
00349 
00350             d.clear();
00351             d.addf("Muscle ID 0", "%d", actuator_wrapper->muscle_id[0]);
00352             d.addf("Muscle driver ID 0", "%d", actuator_wrapper->muscle_driver_id[0]);
00353             d.addf("Muscle ID 1", "%d", actuator_wrapper->muscle_id[1]);
00354             d.addf("Muscle driver ID 1", "%d", actuator_wrapper->muscle_driver_id[1]);
00355           }
00356           else //the data is good
00357           {
00358             d.summary(d.OK, "OK");
00359 
00360             d.clear();
00361             d.addf("Muscle ID 0", "%d", actuator_wrapper->muscle_id[0]);
00362             d.addf("Muscle driver ID 0", "%d", actuator_wrapper->muscle_driver_id[0]);
00363             d.addf("Muscle ID 1", "%d", actuator_wrapper->muscle_id[1]);
00364             d.addf("Muscle driver ID 1", "%d", actuator_wrapper->muscle_driver_id[1]);
00365 
00366             d.addf("Unfiltered position", "%f", state->position_unfiltered_);
00367 
00368             d.addf("Last Commanded Valve 0", "%d", state->last_commanded_valve_[0]);
00369             d.addf("Last Commanded Valve 1", "%d", state->last_commanded_valve_[1]);
00370 
00371             d.addf("Unfiltered Pressure 0", "%u", state->pressure_[0]);
00372             d.addf("Unfiltered Pressure 1", "%u", state->pressure_[1]);
00373 
00374             d.addf("Position", "%f", state->position_);
00375           }
00376         }
00377         else
00378         {
00379           d.summary(d.ERROR, "Motor error");
00380           d.clear();
00381           d.addf("Muscle ID 0", "%d", actuator_wrapper->muscle_id[0]);
00382           d.addf("Muscle driver ID 0", "%d", actuator_wrapper->muscle_driver_id[0]);
00383           d.addf("Muscle ID 1", "%d", actuator_wrapper->muscle_id[1]);
00384           d.addf("Muscle driver ID 1", "%d", actuator_wrapper->muscle_driver_id[1]);
00385         }
00386       }
00387       else
00388       {
00389         d.summary(d.OK, "No motor associated to this joint");
00390         d.clear();
00391       }
00392       vec.push_back(d);
00393 
00394     } //end for each joints
00395 
00396 
00397     boost::ptr_vector<shadow_joints::MuscleDriver>::iterator muscle_driver = this->muscle_drivers_vector_.begin();
00398     for (; muscle_driver != this->muscle_drivers_vector_.end(); ++muscle_driver)
00399     {
00400       std::stringstream name;
00401       name.str("");
00402       name << "Muscle driver " << muscle_driver->muscle_driver_id;
00403       d.name = name.str();
00404 
00405       if (muscle_driver->driver_ok)
00406       {
00407         if (muscle_driver->bad_data)
00408         {
00409           d.summary(d.WARN, "WARNING, bad CAN data received");
00410 
00411           d.clear();
00412           d.addf("Muscle Driver ID", "%d", muscle_driver->muscle_driver_id);
00413         }
00414         else //the data is good
00415         {
00416           d.summary(d.OK, "OK");
00417 
00418           d.clear();
00419           d.addf("Muscle Driver ID", "%d", muscle_driver->muscle_driver_id);
00420           d.addf("Serial Number", "%d", muscle_driver->serial_number );
00421           d.addf("Assembly date", "%d / %d / %d", muscle_driver->assembly_date_day, muscle_driver->assembly_date_month, muscle_driver->assembly_date_year );
00422 
00423           d.addf("Number of CAN messages received", "%lld", muscle_driver->can_msgs_received_);
00424           d.addf("Number of CAN messages transmitted", "%lld", muscle_driver->can_msgs_transmitted_);
00425 
00426           if (muscle_driver->firmware_modified_)
00427             d.addf("Firmware svn revision (server / pic / modified)", "%d / %d / True",
00428                    muscle_driver->server_firmware_svn_revision_, muscle_driver->pic_firmware_svn_revision_);
00429           else
00430             d.addf("Firmware svn revision (server / pic / modified)", "%d / %d / False",
00431                    muscle_driver->server_firmware_svn_revision_, muscle_driver->pic_firmware_svn_revision_);
00432         }
00433       }
00434       else
00435       {
00436         d.summary(d.ERROR, "Muscle Driver error");
00437         d.clear();
00438         d.addf("Muscle Driver ID", "%d", muscle_driver->muscle_driver_id);
00439       }
00440 
00441       vec.push_back(d);
00442     } //end for each muscle driver
00443 
00444   }
00445 
00446 
00447   template <class StatusType, class CommandType>
00448   void SrMuscleRobotLib<StatusType, CommandType>::read_additional_muscle_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data)
00449   {
00450     int packet_offset_muscle_0 = 0;
00451     int packet_offset_muscle_1 = 0;
00452 
00453     boost::shared_ptr<shadow_joints::MuscleWrapper> muscle_wrapper = boost::static_pointer_cast<shadow_joints::MuscleWrapper>(joint_tmp->actuator_wrapper);
00454 
00455     //Every muscle driver sends two muscle_data_packet containing pressures from 5 muscles each
00456     if (muscle_wrapper->muscle_id[0] >= NUM_PRESSURE_SENSORS_PER_MESSAGE)
00457     {
00458       packet_offset_muscle_0 = 1;
00459     }
00460 
00461     if (muscle_wrapper->muscle_id[1] >= NUM_PRESSURE_SENSORS_PER_MESSAGE)
00462     {
00463       packet_offset_muscle_1 = 1;
00464     }
00465 
00466     //check the masks to see if the CAN messages arrived from the muscle driver
00467     //the flag should be set to 1 for each muscle driver CAN message (a muscle driver sends 2 separate CAN messages with 5 muscle pressures each.
00468     // Every actuator (every joint) has two muscles, so we check both flags to decide that the actuator is OK
00469     muscle_wrapper->actuator_ok = sr_math_utils::is_bit_mask_index_true(status_data->which_muscle_data_arrived,
00470                                                                         muscle_wrapper->muscle_driver_id[0] * 2 + packet_offset_muscle_0)
00471                                && sr_math_utils::is_bit_mask_index_true(status_data->which_muscle_data_arrived,
00472                                                                         muscle_wrapper->muscle_driver_id[1] * 2 + packet_offset_muscle_1);
00473 
00474     /*
00475     //check the masks to see if a bad CAN message arrived
00476     //the flag should be 0
00477     muscle_wrapper->bad_data = sr_math_utils::is_bit_mask_index_true(status_data->which_pressure_data_had_errors,
00478                                                                                   muscle_wrapper->muscle_driver_id[0] * 10 + muscle_wrapper->muscle_id[0])
00479                                          && sr_math_utils::is_bit_mask_index_true(status_data->which_pressure_data_had_errors,
00480                                                                                   muscle_wrapper->muscle_driver_id[1] * 10 + muscle_wrapper->muscle_id[1]);
00481     */
00482 
00483     if (muscle_wrapper->actuator_ok)
00484     {
00485       sr_actuator::SrMuscleActuator* actuator = static_cast<sr_actuator::SrMuscleActuator*>(joint_tmp->actuator_wrapper->actuator);
00486       shadow_joints::MuscleWrapper* actuator_wrapper = static_cast<shadow_joints::MuscleWrapper*>(joint_tmp->actuator_wrapper.get());
00487 /*            
00488 #ifdef DEBUG_PUBLISHER
00489       int publisher_index = 0;
00490       //publish the debug values for the given motors.
00491       // NB: debug_motor_indexes_and_data is smaller
00492       //     than debug_publishers.
00493       boost::shared_ptr<std::pair<int,int> > debug_pair;
00494 
00495       if( this->debug_mutex.try_lock() )
00496       {
00497         BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data)
00498         {
00499           if( debug_pair != NULL )
00500           {
00501             //check if we want to publish some data for the current motor
00502             if( debug_pair->first == actuator_wrapper->muscle_id[0] )
00503             {
00504               //if < 0, then we're not asking for a FROM_MOTOR_DATA_TYPE
00505               if( debug_pair->second > 0 )
00506               {
00507                 //check if it's the correct data
00508                 if( debug_pair->second == status_data->muscle_data_type )
00509                 {
00510                   //TODO do something meaningful here
00511                   //this->msg_debug.data = status_data->muscle_data_packet[0].misc;
00512                   this->msg_debug.data = 0;
00513                   this->debug_publishers[publisher_index].publish(this->msg_debug);
00514                 }
00515               }
00516             }
00517           }
00518           publisher_index ++;
00519         }
00520 
00521         this->debug_mutex.unlock();
00522       } //end try_lock
00523 #endif
00524 */
00525       //we received the data and it was correct
00526       unsigned int p1 = 0;
00527       switch (status_data->muscle_data_type)
00528       {
00529       case MUSCLE_DATA_PRESSURE:
00530         // Calibrate the raw values to bar pressure values.
00531         for (int i=0; i<2; ++i)
00532         {
00533           p1 = get_muscle_pressure(muscle_wrapper->muscle_driver_id[i], muscle_wrapper->muscle_id[i], status_data);
00534           std::string name = joint_tmp->joint_name + "_" + boost::lexical_cast<std::string>(i);
00535           // XXX: If the joint isn't found we crash
00536           //ROS_INFO_STREAM("Calib: "<<name);
00537           pressure_calibration_tmp_ = pressure_calibration_map_.find(name);
00538           double bar = pressure_calibration_tmp_->compute(static_cast<double>(p1));
00539           if (bar < 0.0)
00540               bar = 0.0;
00541           actuator->state_.pressure_[i] = static_cast<int16u>(bar);
00542         }
00543         // Raw values
00544         //actuator->state_.pressure_[0] = static_cast<int16u>(get_muscle_pressure(muscle_wrapper->muscle_driver_id[0], muscle_wrapper->muscle_id[0], status_data));
00545         //actuator->state_.pressure_[1] = static_cast<int16u>(get_muscle_pressure(muscle_wrapper->muscle_driver_id[1], muscle_wrapper->muscle_id[1], status_data));
00546         //ROS_WARN("DriverID: %u MuscleID: %u Pressure 0: %u", muscle_wrapper->muscle_driver_id[0], muscle_wrapper->muscle_id[0],  actuator->state_.pressure_[0]);
00547         //ROS_WARN("DriverID: %u MuscleID: %u Pressure 1: %u", muscle_wrapper->muscle_driver_id[1], muscle_wrapper->muscle_id[1],  actuator->state_.pressure_[1]);
00548 
00549 #ifdef DEBUG_PUBLISHER
00550         if( actuator_wrapper->muscle_id[0] == 8 )
00551         {
00552           //ROS_ERROR_STREAM("SGL " <<actuator->state_.strain_gauge_left_);
00553           //this->msg_debug.data = actuator->state_.strain_gauge_left_;
00554           //this->debug_publishers[0].publish(this->msg_debug);
00555         }
00556 #endif
00557         break;
00558 
00559 
00560       default:
00561         break;
00562       }
00563     }
00564   }
00565 
00566 
00567   template <class StatusType, class CommandType>
00568   unsigned int SrMuscleRobotLib<StatusType, CommandType>::get_muscle_pressure(int muscle_driver_id, int muscle_id, StatusType *status_data)
00569   {
00570     unsigned int muscle_pressure = 0;
00571     int packet_offset = 0;
00572     int muscle_index = muscle_id;
00573 
00574     //Every muscle driver sends two muscle_data_packet containing pressures from 5 muscles each
00575     if (muscle_id >= NUM_PRESSURE_SENSORS_PER_MESSAGE)
00576     {
00577       packet_offset = 1;
00578       muscle_index = muscle_id - NUM_PRESSURE_SENSORS_PER_MESSAGE;
00579     }
00580 
00581     switch(muscle_index)
00582     {
00583       case 0:
00584         muscle_pressure = (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure0_H << 8)
00585                           + (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure0_M << 4)
00586                           + status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure0_L;
00587         break;
00588 
00589       case 1:
00590         muscle_pressure = (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure1_H << 8)
00591                           + (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure1_M << 4)
00592                           + status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure1_L;
00593         break;
00594 
00595       case 2:
00596         muscle_pressure = (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure2_H << 8)
00597                           + (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure2_M << 4)
00598                           + status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure2_L;
00599         break;
00600 
00601       case 3:
00602         muscle_pressure = (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure3_H << 8)
00603                           + (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure3_M << 4)
00604                           + status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure3_L;
00605         break;
00606 
00607       case 4:
00608         muscle_pressure = (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure4_H << 8)
00609                           + (status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure4_M << 4)
00610                           + status_data->muscle_data_packet[muscle_driver_id * 2 + packet_offset].packed.pressure4_L;
00611         break;
00612 
00613       default:
00614         ROS_ERROR("Incorrect muscle index: %d", muscle_index);
00615         break;
00616     }
00617 
00618     return muscle_pressure;
00619   }
00620 
00621   template <class StatusType, class CommandType>
00622   void SrMuscleRobotLib<StatusType, CommandType>::read_muscle_driver_data(boost::ptr_vector<shadow_joints::MuscleDriver>::iterator muscle_driver_tmp, StatusType* status_data)
00623   {
00624     //check one the masks (e.g. the first) for this muscle driver to see if the CAN messages arrived correctly from the muscle driver
00625     //the flag should be set to 1 for each message in this driver.
00626     muscle_driver_tmp->driver_ok = sr_math_utils::is_bit_mask_index_true(status_data->which_muscle_data_arrived,
00627                                                                         muscle_driver_tmp->muscle_driver_id * 2 );
00628     /*
00629     //check the masks to see if a bad CAN message arrived
00630     //the flag should be 0
00631     muscle_driver_tmp->bad_data = sr_math_utils::is_bit_mask_index_true(status_data->which_pressure_data_had_errors,
00632                                                                         muscle_driver_tmp->muscle_driver_id * 2 );
00633     */
00634 
00635     if (muscle_driver_tmp->driver_ok)
00636     {
00637       //we received the data and it was correct
00638       set_muscle_driver_data_received_flags(status_data->muscle_data_type, muscle_driver_tmp->muscle_driver_id);
00639 
00640       switch (status_data->muscle_data_type)
00641       {
00642       case MUSCLE_DATA_PRESSURE:
00643         //We don't do anything here. This will be treated in a per-joint loop in read_additional_muscle_data
00644         break;
00645 
00646       case MUSCLE_DATA_CAN_STATS:
00647         // For the moment all the CAN data statistics for a muscle driver are contained in the first packet coming from that driver
00648         // these are 16 bits values and will overflow -> we compute the real value.
00649         // This needs to be updated faster than the overflowing period (which should be roughly every 30s)
00650         muscle_driver_tmp->can_msgs_received_ = sr_math_utils::counter_with_overflow(
00651                                                     muscle_driver_tmp->can_msgs_received_,
00652                                                     static_cast<int16u>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].misc.can_msgs_rx));
00653         muscle_driver_tmp->can_msgs_transmitted_ = sr_math_utils::counter_with_overflow(
00654                                                        muscle_driver_tmp->can_msgs_transmitted_,
00655                                                        static_cast<int16u>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].misc.can_msgs_tx));
00656 
00657         //CAN bus errors
00658         muscle_driver_tmp->can_err_rx = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].misc.can_err_rx);
00659         muscle_driver_tmp->can_err_tx = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].misc.can_err_tx);
00660         break;
00661 
00662       case MUSCLE_DATA_SLOW_MISC:
00663         //We received a slow data:
00664         // the slow data type is not transmitted anymore (as it was in the motor hand protocol)
00665         // Instead, all the information (of every data type) is contained in the 2 packets that come from every muscle driver
00666         // So in fact this message is not "slow" anymore.
00667         muscle_driver_tmp->pic_firmware_svn_revision_ = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].slow_0.SVN_revision);
00668         muscle_driver_tmp->server_firmware_svn_revision_ = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].slow_0.SVN_server);
00669         muscle_driver_tmp->firmware_modified_ = static_cast<bool>(static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2].slow_0.SVN_modified));
00670 
00671         muscle_driver_tmp->serial_number = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2 + 1].slow_1.serial_number);
00672         muscle_driver_tmp->assembly_date_year = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2 + 1].slow_1.assembly_date_YYYY);
00673         muscle_driver_tmp->assembly_date_month = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2 + 1].slow_1.assembly_date_MM);
00674         muscle_driver_tmp->assembly_date_day = static_cast<unsigned int>(status_data->muscle_data_packet[muscle_driver_tmp->muscle_driver_id * 2 + 1].slow_1.assembly_date_DD);
00675         break;
00676 
00677       default:
00678         break;
00679       }
00680 
00681       //Mutual exclusion with the the initialization timeout
00682       boost::mutex::scoped_lock l(*lock_init_timeout_);
00683 
00684       //Check the message to see if everything has already been received
00685       if (muscle_current_state == operation_mode::device_update_state::INITIALIZATION)
00686       {
00687         if ((check_muscle_driver_data_received_flags())
00688             || (muscle_updater_->update_state == operation_mode::device_update_state::OPERATION))
00689         {
00690           muscle_updater_->update_state = operation_mode::device_update_state::OPERATION;
00691           muscle_current_state = operation_mode::device_update_state::OPERATION;
00692           //stop the timer
00693           check_init_timeout_timer.stop();
00694 
00695           ROS_INFO("All muscle data initialized.");
00696         }
00697       }
00698     }
00699   }
00700 
00701   template <class StatusType, class CommandType>
00702   inline void SrMuscleRobotLib<StatusType, CommandType>::set_muscle_driver_data_received_flags(unsigned int msg_type, int muscle_driver_id)
00703   {
00704     if (muscle_current_state == operation_mode::device_update_state::INITIALIZATION)
00705     {
00706       std::map<unsigned int, unsigned int>::iterator it = from_muscle_driver_data_received_flags_.find(msg_type);
00707       if (it != from_muscle_driver_data_received_flags_.end())
00708       {
00709         it->second = it->second | (1 << muscle_driver_id);
00710       }
00711     }
00712   }
00713 
00714   template <class StatusType, class CommandType>
00715   inline bool SrMuscleRobotLib<StatusType, CommandType>::check_muscle_driver_data_received_flags()
00716   {
00717     std::map<unsigned int, unsigned int>::iterator it = from_muscle_driver_data_received_flags_.begin();
00718     for (;it != from_muscle_driver_data_received_flags_.end(); ++it)
00719     {
00720       //We want to detect when the flag for every driver is checked, so, for NUM_MUSCLE_DRIVERS = 4 we want to have 00001111
00721       if (it->second != (1 << NUM_MUSCLE_DRIVERS) -1)
00722         return false;
00723     }
00724     return true;
00725   }
00726 
00727   template <class StatusType, class CommandType>
00728   std::vector<std::pair<std::string, bool> > SrMuscleRobotLib<StatusType, CommandType>::humanize_flags(int flag)
00729   {
00730     std::vector<std::pair<std::string, bool> > flags;
00731 
00732     //16 is the number of flags
00733     for (unsigned int i = 0; i < 16; ++i)
00734     {
00735       std::pair<std::string, bool> new_flag;
00736       //if the flag is set add the name
00737       if (sr_math_utils::is_bit_mask_index_true(flag, i))
00738       {
00739         if (sr_math_utils::is_bit_mask_index_true(SERIOUS_ERROR_FLAGS, i))
00740           new_flag.second = true;
00741         else
00742           new_flag.second = false;
00743 
00744         new_flag.first = error_flag_names[i];
00745         flags.push_back(new_flag);
00746       }
00747     }
00748     return flags;
00749   }
00750 
00751   template <class StatusType, class CommandType>
00752   void SrMuscleRobotLib<StatusType, CommandType>::reinitialize_motors()
00753   {
00754     //Mutual exclusion with the the initialization timeout
00755     boost::mutex::scoped_lock l(*lock_init_timeout_);
00756 
00757     //stop the timer just in case it was still running
00758     check_init_timeout_timer.stop();
00759     //Create a new MuscleUpdater object
00760     muscle_updater_ = boost::shared_ptr<generic_updater::MuscleUpdater<CommandType> >(new generic_updater::MuscleUpdater<CommandType>(muscle_update_rate_configs_vector, operation_mode::device_update_state::INITIALIZATION));
00761     muscle_current_state = operation_mode::device_update_state::INITIALIZATION;
00762     //To reschedule the one-shot timer
00763     check_init_timeout_timer.setPeriod(init_max_duration);
00764     check_init_timeout_timer.start();
00765   }
00766 
00767   template <class StatusType, class CommandType>
00768   void SrMuscleRobotLib<StatusType, CommandType>::init_timer_callback(const ros::TimerEvent& event)
00769   {
00770     //Mutual exclusion with the the initialization timeout
00771     boost::mutex::scoped_lock l(*lock_init_timeout_);
00772 
00773     if(muscle_current_state == operation_mode::device_update_state::INITIALIZATION)
00774     {
00775       muscle_updater_->update_state = operation_mode::device_update_state::OPERATION;
00776       muscle_current_state = operation_mode::device_update_state::OPERATION;
00777       ROS_ERROR_STREAM("Muscle Initialization Timeout: the static information in the diagnostics may not be up to date.");
00778     }
00779   }
00780 
00781   //Only to ensure that the template class is compiled for the types we are interested in
00782   template class SrMuscleRobotLib<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00783 } //end namespace
00784 
00785 /* For the emacs weenies in the crowd.
00786  Local Variables:
00787  c-basic-offset: 2
00788  End:
00789  */


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