motor_data_checker.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_robot_lib/motor_data_checker.hpp"
00028 
00029 namespace generic_updater
00030 {
00031   const double MotorDataChecker::timeout = 5.0;
00032 
00033   MotorDataChecker::MotorDataChecker(boost::ptr_vector<shadow_joints::Joint> joints_vector,
00034                                      std::vector<UpdateConfig> initialization_configs_vector)
00035       : nh_tilde("~"), update_state(operation_mode::device_update_state::INITIALIZATION), init_max_duration(timeout)
00036   {
00037     init(joints_vector, initialization_configs_vector);
00038   }
00039 
00040   MotorDataChecker::~MotorDataChecker()
00041   {
00042     for(unsigned int i=0; i<msg_checkers_.size();i++)
00043     {
00044       for(unsigned int j=0; j<msg_checkers_.at(i).msg_from_motor_checkers.size();j++)
00045       {
00046         delete msg_checkers_.at(i).msg_from_motor_checkers.at(j);
00047       }
00048     }
00049   }
00050 
00051   void MotorDataChecker::init(boost::ptr_vector<shadow_joints::Joint> joints_vector,
00052                               std::vector<UpdateConfig> initialization_configs_vector)
00053   {
00054     //Create a one-shot timer
00055     check_timeout_timer = nh_tilde.createTimer(init_max_duration,
00056                                                boost::bind(&MotorDataChecker::timer_callback, this, _1), true);
00057     update_state = operation_mode::device_update_state::INITIALIZATION;
00058     msg_checkers_.clear();
00059 
00060     std::vector<UpdateConfig>::iterator msg_it;
00061 
00062     for (msg_it = initialization_configs_vector.begin(); msg_it < initialization_configs_vector.end(); msg_it++)
00063     {
00064       MessageChecker tmp_msg_checker( static_cast<FROM_MOTOR_DATA_TYPE>(msg_it->what_to_update) );
00065       boost::ptr_vector<shadow_joints::Joint>::iterator joint;
00066       for (joint = joints_vector.begin(); joint < joints_vector.end(); joint++)
00067       {
00068         if (joint->has_motor)
00069         {
00070           if (msg_it->what_to_update == MOTOR_DATA_SLOW_MISC)
00071           {
00072             tmp_msg_checker.msg_from_motor_checkers.push_back(new SlowMessageFromMotorChecker(joint->motor->motor_id));
00073           }
00074           else
00075           {
00076             tmp_msg_checker.msg_from_motor_checkers.push_back(new MessageFromMotorChecker(joint->motor->motor_id));
00077           }
00078         }
00079       }
00080       msg_checkers_.push_back(tmp_msg_checker);
00081     }
00082   }
00083 
00084   bool MotorDataChecker::check_message(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp,
00085                                        FROM_MOTOR_DATA_TYPE motor_data_type, int16u motor_slow_data_type)
00086   {
00087     int index_motor_data_type = 0;
00088 
00089     index_motor_data_type = find(motor_data_type);
00090     if (index_motor_data_type != (-1))
00091     {
00092       int index_motor_id = 0;
00093       index_motor_id = msg_checkers_.at(index_motor_data_type).find(joint_tmp->motor->motor_id);
00094       if (index_motor_id != (-1))
00095       {
00096         if (motor_data_type == MOTOR_DATA_SLOW_MISC)
00097         {
00098           SlowMessageFromMotorChecker* ptr_tmp_checker = dynamic_cast<SlowMessageFromMotorChecker*>( msg_checkers_.at(index_motor_data_type).msg_from_motor_checkers.at(index_motor_id) );
00099 
00100           if(ptr_tmp_checker != NULL)
00101           {
00102             ptr_tmp_checker->set_received( static_cast<FROM_MOTOR_SLOW_DATA_TYPE>(motor_slow_data_type) );
00103           }
00104           else
00105           {
00106             ROS_ERROR_STREAM("Checker conversion failed");
00107           }
00108         }
00109         else
00110         {
00111           msg_checkers_.at(index_motor_data_type).msg_from_motor_checkers.at(index_motor_id)->set_received();
00112         }
00113       }
00114       else
00115       {
00116         ROS_ERROR_STREAM("Motor id not found: " << joint_tmp->motor->motor_id );
00117       }
00118     }
00119     return ((update_state == operation_mode::device_update_state::OPERATION) || is_everything_checked());
00120   }
00121 
00122   bool MotorDataChecker::is_everything_checked()
00123   {
00124     std::vector<MessageChecker>::iterator it;
00125 
00126     for (it = msg_checkers_.begin(); it < msg_checkers_.end(); it++)
00127     {
00128       std::vector<MessageFromMotorChecker*>::iterator it2;
00129 
00130       for (it2 = it->msg_from_motor_checkers.begin(); it2 < it->msg_from_motor_checkers.end(); it2++)
00131       {
00132         if (!(*it2)->get_received())
00133         {
00134           return false;
00135         }
00136       }
00137     }
00138 
00139     //all the motors are initialized -> we stop the timeout timer
00140     check_timeout_timer.stop();
00141     update_state = operation_mode::device_update_state::OPERATION;
00142     return true;
00143   }
00144 
00145   int MotorDataChecker::find(FROM_MOTOR_DATA_TYPE motor_data_type)
00146   {
00147     for(unsigned int i=0;i<msg_checkers_.size();i++)
00148     {
00149       if (msg_checkers_.at(i).msg_type == motor_data_type)
00150         return i;
00151     }
00152     return (-1);
00153   }
00154 
00155   void MotorDataChecker::timer_callback(const ros::TimerEvent& event)
00156   {
00157     if( update_state == operation_mode::device_update_state::INITIALIZATION )
00158     {
00159       update_state = operation_mode::device_update_state::OPERATION;
00160       ROS_ERROR_STREAM("Motor Initialization Timeout: the static information in the diagnostics may not be uptodate.");
00161     }
00162   }
00163 
00164   int MessageChecker::find(int motor_id)
00165   {
00166     for (unsigned int i=0; i< msg_from_motor_checkers.size(); i++)
00167     {
00168       if (msg_from_motor_checkers.at(i)->motor_id_ == motor_id)
00169         return i;
00170     }
00171     return (-1);
00172   }
00173 
00174   SlowMessageFromMotorChecker::SlowMessageFromMotorChecker(int id)
00175       : MessageFromMotorChecker(id)
00176   {
00177     for (int i = 0; i <= MOTOR_SLOW_DATA_LAST; i++)
00178     {
00179       slow_data_received.at(i) = false;
00180     }
00181   }
00182 
00183   void SlowMessageFromMotorChecker::set_received(FROM_MOTOR_SLOW_DATA_TYPE slow_data_type)
00184   {
00185     if (received_ == false)
00186     {
00187       //Check the slow data type as received
00188       if ( slow_data_type > MOTOR_SLOW_DATA_LAST )
00189       {
00190         ROS_ERROR_STREAM("Received bad slow_data_type: " << slow_data_type << " > " << slow_data_received.size() );
00191         return;
00192       }
00193       slow_data_received.at(slow_data_type) = true;
00194 
00195       //look if every type is received, then change FROM_MOTOR_SLOW_DATA_TYPE general received state accordingly
00196       bool checked = true;
00197       for (int i = MOTOR_SLOW_DATA_SVN_REVISION; i <= MOTOR_SLOW_DATA_LAST; i++)
00198       {
00199         checked = checked && slow_data_received.at(i);
00200         if (!checked)
00201         {
00202           break;
00203         }
00204       }
00205       if (checked)
00206         received_ = true;
00207     }
00208   }
00209 
00210   void MessageFromMotorChecker::set_received()
00211   {
00212     received_ = true;
00213   }
00214 
00215   bool MessageFromMotorChecker::get_received()
00216   {
00217     return received_;
00218   }
00219 
00220 }
00221 
00222 /* For the emacs weenies in the crowd.
00223  Local Variables:
00224  c-basic-offset: 2
00225  End:
00226  */


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Thu Jan 2 2014 12:02:16