navigator/mission_control/navigator_alarm package migrated to ros2

uf-mil · Apr 16, 2024 · 27091f6 · 27091f6
1 parent 4d5ac5f
commit 27091f6
Show file tree

Hide file tree

Showing 7 changed files with 29 additions and 38 deletions.
diff --git a/NaviGator/mission_control/navigator_alarm/CMakeLists.txt b/NaviGator/mission_control/navigator_alarm/CMakeLists.txt
diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py
@@ -1,4 +1,3 @@
-import rospy
 from ros_alarms import AlarmBroadcaster, HandlerBase
 from std_msgs.msg import Float32
 
@@ -8,13 +7,13 @@ class BatteryVoltage(HandlerBase):
 
     def __init__(self):
         self.broadcaster = AlarmBroadcaster(self.alarm_name)
-        self.low_threshold = rospy.get_param("~battery-voltage/low")
-        self.critical_threshold = rospy.get_param("~battery-voltage/critical")
-        self.voltage_sub = rospy.Subscriber(
-            "/battery_monitor",
+        self.low_threshold = self.declare_parameter("~battery-voltage/low")
+        self.critical_threshold = self.declare_parameter("~battery-voltage/critical")
+        self.voltage_sub = self.create_subscription(
             Float32,
+            "/battery_monitor",
             self._check_voltage,
-            queue_size=3,
+            3,
         )
         self._raised = False
         self._severity = 0

diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py
@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import os
 
-import rospy
 from actionlib import SimpleActionClient, TerminalState
 from mil_missions_core import MissionClient
 from mil_msgs.msg import BagOnlineAction, BagOnlineGoal
@@ -25,17 +24,17 @@ def __init__(self):
 
     def _online_bagger_cb(self, status, result):
         if status == 3:
-            rospy.loginfo(f"KILL BAG WRITTEN TO {result.filename}")
+            self.get_logger().info(f"KILL BAG WRITTEN TO {result.filename}")
         else:
-            rospy.logwarn(
+            self.get_logger().warn(
                 f"KILL BAG {TerminalState.to_string(status)}, status: {result.status}",
             )
 
     def _kill_task_cb(self, status, result):
         if status == 3:
-            rospy.loginfo("Killed task success!")
+            self.get_logger().info("Killed task success!")
             return
-        rospy.logwarn(
+        self.get_logger().warn(
             f"Killed task failed ({TerminalState.to_string(status)}): {result.result}",
         )
 
@@ -45,7 +44,9 @@ def raised(self, alarm):
             self.first = False
             return
         if "BAG_ALWAYS" not in os.environ or "bag_kill" not in os.environ:
-            rospy.logwarn("BAG_ALWAYS or BAG_KILL not set. Not making kill bag.")
+            self.get_logger().warn(
+                "BAG_ALWAYS or BAG_KILL not set. Not making kill bag.",
+            )
         else:
             goal = BagOnlineGoal(bag_name="kill.bag")
             goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["BAG_KILL"]
@@ -72,7 +73,7 @@ def meta_predicate(self, meta_alarm, alarms):
             return Alarm(
                 "kill",
                 True,
-                node_name=rospy.get_name(),
+                node_name=self.get_name(),
                 problem_description="Killed by meta alarm(s) " + ", ".join(raised),
                 parameters={"Raised": raised},
             )

diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py
@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 import numpy as np
-import rospy
 from mil_ros_tools import rosmsg_to_numpy
 from nav_msgs.msg import Odometry
 from ros_alarms import AlarmBroadcaster, HandlerBase, HeartbeatMonitor
@@ -32,12 +31,12 @@ def __init__(self):
             prd=self.TIMEOUT_SECONDS,
         )
         self.MAX_JUMP = 0.5
-        self.launch_time = rospy.Time.now()
+        self.launch_time = self.get_clock().now()
         self.last_time = self.launch_time
         self.last_position = None
         self._raised = False
         self.ab = AlarmBroadcaster("odom-kill", node_name="odom-kill")
-        rospy.Subscriber("/odom", Odometry, self.check_continuity, queue_size=5)
+        self.create_subscription(Odometry, "/odom", self.check_continuity, 5)
 
     def check_continuity(self, odom):
         """
@@ -49,7 +48,7 @@ def check_continuity(self, odom):
             jump = np.linalg.norm(position - self.last_position)
             if jump > self.MAX_JUMP and not self._raised:
                 self._raised = True  # Avoid raising multiple times
-                rospy.logwarn("ODOM DISCONTINUITY DETECTED")
+                self.get_logger().warn("ODOM DISCONTINUITY DETECTED")
                 self.ab.raise_alarm(
                     problem_description=f"ODOM DISCONTINUITY DETECTED. JUMPED {jump} METERS",
                     severity=5,

diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/station_hold.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/station_hold.py
@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import rospy
 from actionlib import TerminalState
 from mil_missions_core import MissionClient
 from ros_alarms import AlarmBroadcaster, HandlerBase
@@ -14,18 +13,18 @@ def __init__(self):
 
     def _client_cb(self, terminal_state, result):
         if terminal_state != 3:
-            rospy.logwarn(
+            self.get_logger().warn(
                 "Station hold goal failed (Status={}, Result={})".format(
                     TerminalState.to_string(terminal_state),
                     result.result,
                 ),
             )
             return
-        rospy.loginfo("Station holding!")
+        self.get_logger().info("Station holding!")
         self.broadcaster.clear_alarm()
 
     def raised(self, alarm):
-        rospy.loginfo("Attempting to station hold")
+        self.get_logger().info("Attempting to station hold")
         self.task_client.run_mission("StationHold", done_cb=self._client_cb)
 
     def cleared(self, alarm):

diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py
@@ -1,4 +1,3 @@
-import rospy
 from roboteq_msgs.msg import Status
 from ros_alarms import AlarmBroadcaster, HandlerBase
 
@@ -29,7 +28,12 @@ def __init__(self):
 
         # Subscribe to the status message for all thruster topics
         [
-            rospy.Subscriber(topic + "/status", Status, self._check_faults, topic)
+            self.create_subscription(
+                Status,
+                topic + "/status",
+                self._check_faults,
+                topic,
+            )
             for topic in motor_topics
         ]
 

diff --git a/NaviGator/mission_control/navigator_alarm/package.xml b/NaviGator/mission_control/navigator_alarm/package.xml
@@ -8,6 +8,9 @@
   <buildtool_depend>catkin</buildtool_depend>
   <depend>mil_tools</depend>
   <depend>nav_msgs</depend>
+  <depend>rclpy</depend>
   <depend>ros_alarms</depend>
-  <depend>rospy</depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
 </package>