sub8_thrust_and_kill_board, NaviGator/mission_control/navigator_alarm migration, navigator_emergency_control, navigator_joystick_control, and navigator_keyboard_control to ROS2

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

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},
             )

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,

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):

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
         ]
 

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>

SubjuGator/drivers/sub8_thrust_and_kill_board/package.xml

@@ -10,4 +10,7 @@
   <build_depend>mil_usb_to_can</build_depend>
   <build_export_depend>mil_usb_to_can</build_export_depend>
   <exec_depend>mil_usb_to_can</exec_depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
 </package>

SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py

@@ -1,9 +1,9 @@
 #!/usr/bin/python3
-import rospy
+import rclpy
 from mil_usb_to_can.sub8 import CANDeviceHandle
+from rclpy.timer import TimerEvent
 from ros_alarms import AlarmBroadcaster, AlarmListener
 from ros_alarms_msgs.msg import Alarm
-from rospy.timer import TimerEvent
 from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse
 from subjugator_msgs.msg import Thrust
 
@@ -27,7 +27,7 @@ def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
         # Initialize thruster mapping from params
         self.thrusters, self.name_to_id = make_thruster_dictionary(
-            rospy.get_param("/thruster_layout/thrusters"),
+            self.declare_parameter("/thruster_layout/thrusters").value,
         )
         # Tracks last hw-kill alarm update
         self._last_hw_kill = None
@@ -43,19 +43,19 @@ def __init__(self, *args, **kwargs):
             callback_funct=self.on_hw_kill,
         )
         # Provide service for alarm handler to set/clear the motherboard kill
-        self._unkill_service = rospy.Service(
-            "/set_mobo_kill",
+        self._unkill_service = self.create_service(
             SetBool,
+            "/set_mobo_kill",
             self.set_mobo_kill,
         )
         # Sends heartbeat to board
-        self._heartbeat_timer = rospy.Timer(rospy.Duration(0.4), self.send_heartbeat)
+        self._heartbeat_timer = rclpy.create_timer(0.4, self.send_heartbeat)
         # Create a subscribe for thruster commands
-        self._sub = rospy.Subscriber(
-            "/thrusters/thrust",
+        self._sub = self.create_subscription(
             Thrust,
+            "/thrusters/thrust",
             self.on_command,
-            queue_size=10,
+            10,
         )
 
     def set_mobo_kill(self, req: SetBoolRequest) -> SetBoolResponse:
@@ -108,7 +108,7 @@ def on_command(self, msg: Thrust) -> None:
         for cmd in msg.thruster_commands:
             # If we don't have a mapping for this thruster, ignore it
             if cmd.name not in self.thrusters:
-                rospy.logwarn(
+                self.get_logger().warn(
                     f"Command received for {cmd.name}, but this is not a thruster.",
                 )
                 continue

SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py

@@ -1,6 +1,7 @@
 #!/usr/bin/python3
-import rospy
+import rclpy
 from mil_usb_to_can.sub8 import SimulatedCANDevice
+from rclpy.duration import Duration
 from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse
 
 from .packets import (
@@ -27,7 +28,7 @@ class ThrusterAndKillBoardSimulation(SimulatedCANDevice):
         soft_kill_mobo (bool): Whether the motherboard experienced a soft kill request.
     """
 
-    HEARTBEAT_TIMEOUT_SECONDS = rospy.Duration(1.0)
+    HEARTBEAT_TIMEOUT_SECONDS = Duration(seconds=1.0)
 
     def __init__(self, *args, **kwargs):
         self.hard_kill_plug_pulled = False
@@ -37,18 +38,18 @@ def __init__(self, *args, **kwargs):
         self.go_button = False
         self._last_heartbeat = None
         super().__init__(*args, **kwargs)
-        self._update_timer = rospy.Timer(rospy.Duration(1), self.send_updates)
-        self._soft_kill = rospy.Service(
-            "/simulate_soft_kill",
+        self._update_timer = rclpy.create_timer(1.0, self.send_updates)
+        self._soft_kill = self.create_service(
             SetBool,
+            "/simulate_soft_kill",
             self.set_soft_kill,
         )
-        self._hard_kill = rospy.Service(
-            "/simulate_hard_kill",
+        self._hard_kill = self.create_service(
             SetBool,
+            "/simulate_hard_kill",
             self.set_hard_kill,
         )
-        self._go_srv = rospy.Service("/simulate_go", SetBool, self._on_go_srv)
+        self._go_srv = self.create_service(SetBool, "/simulate_go", self._on_go_srv)
 
     def _on_go_srv(self, req):
         self.go_button = req.data
@@ -102,7 +103,7 @@ def heartbeat_timedout(self) -> bool:
         """
         return (
             self._last_heartbeat is None
-            or (rospy.Time.now() - self._last_heartbeat)
+            or (self.get_clock().now() - self._last_heartbeat)
             > self.HEARTBEAT_TIMEOUT_SECONDS
         )
 
@@ -154,6 +155,6 @@ def on_data(self, data: bytes, can_id: int) -> None:
             packet = ThrustPacket.from_bytes(data)
         elif data[0] == HeartbeatMessage.IDENTIFIER:
             packet = HeartbeatMessage.from_bytes(data)
-            self._last_heartbeat = rospy.Time.now()
+            self._last_heartbeat = self.get_clock().now()
         else:
             raise Exception("No recognized identifier")