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_actuator)
00069         {
00070           boost::shared_ptr<shadow_joints::MotorWrapper> motor_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint->actuator_wrapper);
00071           if (msg_it->what_to_update == MOTOR_DATA_SLOW_MISC)
00072           {
00073             tmp_msg_checker.msg_from_motor_checkers.push_back(new SlowMessageFromMotorChecker(motor_wrapper->motor_id));
00074           }
00075           else
00076           {
00077             tmp_msg_checker.msg_from_motor_checkers.push_back(new MessageFromMotorChecker(motor_wrapper->motor_id));
00078           }
00079         }
00080       }
00081       msg_checkers_.push_back(tmp_msg_checker);
00082     }
00083   }
00084 
00085   bool MotorDataChecker::check_message(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp,
00086                                        FROM_MOTOR_DATA_TYPE motor_data_type, int16u motor_slow_data_type)
00087   {
00088     int index_motor_data_type = 0;
00089 
00090     index_motor_data_type = find(motor_data_type);
00091     if (index_motor_data_type != (-1))
00092     {
00093       int index_motor_id = 0;
00094       boost::shared_ptr<shadow_joints::MotorWrapper> motor_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint_tmp->actuator_wrapper);
00095       index_motor_id = msg_checkers_.at(index_motor_data_type).find(motor_wrapper->motor_id);
00096       if (index_motor_id != (-1))
00097       {
00098         if (motor_data_type == MOTOR_DATA_SLOW_MISC)
00099         {
00100           SlowMessageFromMotorChecker* ptr_tmp_checker = dynamic_cast<SlowMessageFromMotorChecker*>( msg_checkers_.at(index_motor_data_type).msg_from_motor_checkers.at(index_motor_id) );
00101 
00102           if(ptr_tmp_checker != NULL)
00103           {
00104             ptr_tmp_checker->set_received( static_cast<FROM_MOTOR_SLOW_DATA_TYPE>(motor_slow_data_type) );
00105           }
00106           else
00107           {
00108             ROS_ERROR_STREAM("Checker conversion failed");
00109           }
00110         }
00111         else
00112         {
00113           msg_checkers_.at(index_motor_data_type).msg_from_motor_checkers.at(index_motor_id)->set_received();
00114         }
00115       }
00116       else
00117       {
00118         ROS_ERROR_STREAM("Motor id not found: " << motor_wrapper->motor_id );
00119       }
00120     }
00121     return ((update_state == operation_mode::device_update_state::OPERATION) || is_everything_checked());
00122   }
00123 
00124   bool MotorDataChecker::is_everything_checked()
00125   {
00126     std::vector<MessageChecker>::iterator it;
00127 
00128     for (it = msg_checkers_.begin(); it < msg_checkers_.end(); it++)
00129     {
00130       std::vector<MessageFromMotorChecker*>::iterator it2;
00131 
00132       for (it2 = it->msg_from_motor_checkers.begin(); it2 < it->msg_from_motor_checkers.end(); it2++)
00133       {
00134         if (!(*it2)->get_received())
00135         {
00136           return false;
00137         }
00138       }
00139     }
00140 
00141     //all the motors are initialized -> we stop the timeout timer
00142     check_timeout_timer.stop();
00143     update_state = operation_mode::device_update_state::OPERATION;
00144     return true;
00145   }
00146 
00147   int MotorDataChecker::find(FROM_MOTOR_DATA_TYPE motor_data_type)
00148   {
00149     for(unsigned int i=0;i<msg_checkers_.size();i++)
00150     {
00151       if (msg_checkers_.at(i).msg_type == motor_data_type)
00152         return i;
00153     }
00154     return (-1);
00155   }
00156 
00157   void MotorDataChecker::timer_callback(const ros::TimerEvent& event)
00158   {
00159     if( update_state == operation_mode::device_update_state::INITIALIZATION )
00160     {
00161       update_state = operation_mode::device_update_state::OPERATION;
00162       ROS_ERROR_STREAM("Motor Initialization Timeout: the static information in the diagnostics may not be uptodate.");
00163     }
00164   }
00165 
00166   int MessageChecker::find(int motor_id)
00167   {
00168     for (unsigned int i=0; i< msg_from_motor_checkers.size(); i++)
00169     {
00170       if (msg_from_motor_checkers.at(i)->motor_id_ == motor_id)
00171         return i;
00172     }
00173     return (-1);
00174   }
00175 
00176   SlowMessageFromMotorChecker::SlowMessageFromMotorChecker(int id)
00177       : MessageFromMotorChecker(id)
00178   {
00179     for (int i = 0; i <= MOTOR_SLOW_DATA_LAST; i++)
00180     {
00181       slow_data_received.at(i) = false;
00182     }
00183   }
00184 
00185   void SlowMessageFromMotorChecker::set_received(FROM_MOTOR_SLOW_DATA_TYPE slow_data_type)
00186   {
00187     if (received_ == false)
00188     {
00189       //Check the slow data type as received
00190       if ( slow_data_type > MOTOR_SLOW_DATA_LAST )
00191       {
00192         ROS_ERROR_STREAM("Received bad slow_data_type: " << slow_data_type << " > " << slow_data_received.size() );
00193         return;
00194       }
00195       slow_data_received.at(slow_data_type) = true;
00196 
00197       //look if every type is received, then change FROM_MOTOR_SLOW_DATA_TYPE general received state accordingly
00198       bool checked = true;
00199       for (int i = MOTOR_SLOW_DATA_SVN_REVISION; i <= MOTOR_SLOW_DATA_LAST; i++)
00200       {
00201         checked = checked && slow_data_received.at(i);
00202         if (!checked)
00203         {
00204           break;
00205         }
00206       }
00207       if (checked)
00208         received_ = true;
00209     }
00210   }
00211 
00212   void MessageFromMotorChecker::set_received()
00213   {
00214     received_ = true;
00215   }
00216 
00217   bool MessageFromMotorChecker::get_received()
00218   {
00219     return received_;
00220   }
00221 
00222 }
00223 
00224 /* For the emacs weenies in the crowd.
00225  Local Variables:
00226  c-basic-offset: 2
00227  End:
00228  */


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