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>

NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py

@@ -1,5 +1,8 @@
 #!/usr/bin/env python3
-import rospy
+import sys
+
+import rclpy
+from rclpy.duration import Duration
 from remote_control_lib import RemoteControl
 from sensor_msgs.msg import Joy
 
@@ -32,11 +35,11 @@ class Joystick:
     """
 
     def __init__(self):
-        self.force_scale = rospy.get_param("/joystick_wrench/force_scale", 600)
-        self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale", 500)
+        self.force_scale = self.declare_parameter("/joystick_wrench/force_scale", 600)
+        self.torque_scale = self.declare_parameter("/joystick_wrench/torque_scale", 500)
 
         self.remote = RemoteControl("emergency", "/wrench/emergency")
-        rospy.Subscriber("joy_emergency", Joy, self.joy_recieved)
+        self.create_subscription(Joy, "joy_emergency", self.joy_recieved)
 
         self.active = False
         self.reset()
@@ -80,15 +83,16 @@ def check_for_timeout(self, joy: Joy):
             # No change in state
             # The controller times out after 15 minutes
             if (
-                rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60)
+                self.get_clock().now() - self.last_joy.header.stamp
+                > Duration(seconds=(15 * 60))
                 and self.active
             ):
-                rospy.logwarn("Controller Timed out. Hold start to resume.")
+                self.get_logger().warn("Controller Timed out. Hold start to resume.")
                 self.reset()
 
         else:
             joy.header.stamp = (
-                rospy.Time.now()
+                self.get_clock().now()
             )  # In the sim, stamps weren't working right
             self.last_joy = joy
 
@@ -102,7 +106,7 @@ def joy_recieved(self, joy: Joy) -> None:
         Args:
             joy (Joy): The Joy message.
         """
-        self.last_time = rospy.Time.now()
+        self.last_time = self.get_clock().now()
         self.check_for_timeout(joy)
 
         # Assigns readable names to the buttons that are used
@@ -116,14 +120,14 @@ def joy_recieved(self, joy: Joy) -> None:
         thruster_deploy = bool(joy.buttons[5])
 
         if go_inactive and not self.last_go_inactive:
-            rospy.loginfo("Go inactive pressed. Going inactive")
+            self.get_logger().info("Go inactive pressed. Going inactive")
             self.reset()
             return
 
         # Reset controller state if only start is pressed down about 1 seconds
         self.start_count += start
         if self.start_count > 5:
-            rospy.loginfo("Resetting controller state")
+            self.get_logger().info("Resetting controller state")
             self.reset()
             self.active = True
 
@@ -170,19 +174,22 @@ def joy_recieved(self, joy: Joy) -> None:
         rotation = joy.axes[3] * self.torque_scale
         self.remote.publish_wrench(x, y, rotation, joy.header.stamp)
 
-    def die_check(self, _: rospy.timer.TimerEvent) -> None:
+    def die_check(self, _: rclpy.timer.TimerEvent) -> None:
         """
         Publishes zeros after 2 seconds of no update in case node dies.
         """
         # No new instructions after 2 seconds
-        if self.active and rospy.Time.now() - self.last_time > rospy.Duration(2):
+        if self.active and self.get_clock().now() - self.last_time > Duration(
+            seconds=2,
+        ):
             # Zero the wrench, reset
             self.reset()
 
 
 if __name__ == "__main__":
-    rospy.init_node("emergency")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("emergency")
 
     emergency = Joystick()
-    rospy.Timer(rospy.Duration(1), emergency.die_check, oneshot=False)
-    rospy.spin()
+    node.create_timer(Duration(seconds=1.0), emergency.die_check)
+    rclpy.spin(node)

NaviGator/utils/remote_control/navigator_joystick_control/nodes/navigator_joystick.py

@@ -1,5 +1,8 @@
 #!/usr/bin/env python3
-import rospy
+import sys
+
+import rclpy
+from rclpy.duration import Duration
 from remote_control_lib import RemoteControl
 from sensor_msgs.msg import Joy
 
@@ -11,12 +14,12 @@
 
 class Joystick:
     def __init__(self):
-        self.force_scale = rospy.get_param("~force_scale", 600)
-        self.torque_scale = rospy.get_param("~torque_scale", 500)
+        self.force_scale = self.declare_parameter("~force_scale", 600)
+        self.torque_scale = self.declare_parameter("~torque_scale", 500)
 
         self.remote = RemoteControl("joystick", "/wrench/rc")
         self.reset()
-        rospy.Subscriber("joy", Joy, self.joy_recieved)
+        self.create_subscription(Joy, "joy", self.joy_recieved)
 
     def reset(self):
         """
@@ -49,15 +52,16 @@ def check_for_timeout(self, joy):
         if (
             joy.axes == self.last_joy.axes
             and joy.buttons == self.last_joy.buttons
-            and rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60)
+            and self.get_clock().now() - self.last_joy.header.stamp
+            > Duration(seconds=(15 * 60))
             and self.active
         ):
-            rospy.logwarn("Controller Timed out. Hold start to resume.")
+            self.get_logger().warn("Controller Timed out. Hold start to resume.")
             self.reset()
 
         else:
             joy.header.stamp = (
-                rospy.Time.now()
+                self.get_clock().now()
             )  # In the sim, stamps weren't working right
             self.last_joy = joy
 
@@ -78,14 +82,14 @@ def joy_recieved(self, joy):
         thruster_deploy = bool(joy.buttons[5])  # RB
 
         if back and not self.last_back:
-            rospy.loginfo("Back pressed. Going inactive")
+            self.get_logger().info("Back pressed. Going inactive")
             self.reset()
             return
 
         # Reset controller state if only start is pressed down about 1 second
         self.start_count += start
         if self.start_count > 5:
-            rospy.loginfo("Resetting controller state")
+            self.get_logger().info("Resetting controller state")
             self.reset()
             self.active = True
 
@@ -143,11 +147,13 @@ def joy_recieved(self, joy):
         x = joy.axes[1] * self.force_scale
         y = joy.axes[0] * self.force_scale
         rotation = joy.axes[3] * self.torque_scale
-        self.remote.publish_wrench(x, y, rotation, rospy.Time.now())
+        self.remote.publish_wrench(x, y, rotation, self.get_clock().now())
 
 
 if __name__ == "__main__":
-    rospy.init_node("joystick")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("joystick")
 
     joystick = Joystick()
-    rospy.spin()
+
+    rclpy.spin(node)