joint_diagnostics.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 
00036 #include "pr2_mechanism_diagnostics/joint_diagnostics.h"
00037 #include <limits>
00038 
00039 #include <boost/accumulators/statistics/max.hpp>
00040 #include <boost/accumulators/statistics/min.hpp>
00041 
00042 using namespace pr2_mechanism_diagnostics;
00043 using namespace std;
00044 
00045 /*** TODO
00046  * Set deadband properly for each joint
00047  * Figure out max/min
00048  * Track state with accumulators
00049  * Only halt motors if we've had >3 consecutive hits, etc
00050  * Add tests for warnings
00051  * Publish status topic (bool) at 1 Hz
00052  * 
00053  *****/
00054 
00055 
00056 
00057 // Joint statistics
00058 JointStats::JointStats(string nam) : 
00059   needs_reset(true),
00060   name(nam),
00061   position(0),
00062   velocity(0),
00063   measured_effort(0),
00064   commanded_effort(0),
00065   is_calibrated(false),
00066   violated_limits(false),
00067   odometer(0),
00068   max_pos_val(-1 * numeric_limits<double>::max()),
00069   min_pos_val(numeric_limits<double>::max()),
00070   max_abs_vel_val(-1 * numeric_limits<double>::max()),
00071   max_abs_eff_val(-1 * numeric_limits<double>::max())    
00072 { }
00073 
00074 void JointStats::reset_vals()
00075 {
00076   needs_reset = false;
00077   
00078   max_pos_val = -1 * numeric_limits<double>::max();
00079   min_pos_val = numeric_limits<double>::max();
00080   max_abs_vel_val = -1 * numeric_limits<double>::max();
00081   max_abs_eff_val = -1 * numeric_limits<double>::max();
00082 }
00083 
00084 
00085 boost::shared_ptr<diagnostic_updater::DiagnosticStatusWrapper> JointStats::toDiagStat() const
00086 {
00087   boost::shared_ptr<diagnostic_updater::DiagnosticStatusWrapper> stat(new diagnostic_updater::DiagnosticStatusWrapper);
00088   
00089   stat->name = "Joint (" + name + ")";
00090   
00091   if (is_calibrated)
00092     stat->summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
00093   else
00094     stat->summary(diagnostic_msgs::DiagnosticStatus::WARN, "Uncalibrated");
00095   
00096   if ((ros::Time::now() - updateTime).toSec() > 3)
00097     stat->summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No updates");
00098   
00099   if (!is_valid(position) || !is_valid(velocity) || 
00100       !is_valid(measured_effort) || !is_valid(commanded_effort))
00101   {
00102     stat->summary(diagnostic_msgs::DiagnosticStatus::ERROR, "NaN Values in Joint State");
00103   }
00104   
00105   stat->add("Position", position);
00106   stat->add("Velocity", velocity);
00107   stat->add("Measured Effort", measured_effort);
00108   stat->add("Commanded Effort", commanded_effort);
00109   
00110   stat->add("Calibrated", is_calibrated);
00111   
00112   stat->add("Odometer", odometer);
00113   
00114   if (is_calibrated)
00115   {
00116     stat->add("Max Position", max_pos_val);
00117     stat->add("Min Position", min_pos_val);
00118     stat->add("Max Abs. Velocity", max_abs_vel_val);
00119     stat->add("Max Abs. Effort", max_abs_eff_val);
00120   }
00121   else
00122   {
00123     stat->add("Max Position", "N/A");
00124     stat->add("Min Position", "N/A");
00125     stat->add("Max Abs. Velocity", "N/A");
00126     stat->add("Max Abs. Effort", "N/A");
00127   }
00128   
00129   stat->add("Limits Hit", violated_limits);
00130   
00131   needs_reset = true;
00132   
00133   return stat;
00134 }
00135 
00136 bool JointStats::update(const pr2_mechanism_msgs::JointStatistics &js)
00137 {
00138   if (name != js.name)
00139   {
00140     ROS_ERROR("Joint statistics attempted to update with a different name! Old name: %s, new name: %s.", name.c_str(), js.name.c_str());
00141     return false;
00142   }
00143   
00144   if (needs_reset)
00145     reset_vals();
00146   
00147   if (js.is_calibrated)
00148   {
00149     max_pos_val     = max(max_pos_val,     js.max_position);
00150     min_pos_val     = min(max_pos_val,     js.min_position);
00151     max_abs_vel_val = max(max_abs_vel_val, js.max_abs_velocity);
00152     max_abs_eff_val = max(max_abs_eff_val, js.max_abs_effort);
00153   }
00154   
00155   position         = js.position;
00156   velocity         = js.velocity;
00157   measured_effort  = js.measured_effort;
00158   commanded_effort = js.commanded_effort;
00159   is_calibrated    = js.is_calibrated;
00160   violated_limits  = js.violated_limits;
00161   odometer         = js.odometer;
00162   
00163   updateTime = ros::Time::now();
00164   
00165   return true;
00166 }
00167 


pr2_mechanism_diagnostics
Author(s): Kevin Watts
autogenerated on Mon Dec 2 2013 13:13:06