motor_test.cpp
Go to the documentation of this file.
00001 
00028 #include <boost/algorithm/string.hpp>
00029 #include "sr_self_test/motor_test.hpp"
00030 #include <pr2_mechanism_msgs/ListControllers.h>
00031 #include <pr2_mechanism_msgs/SwitchController.h>
00032 #include <pr2_mechanism_msgs/LoadController.h>
00033 #include <sr_robot_msgs/ChangeControlType.h>
00034 #include <std_msgs/Float64.h>
00035 #include <sstream>
00036 
00037 namespace shadow_robot
00038 {
00039   const double MotorTest::STANDARD_PWM_TARGET_ = 150.0; //Used for most of the motors
00040   const double MotorTest::WRJ1_PWM_TARGET_ = 250.0; //Used for WRJ1
00041   const double MotorTest::WRJ2_PWM_TARGET_ = 190.0; //Used for WRJ2
00042 
00043   const int MotorTest::STRAIN_GAUGE_THRESHOLD_ = 40; //the minimum value of the SG when pulling in that direction
00044 
00045   MotorTest::MotorTest(self_test::TestRunner* test_runner,
00046                        std::string joint_name,
00047                        shadowrobot::HandCommander* hand_commander)
00048     : test_runner_(test_runner), joint_name_( joint_name ), hand_commander_(hand_commander),
00049       record_data_(0), test_current_zero_(true), test_current_moving_(true), test_strain_gauge_right_(true), test_strain_gauge_left_(true), PWM_target_(STANDARD_PWM_TARGET_)
00050   {
00051     //joint name is lower case in the controller topics
00052     boost::algorithm::to_lower(joint_name_);
00053 
00054     if(joint_name_.compare("wrj1") == 0)
00055       PWM_target_ = WRJ1_PWM_TARGET_;
00056     else if(joint_name_.compare("wrj2") == 0)
00057       PWM_target_ = WRJ2_PWM_TARGET_;
00058     else 
00059       PWM_target_ = STANDARD_PWM_TARGET_;
00060 
00061     test_runner_->add("Test motor ["+joint_name_+"]", this, &MotorTest::run_test);
00062   }
00063 
00064   void MotorTest::run_test(diagnostic_updater::DiagnosticStatusWrapper& status)
00065   {
00066     //reset test results
00067     test_current_zero_ = true;
00068     test_current_moving_ = true;
00069     test_strain_gauge_right_ = true;
00070     test_strain_gauge_left_ = true;
00071 
00072     //check current control type
00073     sr_robot_msgs::ControlType current_ctrl_type;
00074     sr_robot_msgs::ChangeControlType change_ctrl_type;
00075     change_ctrl_type.request.control_type.control_type = sr_robot_msgs::ControlType::QUERY;
00076     if( ros::service::call("realtime_loop/change_control_type", change_ctrl_type) )
00077     {
00078       current_ctrl_type = change_ctrl_type.response.result;
00079     }
00080     else
00081     {
00082       status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00083                       "Failed to get current control type - aborting test." );
00084       return;
00085     }
00086 
00087     //switch to PWM
00088     change_ctrl_type.request.control_type.control_type = sr_robot_msgs::ControlType::PWM;
00089     if( !ros::service::call("realtime_loop/change_control_type", change_ctrl_type) )
00090     {
00091       status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00092                       "Failed to change to PWM control type - aborting test." );
00093       return;
00094     }
00095 
00096     //subscribe to diagnostic topic
00097     diagnostic_sub_ = nh_.subscribe("diagnostics_agg", 1, &MotorTest::diagnostics_agg_cb_, this);
00098 
00099     std::string current_ctrl, effort_ctrl;
00100     //list currently running controllers and find the one for effort control
00101     pr2_mechanism_msgs::ListControllers list_ctrl;
00102     if( ros::service::call("pr2_controller_manager/list_controllers", list_ctrl))
00103     {
00104       for (size_t i = 0; i < list_ctrl.response.controllers.size(); ++i)
00105       {
00106         if( list_ctrl.response.controllers[i].find( joint_name_ ) != std::string::npos )
00107         {
00108           if( list_ctrl.response.state[i].compare( "running" ) == 0 )
00109           {
00110             current_ctrl = list_ctrl.response.controllers[i];
00111           }
00112           if( list_ctrl.response.controllers[i].find( "_effort_controller" ) != std::string::npos )
00113           {
00114             effort_ctrl = list_ctrl.response.controllers[i];
00115           }
00116         }
00117       }
00118     }
00119     else
00120     {
00121       status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00122                       "Failed to list controllers - aborting test." );
00123       return;
00124     }
00125 
00126     //load the effort controller if necessary
00127     if( effort_ctrl.compare("") == 0 )
00128     {
00129       effort_ctrl = "sh_"+joint_name_+"_effort_controller";
00130       pr2_mechanism_msgs::LoadController load_ctrl;
00131       load_ctrl.request.name = effort_ctrl;
00132       if( !ros::service::call("pr2_controller_manager/load_controller", load_ctrl))
00133       {
00134         status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00135                         "Failed to load controller "+load_ctrl.request.name+" - aborting test." );
00136       }
00137     }
00138 
00139     //switching to effort controllers (in PWM -> send direct PWM demand)
00140     pr2_mechanism_msgs::SwitchController switch_ctrl;
00141     switch_ctrl.request.start_controllers.push_back(effort_ctrl);
00142     switch_ctrl.request.stop_controllers.push_back(current_ctrl);
00143     switch_ctrl.request.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
00144     if( !ros::service::call("pr2_controller_manager/switch_controller", switch_ctrl))
00145     {
00146       status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00147                       "Failed to switch from controller "+current_ctrl+" to controller "+effort_ctrl +" - aborting test." );
00148       return;
00149     }
00150 
00151     //apply effort and start recording data
00152     effort_pub_ = nh_.advertise<std_msgs::Float64>(effort_ctrl+"/command", 5, true);
00153     std_msgs::Float64 target;
00154     ros::Rate rate(2.0);
00155 
00156     //first one way
00157     target.data = PWM_target_;
00158     record_data_ = 1;
00159 
00160     for (unsigned int i=0; i < 10; ++i)
00161     {
00162       effort_pub_.publish(target);
00163       rate.sleep();
00164     }
00165 
00166     //then the other
00167     target.data = -PWM_target_;
00168     record_data_ = -1;
00169     for (unsigned int i=0; i < 10; ++i)
00170     {
00171       effort_pub_.publish(target);
00172       rate.sleep();
00173     }
00174 
00175     //then nothing (to check that current is back to 0)
00176     target.data = 0.0;
00177     record_data_ = 0;
00178     for (unsigned int i=0; i < 10; ++i)
00179     {
00180       effort_pub_.publish(target);
00181       rate.sleep();
00182     }
00183 
00184     //stop the subscriber
00185     diagnostic_sub_.shutdown();
00186 
00187     //reset to previous control mode
00188     change_ctrl_type.request.control_type = current_ctrl_type;
00189     if( !ros::service::call("realtime_loop/change_control_type", change_ctrl_type) )
00190     {
00191       status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00192                       "Failed to change back to the previous control type - aborting test." );
00193       return;
00194     }
00195 
00196     switch_ctrl.request.start_controllers.push_back(current_ctrl);
00197     switch_ctrl.request.stop_controllers.push_back(effort_ctrl);
00198     switch_ctrl.request.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
00199     if( !ros::service::call("pr2_controller_manager/switch_controller", switch_ctrl))
00200     {
00201       status.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
00202                       "Failed to switch from controller "+effort_ctrl+" to controller "+current_ctrl +" - aborting test." );
00203       return;
00204     }
00205 
00206     //test results
00207     std::stringstream ss;
00208     if( test_current_zero_ && test_current_moving_ &&
00209         test_strain_gauge_right_ && test_strain_gauge_left_ )
00210     {
00211       status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Test passed.");
00212       return;
00213     }
00214 
00215     //there was an error during the test
00216     else
00217     {
00218       ss << "Test failed: ";
00219       if(!test_current_zero_)
00220         ss << " Current was not zero when the motor was stopped. ";
00221       if(!test_current_moving_)
00222         ss << " Current was out of specified interval when the motor was driven. ";
00223       if(!test_strain_gauge_right_)
00224         ss << " Problem with Strain Gauge Right.";
00225       if(!test_strain_gauge_left_)
00226         ss << " Problem with Strain Gauge Left.";
00227     }
00228 
00229     status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, ss.str());
00230   }
00231 
00232   void MotorTest::diagnostics_agg_cb_(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
00233   {
00234     for( size_t status_i = 0; status_i < msg->status.size(); ++status_i )
00235     {
00236       if( msg->status[status_i].name.find("SRDMotor "+boost::algorithm::to_upper_copy(joint_name_) ) != std::string::npos )
00237       {
00238         for (size_t value_i = 0; value_i < msg->status[status_i].values.size(); ++value_i)
00239         {
00240           if( msg->status[status_i].values[value_i].key.compare("Measured Current") == 0 )
00241           {
00242             double current = ::atof( msg->status[status_i].values[value_i].value.c_str() );
00243 
00244             //not sending any targets, current should be close to 0
00245             if( record_data_ == 0)
00246             {
00247               if( current > 0.016 )
00248               {
00249                 test_current_zero_ = false;
00250                 ROS_DEBUG("Current zero test error: %f", current);
00251               }
00252               else
00253               {
00254                 test_current_zero_ = true;
00255               }
00256             }
00257             else
00258             {
00259               //sending some targets, current should be between ? and ?
00260               //@todo find min max values for current when driving motors
00261               if( current > 0.5 || current < 0.012 )
00262               {
00263                 test_current_moving_ = false;
00264                 ROS_DEBUG("Current moving test error: %f", current);
00265               }
00266               else
00267               {
00268                 test_current_moving_ = true;
00269               }
00270             }
00271           }
00272           else if( msg->status[status_i].values[value_i].key.compare("Strain Gauge Left") == 0 &&
00273                    record_data_ == -1 )
00274           {
00275             //@todo is it always SGL for + and SGR for - ??
00276             int sgl = ::atoi( msg->status[status_i].values[value_i].value.c_str() );
00277             //@todo check min value for SG under tension
00278             if( sgl < STRAIN_GAUGE_THRESHOLD_ )
00279             {
00280               test_strain_gauge_left_ = false;
00281               ROS_DEBUG("sgl test error: %d", sgl);
00282             }
00283             else
00284             {
00285               test_strain_gauge_left_ = true;
00286             }
00287           }
00288           else if( msg->status[status_i].values[value_i].key.compare("Strain Gauge Right") == 0 &&
00289                    record_data_ == 1)
00290           {
00291             //@todo same here
00292             int sgr = ::atoi( msg->status[status_i].values[value_i].value.c_str() );
00293             if( sgr < STRAIN_GAUGE_THRESHOLD_ )
00294             {
00295               test_strain_gauge_right_ = false;
00296               ROS_DEBUG("sgr test error: %d", sgr);
00297             }
00298             else
00299             {
00300               test_strain_gauge_right_ = true;
00301             }
00302           }
00303         }
00304       }
00305 
00306     }
00307   }
00308 }
00309 
00310 /* For the emacs weenies in the crowd.
00311    Local Variables:
00312    c-basic-offset: 2
00313    End:
00314 */
00315 


sr_self_test
Author(s): Ugo Cupcic
autogenerated on Mon Oct 6 2014 07:52:52