@@ -14,14 +14,32 @@ LinuxThermalZoneBaseNode::LinuxThermalZoneBaseNode(const std::string & node_name
1414{
1515 RCLCPP_INFO_STREAM (this ->get_logger (), " default constructor executed"
1616
17- timer_1s_ =
18- this ->create_wall_timer (1s, std::bind (&LinuxThermalZoneBaseNode::timer_1s_callback, this ));
19- timer_10s_ =
20- this ->create_wall_timer (10s, std::bind (&LinuxThermalZoneBaseNode::timer_10s_callback, this ));
17+ // ros parameters
18+ auto rate_param_desc = rcl_interfaces::msg::ParameterDescriptor{};
19+ rate_param_desc.description = " Frequency (rate in Hz) of type: double"
20+ this ->declare_parameter (" data_pub_rate_hz" 1.0 , rate_param_desc);
21+ this ->declare_parameter (" hk_pub_rate_hz" 0.1 , rate_param_desc);
22+ this ->declare_parameter (" data_acquisition_rate_hz" 0.5 , rate_param_desc);
23+ params.data_pub_rate_hz_ = this ->get_parameter (" data_pub_rate_hz" as_double ();
24+ params.hk_pub_rate_hz_ = this ->get_parameter (" hk_pub_rate_hz" as_double ();
25+ params.data_acquisition_rate_hz_ = this ->get_parameter (" data_acquisition_rate_hz" as_double ();
26+
27+ // threads
28+ data_acquisition_thread_ =
29+ std::thread (std::bind (&LinuxThermalZoneBaseNode::data_acquisition_thread, this ));
30+
31+ // timers
32+ data_pub_timer_ = this ->create_wall_timer (
33+ std::chrono::milliseconds (static_cast <int64_t >(1000.0 / params.data_pub_rate_hz_ )),
34+ std::bind (&LinuxThermalZoneBaseNode::data_pub_timer_callback, this ));
35+ hk_pub_timer_ = this ->create_wall_timer (
36+ std::chrono::milliseconds (static_cast <int64_t >(1000.0 / params.hk_pub_rate_hz_ )),
37+ std::bind (&LinuxThermalZoneBaseNode::hk_pub_timer_callback, this ));
2138 RCLCPP_INFO_STREAM (this ->get_logger (), " timers created"
2239
2340 uint8_t num_thermal_zones = CountMatchingDirectories (" thermal_zone"
2441
42+ // publishers
2543 for (uint8_t zone_index = 0 ; zone_index < num_thermal_zones; zone_index++) {
2644 std::string zone_string = " thermal_zone" std::to_string (zone_index);
2745 publishers_linux_thermal_zone_.push_back (
@@ -37,28 +55,48 @@ LinuxThermalZoneBaseNode::LinuxThermalZoneBaseNode(const std::string & node_name
3755LinuxThermalZoneBaseNode::~LinuxThermalZoneBaseNode ()
3856{
3957 RCLCPP_INFO_STREAM (this ->get_logger (), " destructor executed"
58+
59+ data_acquisition_thread_.join ();
4060}
4161
4262// PROTECTED FUNCTIONS
4363
4464// PRIVATE FUNCTIONS
4565
46- void LinuxThermalZoneBaseNode::timer_1s_callback ()
66+ void LinuxThermalZoneBaseNode::data_acquisition_thread (void )
67+ {
68+ rclcpp::Rate rate (params.data_acquisition_rate_hz_ );
69+ while (rclcpp::ok ()) {
70+ RCLCPP_INFO_STREAM (
71+ this ->get_logger (),
72+ " data_acquisition_thread executed on thread id: " std::this_thread::get_id ());
73+ auto msgs = GetZoneMsgVector ();
74+ std::unique_lock<std::mutex> lock (linux_thermal_zone_msgs_mutex_);
75+ linux_thermal_zone_msgs_ = msgs;
76+ lock.unlock (); // unlock the mutex explicitly
77+ rate.sleep (); // sleep to maintain the loop rate
78+ }
79+ }
80+
81+ void LinuxThermalZoneBaseNode::data_pub_timer_callback ()
4782{
48- RCLCPP_DEBUG_STREAM (this ->get_logger (), " timer_1s_callback executed"
49- std::vector<linux_thermal_zone_interfaces::msg::LinuxThermalZone> msgs = GetZoneMsgVector ();
83+ RCLCPP_DEBUG_STREAM (this ->get_logger (), " data_pub_timer_callback executed"
5084
5185 RCLCPP_INFO_STREAM (
52- this ->get_logger (), " Publishing: " size () << " LinuxThermalZone messages"
86+ this ->get_logger (),
87+ " Publishing: " size () << " LinuxThermalZone messages"
88+
89+ const std::lock_guard<std::mutex> lock (
90+ linux_thermal_zone_msgs_mutex_); // lock until end of scope
5391 for (uint8_t index = 0 ; index < publishers_linux_thermal_zone_.size (); index ++) {
54- publishers_linux_thermal_zone_.at (index )->publish (msgs .at (index ));
92+ publishers_linux_thermal_zone_.at (index )->publish (linux_thermal_zone_msgs_ .at (index ));
5593 linux_thermal_zone_pub_count_++;
5694 }
5795}
5896
59- void LinuxThermalZoneBaseNode::timer_10s_callback ()
97+ void LinuxThermalZoneBaseNode::hk_pub_timer_callback ()
6098{
61- RCLCPP_DEBUG_STREAM (this ->get_logger (), " timer_10s_callback executed"
99+ RCLCPP_DEBUG_STREAM (this ->get_logger (), " hk_pub_timer_callback executed"
62100 linux_thermal_zone_interfaces::msg::LinuxThermalZoneBaseNodeHk message;
63101 message.set__linux_thermal_zone_publish_count (linux_thermal_zone_pub_count_);
64102 publisher_node_hk_->publish (message);
@@ -107,11 +145,15 @@ linux_thermal_zone_interfaces::msg::LinuxThermalZone LinuxThermalZoneBaseNode::G
107145 std::string id = key + std::to_string (zone_index);
108146 std::string thermal_zone_dir = prefix + id;
109147
148+ // header
110149 msg.header .set__stamp (now ());
150+ msg.header .set__frame_id (id);
111151 // temperature
112152 msg.temperature .header .set__stamp (now ());
113153 msg.temperature .header .set__frame_id (id);
114154 msg.temperature .temperature = GetZoneTemperature (thermal_zone_dir);
155+ // type
156+ msg.type = GetZoneString (thermal_zone_dir + " /type"
115157
116158 return msg;
117159}
@@ -146,3 +188,29 @@ double LinuxThermalZoneBaseNode::GetZoneTemperature(std::string thermal_zone_dir
146188
147189 return temperature;
148190}
191+
192+ std::string LinuxThermalZoneBaseNode::GetZoneString (std::string filepath)
193+ {
194+ std::ifstream file (filepath);
195+ std::string return_string;
196+
197+ if (file.is_open ()) {
198+ std::string line;
199+ std::getline (file, line); // Read the first line (assuming it contains the value)
200+ file.close ();
201+
202+ // Copy string to return and handle errors
203+ try {
204+ return_string = line;
205+ RCLCPP_DEBUG_STREAM (this ->get_logger (), filepath << " value: "
206+ } catch (const std::invalid_argument & e) {
207+ RCLCPP_WARN_STREAM (this ->get_logger (), " Invalid type data in file: "
208+ } catch (const std::out_of_range & e) {
209+ RCLCPP_WARN_STREAM (this ->get_logger (), " Type value out of range in file: "
210+ }
211+ } else {
212+ std::cerr << " Failed to open file "
213+ }
214+
215+ return return_string;
216+ }
0 commit comments