sensor_noise_test.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_self_test/sensor_noise_test.hpp"
00029 
00030 namespace shadow_robot
00031 {
00032   const double SensorNoiseTest::MAX_NOISE_CONST_ = 0.0087; //0.5 degrees
00033   const double SensorNoiseTest::NOISE_EPSILON_CONST_ = 0.000000001; //So small that no real sensor can have smaller noise
00034 
00035   SensorNoiseTest::SensorNoiseTest()
00036   {
00037   }
00038 
00039   void SensorNoiseTest::test_sensor_noise(diagnostic_updater::DiagnosticStatusWrapper& status)
00040   {
00041     // We subscribe to the joint_states topic when the test starts running.
00042     // This way we avoid the initial ramping up of the filtered position when the driver starts
00043     // (first pos=0, then the real sensor data is fed into the filter, and the position starts approaching that value)
00044     // We can perfectly assume that the filtered value is stabilized here.
00045     joint_states_sub_ = nh_.subscribe("joint_states", 50, &SensorNoiseTest::joint_states_cb_, this);
00046 
00047     ros::Rate rate(100);
00048     for (size_t i = 0; i < 400; ++i)
00049     {
00050       ros::spinOnce();
00051       rate.sleep();
00052     }
00053 
00054     std::vector<std::string> failed_joints;
00055     std::vector<double> failed_noises;
00056     double min, max;
00057     std::map<std::string, std::vector<double> >::iterator joint_states;
00058     for(joint_states = all_joint_states_.begin(); joint_states != all_joint_states_.end(); ++joint_states)
00059     {
00060       if(joint_states->second.size() > 1 )
00061       {
00062         min = joint_states->second[0];
00063         max = joint_states->second[0];
00064 
00065         //starting loop at 1, as we already assigned 0 to min/max
00066         for (size_t i = 1; i < joint_states->second.size(); ++i)
00067         {
00068           if(joint_states->second[i] < min)
00069             min = joint_states->second[i];
00070           if(joint_states->second[i] > max)
00071             max = joint_states->second[i];
00072         }
00073 
00074         double noise = max - min;
00075         ROS_DEBUG("Joint %s Pos min: %lf  max: %lf  noise: %lf", joint_states->first.c_str(),  min , max , noise );
00076         
00077         if( noise > MAX_NOISE_CONST_ || noise <= NOISE_EPSILON_CONST_ )
00078         {
00079           failed_joints.push_back(joint_states->first);
00080           failed_noises.push_back( noise );
00081         }
00082       }
00083     }
00084 
00085     if( failed_joints.size() > 0 )
00086     {
00087       std::stringstream ss;
00088       ss << "Noises out of threshold: ";
00089       for (size_t i = 0; i < failed_joints.size(); ++i)
00090       {
00091         ss << "[" << failed_joints[i] << " -> " << failed_noises[i] << " ] ";
00092       }
00093       status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, ss.str());
00094     }
00095     else
00096       status.summary(diagnostic_msgs::DiagnosticStatus::OK, "All the noises are below threshold.");
00097 
00098     joint_states_sub_.shutdown();
00099   }
00100 
00101   void SensorNoiseTest::joint_states_cb_(const sensor_msgs::JointState::ConstPtr& msg)
00102   {
00103     for (size_t i = 0; i < msg->name.size(); ++i)
00104     {
00105       all_joint_states_[ msg->name[i] ].push_back( msg->position[i] );
00106     }
00107   }
00108 }
00109 
00110 /* For the emacs weenies in the crowd.
00111    Local Variables:
00112    c-basic-offset: 2
00113    End:
00114 */
00115 


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