Skip to content

Commit 1949776

Browse files
ajanani85ajanani85
committed
feat: messages for system resource
1 parent 6e43738 commit 1949776

2 files changed

Lines changed: 131 additions & 22 deletions

File tree

‎include/resource_monitor/resource_status_class.h‎

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,10 @@
1414
#include <std_msgs/msg/int32.hpp>
1515
#include <sys/statvfs.h> // For statvfs
1616
#include <iomanip> // For std::setprecision
17-
17+
#include <brain_box_msgs/msg/system_report.hpp>
18+
#include <vb_util_lib/bag_logger.h>
19+
#include <vb_util_lib/vb_main.h>
20+
#include <brain_box_msgs/msg/log_control.hpp>
1821

1922
namespace am
2023
{
@@ -76,7 +79,7 @@ class ResourceStatus
7679

7780
double getUpTime();
7881

79-
void updateInfos();
82+
void updateInfos(brain_box_msgs::msg::SystemReport &sys_report);
8083

8184
void print();
8285

@@ -105,10 +108,16 @@ class ResourceStatus
105108

106109
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr stat_sub_;
107110

111+
rclcpp::Subscription<brain_box_msgs::msg::LogControl>::SharedPtr log_ctrl_sub_;
112+
113+
rclcpp::Publisher<brain_box_msgs::msg::SystemReport>::SharedPtr sys_rep_pub_;
114+
108115
void statusCB(const std_msgs::msg::Int32::SharedPtr msg);
109116

110117
void statCB(const std_msgs::msg::Int32::SharedPtr msg);
111118

119+
void logCtrlCB(const brain_box_msgs::msg::LogControl::SharedPtr msg);
120+
112121
int getCPUCoresCount();
113122

114123
std::string readFile(const std::string& path);
@@ -125,8 +134,14 @@ class ResourceStatus
125134

126135
bool is_first_time_ {true};
127136

137+
BagLogger::BagLoggerLevel level_;
138+
139+
bool enabled_ {false};
140+
128141
std::vector<double> cpu_loads_;
129142

143+
std::string system_status_topic_ = "/resource_status";
144+
130145
am::MemoryInfo mi;
131146

132147
std::vector<am::CpuInfo> cpu_infos_;
@@ -148,11 +163,11 @@ class ResourceStatus
148163

149164
void timerCB();
150165

151-
void checkNodeNames();
166+
void checkNodeNames(brain_box_msgs::msg::SystemReport &sys_report);
152167

153-
void checkTransforms();
168+
void checkTransforms(brain_box_msgs::msg::SystemReport &sys_report);
154169

155-
void checkSensorIPs();
170+
void checkSensorIPs(brain_box_msgs::msg::SystemReport &sys_report);
156171
};
157172
}
158173

‎src/resource_monitor/resource_status_class.cpp‎

Lines changed: 111 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,13 @@ namespace am
1111
{
1212
ResourceStatus::ResourceStatus(std::shared_ptr<am::ResourceMonitorStats> stats) : stats_(stats)
1313
{
14+
15+
sys_rep_pub_ = am::Node::node->create_publisher<brain_box_msgs::msg::SystemReport>(system_status_topic_,1);
16+
17+
log_ctrl_sub_ = am::Node::node->create_subscription<brain_box_msgs::msg::LogControl>("/ctrl/log_control", 1, std::bind(&ResourceStatus::logCtrlCB, this, std::placeholders::_1));
18+
19+
level_ = BagLogger::OFF;
20+
1421
transformer_ = std::make_shared<am::Transformer>();
1522

1623
sub_nets_add_ = getInetAddresses();
@@ -37,6 +44,42 @@ ResourceStatus::~ResourceStatus()
3744

3845
}
3946

47+
void ResourceStatus::logCtrlCB(const brain_box_msgs::msg::LogControl::SharedPtr msg)
48+
{
49+
if (msg->enable)
50+
{
51+
ROS_INFO_STREAM(am::Node::node->get_name() << ": start logging to RS, level " << (int)msg->level);
52+
BagLogger::instance()->startLogging("RS", msg->level);
53+
enabled_ = true;
54+
switch (msg->level)
55+
{
56+
case 1:
57+
level_ = BagLogger::NORM;
58+
break;
59+
case 2:
60+
level_ = BagLogger::FINE;
61+
break;
62+
case 3:
63+
level_ = BagLogger::EXTRA;
64+
break;
65+
case 4:
66+
level_ = BagLogger::ALL;
67+
break;
68+
default:
69+
level_ = BagLogger::OFF;
70+
break;
71+
}
72+
}
73+
else
74+
{
75+
ROS_INFO_STREAM(am::Node::node->get_name() << ": stop logging");
76+
BagLogger::instance()->stopLogging();
77+
enabled_ = false;
78+
level_ = BagLogger::OFF;
79+
80+
}
81+
}
82+
4083
void ResourceStatus::getParams()
4184
{
4285

@@ -152,14 +195,19 @@ am::CpuInfo ResourceStatus::parseCpuLine(const std::string& line)
152195
return info;
153196
}
154197

155-
void ResourceStatus::updateInfos()
198+
void ResourceStatus::updateInfos(brain_box_msgs::msg::SystemReport &sys_report)
156199
{
157200
getMemoryInfo();
201+
sys_report.mem_free_percentage = (float)(100 - mi.used_percent);
202+
sys_report.mem_total_mb = (float)mi.total;
203+
sys_report.mem_used_percentage = (float)mi.used_percent;
204+
sys_report.cores_count = cpu_cnt_;
158205
if(cpu_cnt_ < 0)
159206
{
160207
cpu_cnt_ = getCPUCoresCount();
161208
if(cpu_cnt_ < 0)
162209
{
210+
sys_report.error_msg += ", Cannot get CPU count";
163211
ROS_ERROR("Cannot get CPU count");
164212
return;
165213
}
@@ -182,20 +230,25 @@ void ResourceStatus::updateInfos()
182230
double load = calculateCpuLoad(cpu_infos_[i], cpu_infos_old_[i]);
183231
avg_load+=load;
184232
cpu_loads_[i] = load;
233+
sys_report.cpu_percent_cores.push_back(load);
185234
}
186235

187236
if(cpu_cnt_ > 0)
188237
{
189238
avg_load = avg_load/cpu_cnt_;
239+
sys_report.cpu_percent_total = avg_load;
190240
stats_->cpu_stats = (avg_load > 80.0?100:50);
191241
}
192242

193243

194244
uptime_seconds_ = getUpTime();
245+
sys_report.uptime_seconds = uptime_seconds_;
195246

196247
cpu_infos_old_ = cpu_infos_;
197248

198249
gpu_info_ = getGPUInfo();
250+
sys_report.gpu_used_percentage = (float)gpu_info_.load_percent;
251+
sys_report.gpu_temp = (float)gpu_info_.temp;
199252
stats_->gpu_stats = 50;
200253
//ROS_INFO("GPU Load Percent: %d , Temp: %d", gpu_info_.load_percent, gpu_info_.temp);
201254
if(gpu_info_.load_percent > 90)
@@ -207,11 +260,14 @@ void ResourceStatus::updateInfos()
207260

208261
//check the drive stats
209262
stats_->drive_stats = 50;
263+
int coef = 1024*1024;
210264
am::DiskInfo disk_info = getDiskInfo();
265+
sys_report.hd_used_percentage = disk_info.percentUsed;
266+
sys_report.hd_used = (float)(disk_info.usedSpace/coef);
211267
if(disk_info.percentUsed > 98.0)
212268
{
213269
stats_->drive_stats = 100;
214-
int coef = 1024*1024;
270+
215271
ROS_ERROR("Disk total: %lld MB, available: %lld MB, used: %lld MB, percentage: %f", (disk_info.totalSpace/coef), (disk_info.availableSpace/coef), (disk_info.usedSpace/coef), disk_info.percentUsed);
216272
}
217273

@@ -478,11 +534,12 @@ void ResourceStatus::print()
478534
ROS_INFO("%s", msg.c_str());
479535
}
480536

481-
void ResourceStatus::checkNodeNames()
537+
void ResourceStatus::checkNodeNames(brain_box_msgs::msg::SystemReport &sys_report)
482538
{
483539
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph = am::Node::node->get_node_graph_interface();
484540

485541
std::vector<std::string> running_nodes = node_graph->get_node_names();
542+
sys_report.nodes = running_nodes;
486543

487544
std::unordered_map<std::string, int> string_count;
488545

@@ -497,36 +554,48 @@ void ResourceStatus::checkNodeNames()
497554
}
498555

499556
// Collect strings that appear more than once
557+
std::string error_msg = "";
500558
bool node_check = true;
501559
for (const auto& [str, count] : string_count)
502560
{
503561
if (count > 1)
504562
{
505-
ROS_ERROR("Found a duplicate Node: %s", str.c_str());
506-
node_check = false;
563+
error_msg += " Found a duplicate Node: " + str;
564+
ROS_ERROR("%s", error_msg.c_str());
565+
sys_report.error_msg += error_msg;
507566
}
508567
}
509568
stats_->node_stats = (node_check?50:100);
510569
}
511570

512-
void ResourceStatus::checkTransforms()
571+
void ResourceStatus::checkTransforms(brain_box_msgs::msg::SystemReport &sys_report)
513572
{
514573
bool tf_check = true;
515574
for(std::pair<std::string, std::string> &tf_str : transform_list_)
516575
{
517576
geometry_msgs::msg::TransformStamped transform;
577+
brain_box_msgs::msg::TransformStatus tf_status_msg;
578+
tf_status_msg.source = tf_str.first;
579+
tf_status_msg.target = tf_str.second;
580+
tf_status_msg.status = brain_box_msgs::msg::TransformStatus::CONNECTED;
518581
if(!transformer_->getTransform(tf_str.first, tf_str.second, transform, 1.0, false))
519582
{
583+
std::string error_msg = std::string(", Transform tree is broken between: ") + tf_str.first + " and " + tf_str.second;
520584
//ROS_ERROR("Transform tree is broken: %s, %s", tf_str.first.c_str(), tf_str.second.c_str());
521585
tf_check = false;
586+
tf_status_msg.status = brain_box_msgs::msg::TransformStatus::DISCONNECTED;
587+
sys_report.error_msg += error_msg;
522588
}
589+
590+
sys_report.tf_tree_status.push_back(tf_status_msg);
523591
}
524592
stats_->tf_stats = (tf_check?50:100);
525593
}
526594

527-
void ResourceStatus::checkSensorIPs()
595+
void ResourceStatus::checkSensorIPs(brain_box_msgs::msg::SystemReport &sys_report)
528596
{
529597

598+
530599
//IP Address Check
531600
stats_->lidar_ip = 50;
532601
stats_->fl_ip = 50;
@@ -539,36 +608,53 @@ void ResourceStatus::checkSensorIPs()
539608
std::map<std::string, std::string>::iterator it = ip_addresses_.begin();
540609
for(; it != ip_addresses_.end(); ++it)
541610
{
542-
611+
brain_box_msgs::msg::IPSensorStatus ip_sensor_msg;
612+
ip_sensor_msg.ip = it->first;
613+
ip_sensor_msg.alias= it->second;
614+
ip_sensor_msg.status = brain_box_msgs::msg::IPSensorStatus::CONNECTED;
543615
if(!isReachable(it->first))
544616
{
617+
ip_sensor_msg.status = brain_box_msgs::msg::IPSensorStatus::DISCONNECTED;
618+
std::string error_msg;
545619
//THE DEVICE CANNOT BE REACHED
546620
if(it->second == "lidar")
547621
{
622+
548623
stats_->lidar_ip = 100;
549-
ROS_ERROR("Lidar is not reachable");
624+
error_msg = " Lidar is not reachable";
625+
sys_report.error_msg += error_msg;
626+
ROS_ERROR("%s", error_msg.c_str());
550627
}
551628
if(it->second == "front_left")
552629
{
553630
stats_->fl_ip = 100;
554-
ROS_ERROR("Front Left Camera is not reachable");
631+
error_msg = " Front Left is not reachable";
632+
sys_report.error_msg += error_msg;
633+
ROS_ERROR("%s", error_msg.c_str());
555634
}
556635
if(it->second == "front_right")
557636
{
558637
stats_->fr_ip = 100;
559-
ROS_ERROR("Front Right Camera is not reachable");
638+
error_msg = " Front Right is not reachable";
639+
sys_report.error_msg += error_msg;
640+
ROS_ERROR("%s", error_msg.c_str());
560641
}
561642
if(it->second == "rear_right")
562643
{
563644
stats_->rr_ip = 100;
564-
ROS_ERROR("Rear Right Camera is not reachable");
645+
error_msg = " Rear Right Left is not reachable";
646+
sys_report.error_msg += error_msg;
647+
ROS_ERROR("%s", error_msg.c_str());
565648
}
566649
if(it->second == "rear_left")
567650
{
568651
stats_->rl_ip = 100;
569-
ROS_ERROR("Rear Left Camera is not reachable");
652+
error_msg = " Rear Left is not reachable";
653+
sys_report.error_msg += error_msg;
654+
ROS_ERROR("%s", error_msg.c_str());
570655
}
571656
}
657+
sys_report.ip_sensors.push_back(ip_sensor_msg);
572658
}
573659
}
574660
}
@@ -665,17 +751,25 @@ DiskInfo ResourceStatus::getDiskInfo(const std::string& path)
665751
*/
666752
void ResourceStatus::timerCB()
667753
{
754+
755+
brain_box_msgs::msg::SystemReport sr_msg;
756+
sr_msg.header.stamp = am::ClockNow();
757+
668758
//Checking the repeated node name
669-
checkNodeNames();
759+
checkNodeNames(sr_msg);
670760

671761
//Transform check
672-
checkTransforms();
762+
checkTransforms(sr_msg);
673763

674764
//sensor ip check
675-
checkSensorIPs();
765+
checkSensorIPs(sr_msg);
676766

677767
//Resource Check
678-
updateInfos();
768+
updateInfos(sr_msg);
769+
770+
sys_rep_pub_->publish(sr_msg);
771+
772+
LOG_MSG(system_status_topic_, sr_msg, 2);
679773

680774
}
681775
}

0 commit comments

Comments
 (0)