Skip to content

Commit

Permalink
Refined alarm handlers
Browse files Browse the repository at this point in the history
got rid of if main == name
  • Loading branch information
useyourshadow authored Jul 27, 2024
1 parent 5123c64 commit 6b7a9fa
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 0 deletions.
39 changes: 39 additions & 0 deletions mil_common/ros_alarms/src/ros_alarms/alarm_node.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
#!/usr/bin/env python3

import rospy

from std_msgs.msg import Float32

global alarm_pub

def cpu_temp_callback(temp):

# Sets of cpu temp alarm
if float(temp.data) > 75:
rospy.logwarn(f"CPU TEMPERATURE EXCEEDS: {temp.data}")
alarm_pub.publish(True)

else:
alarm_pub.publish(False)



def cpu_temp_alarm():
global alarm_pub

# Initializing node
rospy.init_node('cpu_temp_alarm', anonymous=True)

rospy.Subscriber('cpu_temperature', Float32, cpu_temp_callback)
alarm_pub = rospy.Publisher('cpu_alarm', bool, queue_size=10)

rospy.spin()



if __name__ == '__main__':

try:
cpu_temp_alarm()
except rospy.InterruptedError:
pass
42 changes: 42 additions & 0 deletions mil_common/ros_alarms/src/ros_alarms/cpu_temp_alarm.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
#!/usr/bin/env python3

import psutil
import rospy
from std_msgs.msg import Float32, Bool

def get_cpu_temp():
# Checks computer sensors
temp = psutil.sensors_temperatures()
if 'coretemp' in temp:
return temp['coretemp'][0].current
else:
raise ValueError("Core temp not found!")

def cpu_temp_callback(alarm):
if alarm.data:
rospy.logwarn("ALARM: CPU temperature exceeds the threshold!")

def cpu_temp_ROS():
# ROS initializing
rospy.init_node('cpu_temp_node', anonymous=True)
temp_pub = rospy.Publisher('cpu_temperature', Float32, queue_size=10)
rospy.Subscriber('cpu_alarm', Bool, cpu_temp_callback)
rate = rospy.Rate(1)

# Loops through and actively gets CPU temp
while not rospy.is_shutdown():
try:
cpu_temp = get_cpu_temp()
rospy.loginfo("CPU TEMP: %f", cpu_temp)
temp_pub.publish(cpu_temp)
except ValueError as e:
rospy.logwarn(str(e))
rate.sleep()

# if __name__ == '__main__':
# try:
# cpu_temp_ROS()
# except rospy.ROSInterruptException:
# pass


0 comments on commit 6b7a9fa

Please sign in to comment.