From c52711b5cc33fd42feb9bb00f1ebf3643a4647a1 Mon Sep 17 00:00:00 2001 From: Nihar Devireddy <145716162+Nihar3430@users.noreply.github.com> Date: Fri, 29 Mar 2024 21:20:20 -0400 Subject: [PATCH] navigator_battery_monitor package migrated to ros2 (#1167) --- .../nodes/navigator_battery_monitor.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py b/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py index 13876a8ef..a7f03a5dd 100755 --- a/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py +++ b/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py @@ -6,8 +6,10 @@ """ +import sys + import message_filters -import rospy +import rclpy from roboteq_msgs.msg import Feedback, Status from ros_alarms import AlarmListener from ros_alarms_msgs.msg import Alarm @@ -49,7 +51,7 @@ def __init__(self): self._hw_kill_listener.wait_for_server() # The publisher for the averaged voltage - self.pub_voltage = rospy.Publisher("/battery_monitor", Float32, queue_size=1) + self.pub_voltage = node.create_publisher(Float32, "/battery_monitor", 1) # Subscribes to the feedback from each of the four thrusters motor_topics = ["/FL_motor", "/FR_motor", "/BL_motor", "/BR_motor"] @@ -117,7 +119,8 @@ def publish_voltage(self, _) -> None: if __name__ == "__main__": - rospy.init_node("battery_monitor") + rclpy.init(args=sys.argv) + node = rclpy.create_node("battery_monitor") monitor = BatteryMonitor() - rospy.Timer(rospy.Duration(1), monitor.publish_voltage, oneshot=False) - rospy.spin() + node.create_timer(1.0, monitor.publish_voltage) + rclpy.spin()