pr2_mechanism_diagnostics.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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 <exception>
38 #include <limits>
39 #include "angles/angles.h"
40 
41 using namespace pr2_mechanism_diagnostics;
42 using namespace std;
43 
44 // Diagnostic publisher
46  pnh_("~"),
47  use_sim_time_(false),
48  disable_controller_warnings_(false)
49 {
50  mech_st_sub_ = n_.subscribe("mechanism_statistics", 1000, &CtrlJntDiagnosticPublisher::mechCallback, this);
51  diag_pub_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
52 
53  n_.param("/use_sim_time", use_sim_time_, false);
54 
55  pnh_.param("disable_controller_warnings", disable_controller_warnings_, false);
56 }
57 
58 
59 
60 
61 
62 void CtrlJntDiagnosticPublisher::mechCallback(const pr2_mechanism_msgs::MechanismStatistics::ConstPtr& mechMsg)
63 {
64  // Update joints
65  vector<pr2_mechanism_msgs::JointStatistics>::const_iterator it;
66  for (it = mechMsg->joint_statistics.begin(); it != mechMsg->joint_statistics.end(); ++it)
67  {
68  if (joint_stats.count(it->name) == 0)
69  joint_stats[it->name] = boost::shared_ptr<JointStats>(new JointStats(it->name));
70 
71  joint_stats[it->name]->update(*it);
72  }
73 
74  // Update controllers
75  vector<pr2_mechanism_msgs::ControllerStatistics>::const_iterator c_it;
76  for (c_it = mechMsg->controller_statistics.begin(); c_it != mechMsg->controller_statistics.end(); ++c_it)
77  {
78  if (controller_stats.count(c_it->name) == 0)
80 
81  controller_stats[c_it->name]->update(*c_it);
82  }
83 }
84 
86 {
87  diagnostic_msgs::DiagnosticArray array;
88 
89  // Update joints
90  map<string, boost::shared_ptr<JointStats> >::iterator it;
91  for (it = joint_stats.begin(); it != joint_stats.end(); ++it)
92  array.status.push_back(*(it->second->toDiagStat()));
93 
94  // Update controllers. Note controllers that haven't update are discarded
95  map<string, boost::shared_ptr<ControllerStats> >::iterator c_it;
96  unsigned int num_controllers = 0;
97  vector<string> erase_controllers;
98  for (c_it = controller_stats.begin(); c_it != controller_stats.end(); ++c_it)
99  {
100  if (c_it->second->shouldDiscard())
101  erase_controllers.push_back(c_it->first);
102 
103  array.status.push_back(*(c_it->second->toDiagStat()));
104  num_controllers++;
105  }
106  for (unsigned int i=0; i<erase_controllers.size(); i++)
107  controller_stats.erase(erase_controllers[i]);
108 
109  // Publish even if we have no controllers loaded
110  if (num_controllers == 0)
111  {
113  stat.name = "Controller: No controllers loaded in controller manager";
114  array.status.push_back(stat);
115  }
116 
117  array.header.stamp = ros::Time::now();
118  diag_pub_.publish(array);
119 }
120 
121 
122 int main(int argc, char **argv)
123 {
124  ros::init(argc, argv, "pr2_mechanism_diagnostics");
125 
126  try
127  {
128  CtrlJntDiagnosticPublisher ctrlJntPub;
129 
130  ros::Rate pub_rate(1.0);
131  while (ctrlJntPub.ok())
132  {
133  pub_rate.sleep();
134  ros::spinOnce();
135  ctrlJntPub.publishDiag();
136  }
137  }
138  catch (exception& e)
139  {
140  ROS_FATAL("Controllers/Joint to Diagnostics node caught exception. Aborting. %s", e.what());
141  ROS_BREAK();
142  }
143 
144  exit(0);
145  return 0;
146 }
#define ROS_FATAL(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void mechCallback(const pr2_mechanism_msgs::MechanismStatistics::ConstPtr &mechMsg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publishes diagnostics for controllers, joints from pr2_mechanism_msgs/MechanismStatistics message...
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
int main(int argc, char **argv)
static Time now()
std::map< std::string, boost::shared_ptr< JointStats > > joint_stats
ROSCPP_DECL void spinOnce()
#define ROS_BREAK()
std::map< std::string, boost::shared_ptr< ControllerStats > > controller_stats


pr2_mechanism_diagnostics
Author(s): Kevin Watts
autogenerated on Tue Mar 7 2023 03:54:57