joint_diagnostics.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 
37 #include <limits>
38 
39 #include <boost/accumulators/statistics/max.hpp>
40 #include <boost/accumulators/statistics/min.hpp>
41 
42 using namespace pr2_mechanism_diagnostics;
43 using namespace std;
44 
45 /*** TODO
46  * Set deadband properly for each joint
47  * Figure out max/min
48  * Track state with accumulators
49  * Only halt motors if we've had >3 consecutive hits, etc
50  * Add tests for warnings
51  * Publish status topic (bool) at 1 Hz
52  *
53  *****/
54 
55 
56 
57 // Joint statistics
58 JointStats::JointStats(string nam) :
59  needs_reset(true),
60  name(nam),
61  position(0),
62  velocity(0),
63  measured_effort(0),
64  commanded_effort(0),
65  is_calibrated(false),
66  violated_limits(false),
67  odometer(0),
68  max_pos_val(-1 * numeric_limits<double>::max()),
69  min_pos_val(numeric_limits<double>::max()),
70  max_abs_vel_val(-1 * numeric_limits<double>::max()),
71  max_abs_eff_val(-1 * numeric_limits<double>::max())
72 { }
73 
75 {
76  needs_reset = false;
77 
78  max_pos_val = -1 * numeric_limits<double>::max();
79  min_pos_val = numeric_limits<double>::max();
80  max_abs_vel_val = -1 * numeric_limits<double>::max();
81  max_abs_eff_val = -1 * numeric_limits<double>::max();
82 }
83 
84 
86 {
88 
89  stat->name = "Joint (" + name + ")";
90 
91  if (is_calibrated)
92  stat->summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
93  else
94  stat->summary(diagnostic_msgs::DiagnosticStatus::WARN, "Uncalibrated");
95 
96  if ((ros::Time::now() - updateTime).toSec() > 3)
97  stat->summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No updates");
98 
99  if (!is_valid(position) || !is_valid(velocity) ||
101  {
102  stat->summary(diagnostic_msgs::DiagnosticStatus::ERROR, "NaN Values in Joint State");
103  }
104 
105  stat->add("Position", position);
106  stat->add("Velocity", velocity);
107  stat->add("Measured Effort", measured_effort);
108  stat->add("Commanded Effort", commanded_effort);
109 
110  stat->add("Calibrated", is_calibrated);
111 
112  stat->add("Odometer", odometer);
113 
114  if (is_calibrated)
115  {
116  stat->add("Max Position", max_pos_val);
117  stat->add("Min Position", min_pos_val);
118  stat->add("Max Abs. Velocity", max_abs_vel_val);
119  stat->add("Max Abs. Effort", max_abs_eff_val);
120  }
121  else
122  {
123  stat->add("Max Position", "N/A");
124  stat->add("Min Position", "N/A");
125  stat->add("Max Abs. Velocity", "N/A");
126  stat->add("Max Abs. Effort", "N/A");
127  }
128 
129  stat->add("Limits Hit", violated_limits);
130 
131  needs_reset = true;
132 
133  return stat;
134 }
135 
136 bool JointStats::update(const pr2_mechanism_msgs::JointStatistics &js)
137 {
138  if (name != js.name)
139  {
140  ROS_ERROR("Joint statistics attempted to update with a different name! Old name: %s, new name: %s.", name.c_str(), js.name.c_str());
141  return false;
142  }
143 
144  if (needs_reset)
145  reset_vals();
146 
147  if (js.is_calibrated)
148  {
149  max_pos_val = max(max_pos_val, js.max_position);
150  min_pos_val = min(max_pos_val, js.min_position);
151  max_abs_vel_val = max(max_abs_vel_val, js.max_abs_velocity);
152  max_abs_eff_val = max(max_abs_eff_val, js.max_abs_effort);
153  }
154 
155  position = js.position;
156  velocity = js.velocity;
157  measured_effort = js.measured_effort;
158  commanded_effort = js.commanded_effort;
159  is_calibrated = js.is_calibrated;
160  violated_limits = js.violated_limits;
161  odometer = js.odometer;
162 
164 
165  return true;
166 }
167 
pr2_mechanism_diagnostics
Publishes diagnostics for controllers, joints from pr2_mechanism_msgs/MechanismStatistics message.
Definition: controller_diagnostics.h:47
boost::shared_ptr
pr2_mechanism_diagnostics::JointStats::measured_effort
double measured_effort
Definition: joint_diagnostics.h:118
pr2_mechanism_diagnostics::JointStats::updateTime
ros::Time updateTime
Definition: joint_diagnostics.h:111
joint_diagnostics.h
pr2_mechanism_diagnostics::JointStats::name
std::string name
Definition: joint_diagnostics.h:115
pr2_mechanism_diagnostics::JointStats::needs_reset
bool needs_reset
Definition: joint_diagnostics.h:113
pr2_mechanism_diagnostics::JointStats::is_calibrated
bool is_calibrated
Definition: joint_diagnostics.h:120
pr2_mechanism_diagnostics::JointStats::odometer
double odometer
Definition: joint_diagnostics.h:122
pr2_mechanism_diagnostics::JointStats::update
bool update(const pr2_mechanism_msgs::JointStatistics &js)
Definition: joint_diagnostics.cpp:136
pr2_mechanism_diagnostics::JointStats::position
double position
Definition: joint_diagnostics.h:116
ROS_ERROR
#define ROS_ERROR(...)
pr2_mechanism_diagnostics::JointStats::max_abs_vel_val
double max_abs_vel_val
Definition: joint_diagnostics.h:125
pr2_mechanism_diagnostics::JointStats::min_pos_val
double min_pos_val
Definition: joint_diagnostics.h:125
pr2_mechanism_diagnostics::JointStats::violated_limits
bool violated_limits
Definition: joint_diagnostics.h:121
pr2_mechanism_diagnostics::JointStats::reset_vals
void reset_vals()
Definition: joint_diagnostics.cpp:74
pr2_mechanism_diagnostics::JointStats::velocity
double velocity
Definition: joint_diagnostics.h:117
pr2_mechanism_diagnostics::JointStats::max_pos_val
double max_pos_val
Definition: joint_diagnostics.h:125
pr2_mechanism_diagnostics::JointStats::max_abs_eff_val
double max_abs_eff_val
Definition: joint_diagnostics.h:125
pr2_mechanism_diagnostics::is_valid
bool is_valid(T t)
Returns false if a value is infinity, NaN, etc.
Definition: joint_diagnostics.h:93
std
pr2_mechanism_diagnostics::JointStats::toDiagStat
boost::shared_ptr< diagnostic_updater::DiagnosticStatusWrapper > toDiagStat() const
Definition: joint_diagnostics.cpp:85
diagnostic_updater::DiagnosticStatusWrapper
pr2_mechanism_diagnostics::JointStats::commanded_effort
double commanded_effort
Definition: joint_diagnostics.h:119
pr2_mechanism_diagnostics::JointStats::JointStats
JointStats(std::string nam)
Definition: joint_diagnostics.cpp:58
true
return true
ros::Time::now
static Time now()


pr2_mechanism_diagnostics
Author(s): Kevin Watts
autogenerated on Mon Mar 6 2023 03:49:19