DiagnosticAggregator Analyzer: report() does not report every analyzed message
I am not sure if I understand correctly how the report function from DiagnosticAggregator::Analyzer works.
I've written a simple analyzer for testing with the result that only one message is being reported although all are being analyzed. I am publishing two messages with names "msg_1" and "msg_2", I'll put the code below. The analyzer is supposed to match, analyze and report both of them with the status name "Status". However, this is the terminal output:
[ INFO] [1652810163.795448496]: init
[ INFO] [1652810163.799310391]: report new StatusItem
[ INFO] [1652810164.800081138]: match msg_1
[ INFO] [1652810164.800298881]: analyze msg_1
[ INFO] [1652810164.800503410]: match msg_2
[ INFO] [1652810164.800608405]: analyze msg_2
[ INFO] [1652810164.800724279]: report msg_2
[ INFO] [1652810165.799693929]: analyze msg_1
[ INFO] [1652810165.799833866]: analyze msg_2
[ INFO] [1652810165.799994756]: report msg_2
[ INFO] [1652810166.799601716]: analyze msg_1
[ INFO] [1652810166.799831711]: analyze msg_2
[ INFO] [1652810166.800042445]: report msg_2
...
So even though both messages are being analyzed, only the second is being reported. I just can't figure out what is the error. Or is this even the supposed behaviour and I've missunderstood the usage of the Analyzer? (I followed this tutorial which is not very explanatory).
Any help is appreciated very much!
Here is my analyzer class definition:
class SimpleAnalyzer : public diagnostic_aggregator::Analyzer
{
public:
SimpleAnalyzer();
~SimpleAnalyzer();
bool init(const std::string base_name, const ros::NodeHandle& n) override;
bool match(const std::string name) override;
bool analyze(const boost::shared_ptr<diagnostic_aggregator::StatusItem> item) override;
std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > report() override;
std::string getPath() const override { return path_; }
std::string getName() const override { return nice_name_; }
private:
std::string path_, nice_name_;
boost::shared_ptr<diagnostic_aggregator::StatusItem> item_;
};
And the source:
SimpleAnalyzer::SimpleAnalyzer(): path_(""), nice_name_("Simple"){}
SimpleAnalyzer::~SimpleAnalyzer() {}
bool SimpleAnalyzer::init(const std::string base_name, const ros::NodeHandle& n)
{
ROS_INFO("simple init");
path_ = base_name + "/" + nice_name_;
boost::shared_ptr<diagnostic_aggregator::StatusItem> item(new diagnostic_aggregator::StatusItem("new StatusItem"));
item_ = item;
return true;
}
bool SimpleAnalyzer::match(const std::string name)
{
ROS_INFO("match %s", name.c_str());
return true;
}
bool SimpleAnalyzer::analyze(const boost::shared_ptr<diagnostic_aggregator::StatusItem> item)
{
ROS_INFO("analyze %s", item->getName().c_str());
item_ = item;
return true;
}
std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > SimpleAnalyzer::report()
{
ROS_INFO("report %s", item_->getName().c_str());
boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> status = item_->toStatusMsg(path_);
status->name = "Status";
status->level = diagnostic_msgs::DiagnosticStatus::OK;
status->message = "simple message";
std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > output;
output.push_back(status);
return output;
}
This is the code I use for publishing the dummy DiagnosticStatus msgs:
while (ros::ok())
{
diagnostic_msgs::DiagnosticArray array;
diagnostic_msgs::DiagnosticStatus simple_status;
simple_status.name = "msg_1";
array.status.push_back(simple_status);
simple_status.name = "msg_2";
array.status.push_back(simple_status);
array.header.stamp = ros::Time::now();
pub.publish(array);
ros::Duration(1).sleep();
ros::spinOnce();
}