From f09bc7ca53ae5b41998edea839370687a230b5de Mon Sep 17 00:00:00 2001
From: anvitD <anvitd@SubjuGator.myguest.virtualbox.org>
Date: Thu, 4 Apr 2024 10:59:30 -0400
Subject: [PATCH 1/6] Migrated to ROS2

---
 .../test/navigator_test/nodes/circle_sim.py   | 26 ++++++++++++-------
 1 file changed, 16 insertions(+), 10 deletions(-)

diff --git a/NaviGator/test/navigator_test/nodes/circle_sim.py b/NaviGator/test/navigator_test/nodes/circle_sim.py
index 2cb0d2598..a7fe5ec6e 100755
--- a/NaviGator/test/navigator_test/nodes/circle_sim.py
+++ b/NaviGator/test/navigator_test/nodes/circle_sim.py
@@ -1,10 +1,11 @@
 #!/usr/bin/env python3
+import sys
 from typing import Optional, Sequence
 
 import cv2
 import mil_tools
 import numpy as np
-import rospy
+import rclpy
 import tf.transformations as trns
 from mil_misc_tools.text_effects import fprint as _fprint
 from nav_msgs.msg import OccupancyGrid, Odometry
@@ -27,12 +28,16 @@ class DoOdom:
     """
 
     def __init__(self, rand_size: float):
-        self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
+        self.odom_pub = node.create_publisher(Odometry, "/odom", 2)
         self.odom = None
-        self.carrot_sub = rospy.Subscriber("/trajectory/cmd", Odometry, self.set_odom)
+        self.carrot_sub = node.create_publisher(
+            Odometry,
+            "/trajectory/cmd",
+            self.set_odom,
+        )
 
         fprint("Shaking hands and taking names.")
-        rospy.sleep(1)
+        rclpy.sleep(1)
 
         # We need to publish an initial odom message for lqrrt
         start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
@@ -56,7 +61,7 @@ def __init__(
         min_t_spacing: float = 9,
         num_of_buoys: int = 20,
     ):
-        self.ogrid_pub = rospy.Publisher("/ogrid", OccupancyGrid, queue_size=2)
+        self.ogrid_pub = node.create_publisher("/ogrid", OccupancyGrid, 2)
         self.odom = DoOdom(bf_size)
 
         self.bf_size = bf_size
@@ -84,10 +89,10 @@ def __init__(
         self.publish_ogrid()
 
         # Now set up the database request service
-        rospy.Service("/database/requests", ObjectDBQuery, self.got_request)
-        rospy.Service("/reseed", Trigger, self.reseed)
+        node.create_service(ObjectDBQuery, "/database/requests", self.got_request)
+        node.create_service(Trigger, "/reseed", self.reseed)
 
-        rospy.Timer(rospy.Duration(1), self.publish_ogrid)
+        rclpy.Timer(rclpy.Duration(1), self.publish_ogrid)
 
     def _make_ogrid_transform(self):
         self.grid = np.zeros(
@@ -236,7 +241,8 @@ def draw_totems(self) -> None:
 
 if __name__ == "__main__":
     fprint("Starting", msg_color="blue")
-    rospy.init_node("Sim")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("Sim")
 
     Sim()
-    rospy.spin()
+    rclpy.spin()

From d7aca5a8c7f07db73ca8a17372dececda8fbe8f2 Mon Sep 17 00:00:00 2001
From: anvitD <anvitd@SubjuGator.myguest.virtualbox.org>
Date: Sun, 14 Apr 2024 22:24:26 -0400
Subject: [PATCH 2/6] Migrated to ros2

---
 NaviGator/test/navigator_test/nodes/circle_sim.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/NaviGator/test/navigator_test/nodes/circle_sim.py b/NaviGator/test/navigator_test/nodes/circle_sim.py
index a7fe5ec6e..d1312d406 100755
--- a/NaviGator/test/navigator_test/nodes/circle_sim.py
+++ b/NaviGator/test/navigator_test/nodes/circle_sim.py
@@ -28,9 +28,9 @@ class DoOdom:
     """
 
     def __init__(self, rand_size: float):
-        self.odom_pub = node.create_publisher(Odometry, "/odom", 2)
+        self.odom_pub = self.create_publisher(Odometry, "/odom", 2)
         self.odom = None
-        self.carrot_sub = node.create_publisher(
+        self.carrot_sub = self.create_publisher(
             Odometry,
             "/trajectory/cmd",
             self.set_odom,
@@ -89,8 +89,8 @@ def __init__(
         self.publish_ogrid()
 
         # Now set up the database request service
-        node.create_service(ObjectDBQuery, "/database/requests", self.got_request)
-        node.create_service(Trigger, "/reseed", self.reseed)
+        self.create_service(ObjectDBQuery, "/database/requests", self.got_request)
+        self.create_service(Trigger, "/reseed", self.reseed)
 
         rclpy.Timer(rclpy.Duration(1), self.publish_ogrid)
 

From bfee1adfaefb622d76d2e02d1e823c61060d1364 Mon Sep 17 00:00:00 2001
From: anvitD <anvitd@SubjuGator.myguest.virtualbox.org>
Date: Wed, 17 Apr 2024 18:15:40 -0400
Subject: [PATCH 3/6] migrated mil_tools and mil_poi to ros2

---
 mil_common/axros                              |  2 +-
 mil_common/utils/mil_poi/mil_poi/server.py    | 40 +++++-----
 mil_common/utils/mil_poi/test/test_sync.py    | 58 ++++++++------
 .../utils/mil_tools/mil_misc_tools/times.py   |  8 +-
 .../utils/mil_tools/mil_ros_tools/cv_debug.py |  5 +-
 .../mil_ros_tools/extract_bag_images.py       |  8 +-
 .../utils/mil_tools/mil_ros_tools/fix_bag.py  |  6 +-
 .../mil_tools/mil_ros_tools/image_helpers.py  | 68 ++++++++--------
 .../mil_tools/mil_ros_tools/init_helpers.py   | 25 +++---
 .../mil_tools/mil_ros_tools/label_me_bag.py   |  8 +-
 .../mil_tools/mil_ros_tools/mag_to_marker.py  | 18 +++--
 .../mil_tools/mil_ros_tools/msg_helpers.py    | 11 +--
 .../utils/mil_tools/mil_ros_tools/plotter.py  |  5 +-
 .../mil_tools/mil_ros_tools/rviz_helpers.py   |  9 ++-
 .../mil_ros_tools/vector_to_marker.py         | 19 +++--
 .../mil_tools/nodes/clicked_point_recorder.py | 20 +++--
 .../mil_tools/nodes/network_broadcaster.py    | 28 ++++---
 .../utils/mil_tools/nodes/ogrid_draw.py       | 25 +++---
 .../utils/mil_tools/nodes/online_bagger.py    | 80 +++++++++++--------
 mil_common/utils/mil_tools/nodes/tf_fudger.py | 16 ++--
 .../utils/mil_tools/nodes/tf_to_gazebo.py     | 18 +++--
 .../utils/mil_tools/nodes/video_player.py     | 31 +++----
 .../utils/mil_tools/scripts/param_sever.py    | 14 ++--
 .../utils/mil_tools/test/test_ros_tools.py    |  4 +-
 24 files changed, 290 insertions(+), 236 deletions(-)

diff --git a/mil_common/axros b/mil_common/axros
index d0f12eedb..1b0399935 160000
--- a/mil_common/axros
+++ b/mil_common/axros
@@ -1 +1 @@
-Subproject commit d0f12eedb4622f5bbc5ba4fb63ae350f27a51f4b
+Subproject commit 1b03999351fb5a61b202ff125f493229c2a1676b
diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py
index bf7fee2cd..8bdee09cf 100644
--- a/mil_common/utils/mil_poi/mil_poi/server.py
+++ b/mil_common/utils/mil_poi/mil_poi/server.py
@@ -3,7 +3,7 @@
 from threading import Lock
 from typing import Optional
 
-import rospy
+import rclpy
 import tf2_ros
 from geometry_msgs.msg import Point, PointStamped, Pose, Quaternion
 from interactive_markers.interactive_marker_server import InteractiveMarkerServer
@@ -49,27 +49,27 @@ def __init__(self):
         self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
 
         # Radius of interactive marker for POIs
-        self.marker_scale = rospy.get_param("~marker_scale", 0.5)
+        self.marker_scale = self.declare_parameter("~marker_scale", 0.5)
 
         # Create initial empty POI array
         self.pois = POIArray()
 
         # Get the global frame to be used
-        self.global_frame = rospy.get_param("~global_frame", "map")
+        self.global_frame = self.declare_parameter("~global_frame", "map")
         self.pois.header.frame_id = self.global_frame
 
         # Create publisher to notify clients of updates and interactive marker server
-        self.pub = rospy.Publisher(
-            "points_of_interest",
+        self.pub = self.create_publisher(
             POIArray,
-            queue_size=1,
+            "points_of_interest",
+            1,
             latch=True,
         )
         self.interactive_marker_server = InteractiveMarkerServer("points_of_interest")
 
         # Load initial POIs from params
-        if rospy.has_param("~initial_pois"):
-            pois = rospy.get_param("~initial_pois")
+        if rclpy.has_param("~initial_pois"):
+            pois = self.declare_parameter("~initial_pois")
             assert isinstance(pois, dict)
             for key, value in pois.items():
                 assert isinstance(key, str)
@@ -85,12 +85,16 @@ def __init__(self):
         self.update()
 
         # Create services to add / delete / move a POI
-        self.add_poi_server = rospy.Service("~add", AddPOI, self.add_poi_cb)
-        self.delete_poi_server = rospy.Service("~delete", DeletePOI, self.delete_poi_cb)
-        self.move_poi_service = rospy.Service("~move", MovePOI, self.move_poi_cb)
-        self.save_to_param = rospy.Service(
-            "~save_to_param",
+        self.add_poi_server = self.create_service(AddPOI, "~add", self.add_poi_cb)
+        self.delete_poi_server = self.create_service(
+            DeletePOI,
+            "~delete",
+            self.delete_poi_cb,
+        )
+        self.move_poi_service = self.create_service(MovePOI, "~move", self.move_poi_cb)
+        self.save_to_param = self.create_service(
             Trigger,
+            "~save_to_param",
             self.save_to_param_cb,
         )
 
@@ -114,11 +118,11 @@ def transform_position(self, ps: PointStamped) -> Optional[Point]:
             ps_tf = self.tf_buffer.transform(
                 ps,
                 self.global_frame,
-                timeout=rospy.Duration(5),
+                timeout=rclpy.Duration(5),
             )
             return ps_tf.point
         except tf2_ros.TransformException as e:
-            rospy.logwarn(
+            rclpy.logwarn(
                 'Error transforming "{}" to "{}": {}'.format(
                     ps.header.frame_id,
                     self.global_frame,
@@ -163,7 +167,7 @@ def save_to_param_cb(self, req: TriggerRequest) -> TriggerResponse:
             TriggerResponse: The response from the service; ie, whether the operation
             succeeded.
         """
-        rospy.set_param("~global_frame", self.global_frame)
+        self.declare_parameter("~global_frame", self.global_frame)
         d = {}
         for poi in self.pois.pois:
             d[poi.name] = [
@@ -171,7 +175,7 @@ def save_to_param_cb(self, req: TriggerRequest) -> TriggerResponse:
                 float(poi.position.y),
                 float(poi.position.z),
             ]
-        rospy.set_param("~initial_pois", d)
+        self.declare_parameter("~initial_pois", d)
         return TriggerResponse(success=True)
 
     def add_poi_cb(self, req: AddPOIRequest) -> AddPOIResponse:
@@ -296,7 +300,7 @@ def update(self) -> None:
         """
         Update interactive markers server and POI publish of changes.
         """
-        self.pois.header.stamp = rospy.Time.now()
+        self.pois.header.stamp = rclpy.Time.now()
         self.pub.publish(self.pois)
         self.interactive_marker_server.applyChanges()
 
diff --git a/mil_common/utils/mil_poi/test/test_sync.py b/mil_common/utils/mil_poi/test/test_sync.py
index 661153e4d..cf15dc7b8 100755
--- a/mil_common/utils/mil_poi/test/test_sync.py
+++ b/mil_common/utils/mil_poi/test/test_sync.py
@@ -1,10 +1,11 @@
 #!/usr/bin/env python3
 import random
 import string
+import sys
 import unittest
 
 import genpy
-import rospy
+import rclpy
 import rostest
 from geometry_msgs.msg import Point, PointStamped
 from mil_poi.srv import (
@@ -19,15 +20,18 @@
 class POITest(unittest.TestCase):
     def setUp(self):
         # make one poi here
-        rospy.init_node("poi_test_node")
+        rclpy.init(args=sys.argv)
+
         self.poi_name = "test_poi"
         self.poi_position = PointStamped(header=Header(), point=Point(0.0, 1.0, 2.0))
         self.add_poi()
 
     def add_poi(self):
-        rospy.wait_for_service("/poi_server/add")
-        service = rospy.ServiceProxy("/poi_server/add", AddPOI)
-        response = service(self.poi_name, self.poi_position)
+        service = self.create_client(AddPOI, "/poi_server/add")
+        while not service.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+        response = service.call_async(self.poi_name, self.poi_position)
+        rclpy.spin_until_future_complete(self, response)
         self.assertTrue(response.success)
 
     def test_add(self):
@@ -39,18 +43,19 @@ def test_move(self):
         self.add_poi()
 
         # Wait for the move_poi service to become available
-        rospy.wait_for_service("/poi_server/move")
-        move_service = rospy.ServiceProxy("/poi_server/move", MovePOI)
-
+        move_service = self.create_client(MovePOI, "/poi_server/move")
+        while not move_service.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
         # Move the POI to a new position
         new_position = [1.0, 2.0, 3.0]  # New position coordinates
-        move_response = move_service(
+        move_response = move_service.call_async(
             MovePOIRequest(
                 name=self.poi_name,
                 position=PointStamped(point=Point(*new_position)),
             ),
         )
         # Check if the additions were unsuccessful
+        rclpy.spin_until_future_complete(self, move_response)
         self.assertTrue(move_response.success, f"Failed to move POI '{self.poi_name}'")
 
     def test_delete(self):
@@ -58,11 +63,12 @@ def test_delete(self):
         self.add_poi()
 
         # Wait for the remove_poi service to become available
-        rospy.wait_for_service("/poi_server/delete")
-        remove_service = rospy.ServiceProxy("/poi_server/delete", DeletePOI)
-
+        remove_service = self.create_client(DeletePOI, "/poi_server/delete")
+        while not remove_service.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
         # Remove the added POI
-        remove_response = remove_service(self.poi_name)
+        remove_response = remove_service.call_async(self.poi_name)
+        rclpy.spin_until_future_complete(self, remove_response)
 
         # Check if the removal was successful
         self.assertTrue(
@@ -71,24 +77,27 @@ def test_delete(self):
         )
 
     def test_long_string(self):
-        rospy.wait_for_service("/poi_server/add")
-        service = rospy.ServiceProxy("/poi_server/add", AddPOI)
-
+        service = self.create_client(AddPOI, "/poi_server/add")
+        while not service.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
         # Create a long string for the POI name
         long_string = "".join(random.choices(string.ascii_letters, k=20000))
 
         # Call the service to add a new POI with the long string name
-        response = service(
+        response = service.call_async(
             long_string,
             PointStamped(header=Header(), point=Point(0.0, 1.0, 2.0)),
         )
+        rclpy.spin_until_future_complete(self, response)
         self.assertTrue(response.success, response.message)
 
     def test_wrong_types(self):
         # Wait for the add_poi service to become available
-        rospy.wait_for_service("/poi_server/add")
-        service = rospy.ServiceProxy("/poi_server/add", AddPOI)
-
+        service = self.create_client(AddPOI, "/poi_server/add")
+        while not service.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+        response = service.call_async(self.poi_name, self.poi_position)
+        rclpy.spin_until_future_complete(self, response)
         # Try adding a POI with wrong types of arguments
         with self.assertRaises(genpy.message.SerializationError):
             service(12321, [0.0, 2.3, 21.3])  # Incorrect name type
@@ -99,14 +108,15 @@ def test_delete_twice(self):
         self.add_poi()
 
         # Wait for the remove_poi service to become available
-        rospy.wait_for_service("/poi_server/delete")
-        remove_service = rospy.ServiceProxy("/poi_server/delete", DeletePOI)
+        remove_service = self.create_client(DeletePOI, "/poi_server/delete")
+        while not remove_service.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
 
         # Remove the added POI
-        remove_response1 = remove_service(self.poi_name)
+        remove_response1 = remove_service.call_async(self.poi_name)
 
         # Try removing the same POI again
-        remove_response2 = remove_service(self.poi_name)
+        remove_response2 = remove_service.call_async(self.poi_name)
 
         # Check if the first removal was successful and the second removal was unsuccessful
         self.assertTrue(
diff --git a/mil_common/utils/mil_tools/mil_misc_tools/times.py b/mil_common/utils/mil_tools/mil_misc_tools/times.py
index 554e8fbd8..1666b132c 100644
--- a/mil_common/utils/mil_tools/mil_misc_tools/times.py
+++ b/mil_common/utils/mil_tools/mil_misc_tools/times.py
@@ -1,11 +1,11 @@
 import datetime
 
-import rospy
+import rclpy
 
 
-def rospy_to_datetime(time: rospy.Time) -> datetime.datetime:
+def rospy_to_datetime(time: rclpy.Time) -> datetime.datetime:
     return datetime.datetime.utcfromtimestamp(time.to_sec())
 
 
-def datetime_to_rospy(dt: datetime.datetime) -> rospy.Time:
-    return rospy.Time.from_sec(dt.replace(tzinfo=datetime.timezone.utc).timestamp())
+def datetime_to_rospy(dt: datetime.datetime) -> rclpy.Time:
+    return rclpy.Time.from_sec(dt.replace(tzinfo=datetime.timezone.utc).timestamp())
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py b/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py
index b5695ded9..29ef0d388 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py
@@ -7,7 +7,6 @@
 
 import cv2
 import numpy as np
-import rospy
 from axros import NodeHandle
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
@@ -72,7 +71,7 @@ def __init__(
         self.base_topic = "/debug/"
         self.topic_to_pub = {}
         if nh is None:
-            self.pub = rospy.Publisher("/debug/image", Image, queue_size=10)
+            self.pub = self.create_publisher(Image, "/debug/image", 10)
         else:
             self.pub = nh.advertise("/debug/image", Image)
 
@@ -136,7 +135,7 @@ def _add_new_topic(self, img, name: str, wait: int, topic: str) -> None:
         if topic in list(self.topic_to_pub.keys()):
             pub = self.topic_to_pub[topic]
         elif self.nh is None:
-            pub = rospy.Publisher("/debug/" + topic, Image, queue_size=10)
+            pub = self.create_publisher(Image, "/debug/" + topic, 10)
         elif self.nh is not None:
             pub = self.nh.advertise("/debug/" + topic, Image)
         self.topic_to_pub[topic] = pub
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py b/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py
index 22e52987c..5c7648cc1 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py
@@ -18,8 +18,8 @@
 from typing import Optional
 
 import cv2
+import rclpy
 import rosbag
-import rospy
 import yaml
 from cv_bridge.boost.cv_bridge_boost import cvtColor2
 from genpy import Time
@@ -221,9 +221,9 @@ def extract_images(self, source_dir=".", image_dir=".", verbose=False):
         else:
             self.camera_model = None
         _, _, first_time = next(b.read_messages())
-        start = first_time + rospy.Duration(self.start) if self.start else first_time
-        stop = first_time + rospy.Duration(self.stop) if self.stop else None
-        interval = rospy.Duration(1.0 / self.freq) if self.freq else rospy.Duration(0)
+        start = first_time + rclpy.Duration(self.start) if self.start else first_time
+        stop = first_time + rclpy.Duration(self.stop) if self.stop else None
+        interval = rclpy.Duration(1.0 / self.freq) if self.freq else rclpy.Duration(0)
         next_time = start
         prefix = slugify(str(self.filename)) + "_" + slugify(str(self.topic))
         for _, msg, time in b.read_messages(
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/fix_bag.py b/mil_common/utils/mil_tools/mil_ros_tools/fix_bag.py
index f4796ace9..bc283dd5f 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/fix_bag.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/fix_bag.py
@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 import argparse
 
+import rclpy
 import rosbag
-import rospy
 from tqdm import tqdm
 
 
@@ -46,9 +46,9 @@ def fix_bag(self, infile, outfile=None):
         assert first_time is not None
         start, stop = None, None
         if self.start is not None:
-            start = first_time + rospy.Duration(self.start)
+            start = first_time + rclpy.Duration(self.start)
         if self.stop is not None:
-            stop = first_time + rospy.Duration(self.stop)
+            stop = first_time + rclpy.Duration(self.stop)
         total_messages = bag.get_message_count()
         # This could be made significantly faster by using ag.get_type_and_topic_info
         # to do some preprocessing on what topics will be used / remapped /
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py
index 45e12b931..d87d7f487 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py
@@ -9,9 +9,10 @@
 
 import message_filters
 import numpy as np
-import rospy
+import rclpy
 from cv_bridge import CvBridge, CvBridgeError
 from image_geometry import PinholeCameraModel
+from rclpy.node import Node
 from sensor_msgs.msg import CameraInfo, Image
 
 from .init_helpers import wait_for_param
@@ -23,14 +24,14 @@ def get_parameter_range(parameter_root: str):
     """
     low_param, high_param = parameter_root + "/hsv_low", parameter_root + "/hsv_high"
 
-    rospy.logwarn(f"Blocking -- waiting for parameters {low_param} and {high_param}")
+    rclpy.logwarn(f"Blocking -- waiting for parameters {low_param} and {high_param}")
 
     wait_for_param(low_param)
     wait_for_param(high_param)
-    low = rospy.get_param(low_param)
-    high = rospy.get_param(high_param)
+    low = Node.declare_parameter(low_param)
+    high = Node.declare_parameter(high_param)
 
-    rospy.loginfo(f"Got {low_param} and {high_param}")
+    Node.get_logger.info()(f"Got {low_param} and {high_param}")
     return np.array([low, high]).transpose()
 
 
@@ -69,7 +70,7 @@ class Image_Publisher:
     def __init__(self, topic: str, encoding: str = "bgr8", queue_size: int = 1):
         self.bridge = CvBridge()
         self.encoding = encoding
-        self.im_pub = rospy.Publisher(topic, Image, queue_size=queue_size)
+        self.im_pub = self.create_publisher(Image, topic, queue_size)
 
     def publish(self, cv_image: np.ndarray):
         """
@@ -81,7 +82,7 @@ def publish(self, cv_image: np.ndarray):
             self.im_pub.publish(image_message)
         except CvBridgeError as e:
             # Intentionally absorb CvBridge Errors
-            rospy.logerr(str(e))
+            rclpy.logerr(str(e))
 
 
 class Image_Subscriber:
@@ -117,19 +118,14 @@ def callback(im):
         self.last_image_header = None
         self.last_image_time = None
         self.last_image = None
-        self.im_sub = rospy.Subscriber(
-            topic,
-            Image,
-            self.convert,
-            queue_size=queue_size,
-        )
+        self.im_sub = self.create_subscription(Image, topic, self.convert, queue_size)
 
-        root_topic, image_subtopic = path.split(rospy.remap_name(topic))
-        self.info_sub = rospy.Subscriber(
-            root_topic + "/camera_info",
+        root_topic, image_subtopic = path.split(rclpy.remap_name(topic))
+        self.info_sub = self.create_subscription(
             CameraInfo,
+            root_topic + "/camera_info",
             self.info_cb,
-            queue_size=queue_size,
+            queue_size,
         )
 
         self.bridge = CvBridge()
@@ -146,21 +142,21 @@ def wait_for_camera_info(self, timeout: int = 10):
         Raises:
             Exception: No camera info was found after the timeout had finished.
         """
-        rospy.logwarn(
+        rclpy.logwarn(
             "Blocking -- waiting at most %d seconds for camera info." % timeout,
         )
 
-        timeout = rospy.Duration(timeout)
-        rospy.sleep(0.1)  # Make sure we don't have negative time
-        start_time = rospy.Time.now()
+        timeout = rclpy.Duration(timeout)
+        rclpy.sleep(0.1)  # Make sure we don't have negative time
+        start_time = rclpy.Time.now()
 
-        while (rospy.Time.now() - start_time < timeout) and (not rospy.is_shutdown()):
+        while (rclpy.Time.now() - start_time < timeout) and (not rclpy.is_shutdown()):
             if self.camera_info is not None:
-                rospy.loginfo("Camera info found!")
+                rclpy.loginfo("Camera info found!")
                 return self.camera_info
-            rospy.sleep(0.2)
+            rclpy.sleep(0.2)
 
-        rospy.logerr("Camera info not found.")
+        rclpy.logerr("Camera info not found.")
         raise Exception("Camera info not found.")
 
     def wait_for_camera_model(self, **kwargs):
@@ -208,7 +204,7 @@ def convert(self, data: Image):
             self.callback(image)
         except CvBridgeError as e:
             # Intentionally absorb CvBridge Errors
-            rospy.logerr(e)
+            rclpy.logerr(e)
 
 
 class StereoImageSubscriber:
@@ -292,19 +288,19 @@ def callback(image_left, image_right):
 
         # Subscribe to image and camera info topics for both cameras
         root_topic_left, image_subtopic_left = path.split(left_image_topic)
-        self._info_sub_left = rospy.Subscriber(
-            root_topic_left + "/camera_info",
+        self._info_sub_left = self.create_subscription(
             CameraInfo,
+            root_topic_left + "/camera_info",
             lambda info: setattr(self, "camera_info_left", info),
-            queue_size=queue_size,
+            queue_size,
         )
         image_sub_left = message_filters.Subscriber(left_image_topic, Image)
         root_topic_right, image_subtopic_right = path.split(right_image_topic)
-        self._info_sub_right = rospy.Subscriber(
-            root_topic_right + "/camera_info",
+        self._info_sub_right = self.create_subscription(
             CameraInfo,
+            root_topic_right + "/camera_info",
             lambda info: setattr(self, "camera_info_right", info),
-            queue_size=queue_size,
+            queue_size,
         )
         image_sub_right = message_filters.Subscriber(right_image_topic, Image)
 
@@ -342,14 +338,14 @@ def wait_for_camera_info(
         Raises:
             Exception: if camera info for both cameras is not received within timeout
         """
-        timeout = rospy.Time.now() + rospy.Duration(timeout)
-        while (rospy.Time.now() < timeout) and (not rospy.is_shutdown()):
+        timeout = rclpy.Time.now() + rclpy.Duration(timeout)
+        while (rclpy.Time.now() < timeout) and (not rclpy.is_shutdown()):
             if self.camera_info_left is not None and self.camera_info_right is not None:
                 if unregister:
                     self._info_sub_left.unregister()
                     self._info_sub_right.unregister()
                 return self.camera_info_left, self.camera_info_right
-            rospy.sleep(0.05)
+            rclpy.sleep(0.05)
         if self.camera_info_left is not None and self.camera_info_right is not None:
             if unregister:
                 self._info_sub_left.unregister()
@@ -381,4 +377,4 @@ def _image_callback(self, left_img: Image, right_img: Image):
             self.callback(img_left, img_right)
         except CvBridgeError as e:
             # Intentionally absorb CvBridge Errors
-            rospy.logerr(e)
+            rclpy.logerr(e)
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py
index 789b36153..c1d10653e 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py
@@ -6,8 +6,9 @@
 import time
 from typing import Any, Optional
 
-import rospy
+import rclpy
 import rostest
+from rclpy.node import Node
 
 
 def wait_for_param(
@@ -32,11 +33,11 @@ def wait_for_param(
         the parameter never came to exist.
     """
     start_time = time.time()
-    rate = rospy.Rate(poll_rate)
-    while not rospy.is_shutdown():
+    rate = rclpy.Rate(poll_rate)
+    while not rclpy.is_shutdown():
         # Check if the parameter now exists
-        if rospy.has_param(param_name):
-            return rospy.get_param(param_name)
+        if rclpy.has_param(param_name):
+            return Node.declare_parameter(param_name)
 
         # If we exceed a defined timeout, return None
         if timeout is not None and time.time() - start_time > timeout:
@@ -65,10 +66,10 @@ def wait_for_subscriber(node_name: str, topic: str, timeout: float = 5.0) -> boo
     end_time = time.time() + timeout
 
     # Wait for time-out or ros-shutdown
-    while (time.time() < end_time) and (not rospy.is_shutdown()):
+    while (time.time() < end_time) and (not rclpy.is_shutdown()):
         subscribed = rostest.is_subscriber(
-            rospy.resolve_name(topic),
-            rospy.resolve_name(node_name),
+            rclpy.resolve_name(topic),
+            rclpy.resolve_name(node_name),
         )
         # Success scenario: node subscribes
         if subscribed:
@@ -78,8 +79,8 @@ def wait_for_subscriber(node_name: str, topic: str, timeout: float = 5.0) -> boo
     # Could do this with a while/else
     # But chose to explicitly check
     success = rostest.is_subscriber(
-        rospy.resolve_name(topic),
-        rospy.resolve_name(node_name),
+        rclpy.resolve_name(topic),
+        rclpy.resolve_name(node_name),
     )
     return success
 
@@ -101,8 +102,8 @@ def wait_for_service(
     """
     try:
         service.wait_for_service(warn_time)
-    except rospy.ROSException:
+    except rclpy.ROSException:
         if timeout is not None:
             timeout = timeout - warn_time
-        rospy.logwarn(warn_msg)
+        rclpy.logwarn(warn_msg)
         service.wait_for_service(timeout)
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py b/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py
index aa7f91142..216b6d12a 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py
@@ -23,8 +23,8 @@
 import xml.etree.ElementTree
 
 import cv2
+import rclpy
 import rosbag
-import rospy
 import yaml
 from cv_bridge import CvBridge
 from geometry_msgs.msg import Point
@@ -174,11 +174,11 @@ def _read_bag(self, config):
         # Load start, stop, and frequency from config or defaults
         _, _, first_time = next(bag.read_messages())
         start = (
-            first_time + rospy.Duration(config.start) if config.start else first_time
+            first_time + rclpy.Duration(config.start) if config.start else first_time
         )
-        stop = first_time + rospy.Duration(config.stop) if config.stop else None
+        stop = first_time + rclpy.Duration(config.stop) if config.stop else None
         interval = (
-            rospy.Duration(1.0 / config.freq) if config.freq else rospy.Duration(0)
+            rclpy.Duration(1.0 / config.freq) if config.freq else rclpy.Duration(0)
         )
 
         # Crawl through bag in configured time and frequency, writing images into labelme
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py b/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py
index 00fa3599f..be4e78347 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py
@@ -2,9 +2,10 @@
 from __future__ import annotations
 
 import argparse
+import sys
 
 import numpy as np
-import rospy
+import rclpy
 from mil_tools import numpy_to_point, rosmsg_to_numpy
 from sensor_msgs.msg import MagneticField
 from std_msgs.msg import ColorRGBA
@@ -41,12 +42,12 @@ def __init__(
         if color is None:
             color = [0, 0, 1, 1]
         self.length = length
-        self.pub = rospy.Publisher(marker_topic, Marker, queue_size=1)
+        self.pub = self.create_publisher(Marker, marker_topic, 1)
         self.color = ColorRGBA(*color)
-        rospy.Subscriber(mag_topic, MagneticField, self.publish)
+        self.create_subscription(MagneticField, mag_topic, self.publish)
 
     def publish(self, vec: MagneticField):
-        rospy.logdebug("mag received")
+        rclpy.logdebug("mag received")
         marker = Marker()
         marker.header = vec.header
         marker.type = 0
@@ -59,7 +60,7 @@ def publish(self, vec: MagneticField):
         if self.length is not None:
             norm = np.linalg.norm(vec)
             if norm == 0:
-                rospy.logwarn("Zero vector received, skipping")
+                rclpy.logwarn("Zero vector received, skipping")
                 return
             vec = (self.length / norm) * vec
         marker.points.append(numpy_to_point(vec))
@@ -97,8 +98,9 @@ def publish(self, vec: MagneticField):
         metavar=("R", "G", "B", "A"),
         help="Color of vector to publish as floats RGBA 0 to 1",
     )
-    args = rospy.myargv()
+    args = rclpy.myargv()
     args = parser.parse_args(args[1:])
-    rospy.init_node("mag_to_marker")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("mag_to_marker")
     MagToMarker(args.mag_topic, args.marker_topic, length=args.length, color=args.color)
-    rospy.spin()
+    rclpy.spin()
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py
index 8ab315818..26afe8b05 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py
@@ -3,9 +3,10 @@
 import geometry_msgs.msg as geometry_msgs
 import nav_msgs.msg as nav_msgs
 import numpy as np
-import rospy
+import rclpy
 import std_msgs.msg as std_msgs
 from mil_msgs.msg import Point2D, PoseTwist
+from rclpy.node import Node
 from tf import transformations
 
 
@@ -415,9 +416,9 @@ def make_header(frame="/body", stamp=None) -> std_msgs.Header:
     """
     if stamp is None:
         try:
-            stamp = rospy.Time.now()
-        except rospy.ROSInitException:
-            stamp = rospy.Time(0)
+            stamp = rclpy.Time.now()
+        except rclpy.ROSInitException:
+            stamp = rclpy.Time(0)
 
     header = std_msgs.Header(stamp=stamp, frame_id=frame)
     return header
@@ -481,7 +482,7 @@ def wrapped_callback(*args):
         msg = args[-1]
         callback(odometry_to_numpy(msg))
 
-    return rospy.Subscriber(topic, nav_msgs.Odometry, wrapped_callback, queue_size=1)
+    return Node.create_subcription(nav_msgs.Odometry, topic, wrapped_callback, 1)
 
 
 def ros_to_np_3D(msg):
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/plotter.py b/mil_common/utils/mil_tools/mil_ros_tools/plotter.py
index 92c8efed9..316b52492 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/plotter.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/plotter.py
@@ -4,7 +4,6 @@
 
 import matplotlib
 import numpy as np
-import rospy
 from cv_bridge import CvBridge
 from matplotlib.backends.backend_agg import FigureCanvasAgg
 from matplotlib.figure import Figure
@@ -66,14 +65,14 @@ def __init__(
         dpi: int = 150,
     ):
         matplotlib.rcParams.update({"font.size": 22})
-        self.pub = rospy.Publisher(topic_name, Image, queue_size=1)
+        self.pub = self.create_publisher(Image, topic_name, 1)
         self.bridge = CvBridge()
         self.fig = Figure(figsize=(width, height), dpi=dpi)
         self.canvas = FigureCanvasAgg(self.fig)
         self.enabled = True
         self.thread = None
 
-        rospy.Service(f"{topic_name}_enable", SetBool, self.enable_disable)
+        self.create_service(SetBool, f"{topic_name}_enable", self.enable_disable)
 
     def enable_disable(self, req: SetBoolRequest) -> SetBoolResponse:
         """
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/rviz_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/rviz_helpers.py
index fdbc6d788..9803c7667 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/rviz_helpers.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/rviz_helpers.py
@@ -2,15 +2,16 @@
 from typing import List, Tuple
 
 import numpy as np
-import rospy
+import rclpy
 import visualization_msgs.msg as visualization_msgs
 from geometry_msgs.msg import Point, Pose, Vector3
 from image_geometry import PinholeCameraModel
+from rclpy.node import Node
 from std_msgs.msg import ColorRGBA
 
 import mil_ros_tools
 
-rviz_pub = rospy.Publisher("visualization", visualization_msgs.Marker, queue_size=3)
+rviz_pub = Node.create_publisher(visualization_msgs.Marker, "visualization", 3)
 
 
 def draw_sphere(
@@ -46,7 +47,7 @@ def draw_sphere(
         pose=pose,
         color=ColorRGBA(*color),
         scale=Vector3(*scaling),
-        lifetime=rospy.Duration(),
+        lifetime=rclpy.Duration(),
     )
     rviz_pub.publish(marker)
 
@@ -115,7 +116,7 @@ def make_ray(
         color=ColorRGBA(*color),
         scale=Vector3(0.05, 0.05, 0.05),
         points=[Point(*o) for o in [base, direction * length]],
-        lifetime=rospy.Duration(),
+        lifetime=rclpy.Duration(),
         **kwargs,
     )
     return marker
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py b/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py
index 96330d57a..081619edf 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py
@@ -1,9 +1,10 @@
 #!/usr/bin/env python3
 import argparse
+import sys
 from typing import List, Optional
 
 import numpy as np
-import rospy
+import rclpy
 from geometry_msgs.msg import Vector3Stamped
 from mil_tools import numpy_to_colorRGBA, numpy_to_point, rosmsg_to_numpy
 from visualization_msgs.msg import Marker
@@ -40,12 +41,12 @@ def __init__(
         color: List[float] = [0, 0, 1, 1],
     ):
         self.length = length
-        self.pub = rospy.Publisher(marker_topic, Marker, queue_size=1)
+        self.pub = self.create_publisher(Marker, marker_topic, 1)
         self.color = numpy_to_colorRGBA(color)
-        rospy.Subscriber(vector_topic, Vector3Stamped, self.publish)
+        self.create_subscription(Vector3Stamped, vector_topic, self.publish)
 
     def publish(self, vec):
-        rospy.logdebug("vector received")
+        rclpy.logdebug("vector received")
         marker = Marker()
         marker.header = vec.header
         marker.type = 0
@@ -62,7 +63,7 @@ def publish(self, vec):
         if self.length is not None:
             norm = np.linalg.norm(vec)
             if norm == 0:
-                rospy.logwarn("Zero vector received, skipping")
+                rclpy.logwarn("Zero vector received, skipping")
                 return
             vec = (self.length / norm) * vec
         marker.points.append(numpy_to_point(vec))
@@ -98,13 +99,15 @@ def publish(self, vec):
         metavar=("R", "G", "B", "A"),
         help="Color of vector to publish as floats RGBA 0 to 1",
     )
-    args = rospy.myargv()
+    args = rclpy.myargv()
     args = parser.parse_args(args[1:])
-    rospy.init_node("vector_to_marker")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("vector_to_marker")
+
     VectorToMarker(
         args.vector_topic,
         args.marker_topic,
         length=args.length,
         color=args.color,
     )
-    rospy.spin()
+    rclpy.spin()
diff --git a/mil_common/utils/mil_tools/nodes/clicked_point_recorder.py b/mil_common/utils/mil_tools/nodes/clicked_point_recorder.py
index 542526af1..4e263346c 100755
--- a/mil_common/utils/mil_tools/nodes/clicked_point_recorder.py
+++ b/mil_common/utils/mil_tools/nodes/clicked_point_recorder.py
@@ -2,9 +2,10 @@
 
 import csv
 import datetime
+import sys
 from typing import Dict, Union
 
-import rospy
+import rclpy
 from geometry_msgs.msg import PointStamped
 
 
@@ -16,7 +17,11 @@ class ClickedPointRecorder:
 
     def __init__(self):
         self.points = []
-        self.point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.point_cb)
+        self.point_sub = self.create_subscription(
+            PointStamped,
+            "/clicked_point",
+            self.point_cb,
+        )
 
     def point_to_dict(self, point: PointStamped) -> Dict[str, Union[str, int, float]]:
         """
@@ -52,7 +57,7 @@ def write_file(self) -> None:
             for p in self.points:
                 d = self.point_to_dict(p)
                 writer.writerow(d)
-        rospy.loginfo(f"Writing points to {filename}")
+        self.get_logger().info(f"Writing points to {filename}")
 
     def point_cb(self, point: PointStamped) -> None:
         """
@@ -62,16 +67,17 @@ def point_cb(self, point: PointStamped) -> None:
         Args:
             point (PointStamped): The message input to the callback.
         """
-        rospy.loginfo(f"Received new point: {point}")
+        self.get_logger().info(f"Received new point: {point}")
         self.points.append(point)
 
 
 if __name__ == "__main__":
-    rospy.init_node("clicked_point_recorder")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("clicked_point_recorder")
     recorder = ClickedPointRecorder()
 
     def shutdown_cb():
         recorder.write_file()
 
-    rospy.on_shutdown(shutdown_cb)
-    rospy.spin()
+    rclpy.on_shutdown(shutdown_cb)
+    rclpy.spin()
diff --git a/mil_common/utils/mil_tools/nodes/network_broadcaster.py b/mil_common/utils/mil_tools/nodes/network_broadcaster.py
index 05c612e53..ee328f64e 100755
--- a/mil_common/utils/mil_tools/nodes/network_broadcaster.py
+++ b/mil_common/utils/mil_tools/nodes/network_broadcaster.py
@@ -4,7 +4,9 @@
 at a fixed interval. Useful for monitoring network loss
 or for a safety network heartbeat.
 """
-import rospy
+import sys
+
+import rclpy
 from std_msgs.msg import Header
 
 
@@ -22,30 +24,34 @@ class NetworkBroadcaster:
     """
 
     def __init__(self):
-        hz = rospy.get_param("~hz", 20)
-        topic = rospy.get_param("~topic", "network")
+        hz = self.declare_parameter("~hz", 20)
+        topic = self.declare_parameter("~topic", "network")
 
-        rospy.loginfo(f"NETWORK BROADCASTER: publishing to {topic} at {hz}hz")
+        rclpy.loginfo(f"NETWORK BROADCASTER: publishing to {topic} at {hz}hz")
         self.msg = Header()
         self.msg.seq = 0
         self.num_connections = -1
-        self.pub = rospy.Publisher(topic, Header, queue_size=1, tcp_nodelay=True)
-        rospy.Timer(rospy.Duration(1 / hz), self._publish)
+        self.pub = self.create_publisher(Header, topic, 1, tcp_nodelay=True)
+        rclpy.Timer(rclpy.Duration(1 / hz), self._publish)
 
     def _publish(self, *args):
         connections = self.pub.get_num_connections()
         if connections != self.num_connections:
             if connections == 0:
-                rospy.loginfo("NETWORK BROADCASTER: no connections")
+                node.get_logger().info("NETWORK BROADCASTER: no connections")
             else:
-                rospy.loginfo(f"NETWORK BROADCASTER: connected to {connections} nodes")
+                node.get_logger().info(
+                    f"NETWORK BROADCASTER: connected to {connections} nodes",
+                )
             self.num_connections = connections
-        self.msg.stamp = rospy.Time.now()
+        self.msg.stamp = rclpy.Time.now()
         self.pub.publish(self.msg)
         self.msg.seq += 1
 
 
 if __name__ == "__main__":
-    rospy.init_node("network_broadcaster", anonymous=True)
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("network_broadcaster")
+
     NetworkBroadcaster()
-    rospy.spin()
+    rclpy.spin()
diff --git a/mil_common/utils/mil_tools/nodes/ogrid_draw.py b/mil_common/utils/mil_tools/nodes/ogrid_draw.py
index 42b7552e0..562b12b6f 100755
--- a/mil_common/utils/mil_tools/nodes/ogrid_draw.py
+++ b/mil_common/utils/mil_tools/nodes/ogrid_draw.py
@@ -7,11 +7,12 @@
 
 import cv2
 import numpy as np
-import rospy
+import rclpy
 from geometry_msgs.msg import Pose
 from nav_msgs.msg import MapMetaData, OccupancyGrid
 
-rospy.init_node("ogrid_draw_node", anonymous=True)
+rclpy.init(args=sys.argv)
+node = rclpy.create_node("ogrid_draw_node")
 
 
 class DrawGrid:
@@ -61,13 +62,17 @@ class OGridPub:
     """
 
     def __init__(self, image_path=None):
-        height = int(rospy.get_param("~grid_height", 800))
-        width = int(rospy.get_param("~grid_width", 800))
-        resolution = rospy.get_param("~grid_resolution", 0.3)
-        ogrid_topic = rospy.get_param("~grid_topic", "/ogrid")
+        height = int(self.declare_parameter("~grid_height", 800))
+        width = int(self.declare_parameter("~grid_width", 800))
+        resolution = self.declare_parameter("~grid_resolution", 0.3)
+        ogrid_topic = self.declare_parameter("~grid_topic", "/ogrid")
 
         self.grid_drawer = DrawGrid(height, width, image_path)
-        self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)
+        self.ogrid_pub = self.declare_parameter(
+            ogrid_topic,
+            OccupancyGrid,
+            queue_size=1,
+        )
 
         m = MapMetaData()
         m.resolution = resolution
@@ -79,14 +84,14 @@ def __init__(self, image_path=None):
 
         self.map_meta_data = m
 
-        rospy.Timer(rospy.Duration(1), self.pub_grid)
+        rclpy.Timer(rclpy.Duration(1), self.pub_grid)
 
     def pub_grid(self, *args):
         grid = self.grid_drawer.img
 
         ogrid = OccupancyGrid()
         ogrid.header.frame_id = "/enu"
-        ogrid.header.stamp = rospy.Time.now()
+        ogrid.header.stamp = rclpy.Time.now()
         ogrid.info = self.map_meta_data
         ogrid.data = np.subtract(np.flipud(grid).flatten(), 1).astype(np.int8).tolist()
 
@@ -116,7 +121,7 @@ def pub_grid(self, *args):
 
     o = OGridPub(image_path=im_path)
 
-    while not rospy.is_shutdown():
+    while not rclpy.is_shutdown():
         cv2.imshow("Draw OccupancyGrid", o.grid_drawer.img)
         k = cv2.waitKey(100) & 0xFF
 
diff --git a/mil_common/utils/mil_tools/nodes/online_bagger.py b/mil_common/utils/mil_tools/nodes/online_bagger.py
index 5055d3a5f..5839afd38 100755
--- a/mil_common/utils/mil_tools/nodes/online_bagger.py
+++ b/mil_common/utils/mil_tools/nodes/online_bagger.py
@@ -6,11 +6,12 @@
 import datetime
 import itertools
 import os
+import sys
 from collections import deque
 from typing import TYPE_CHECKING
 
+import rclpy
 import rosbag
-import rospy
 import rostopic
 from actionlib import SimpleActionClient, SimpleActionServer, TerminalState
 from mil_msgs.msg import (
@@ -60,8 +61,8 @@ def __init__(self):
         self.streaming = True
         self.get_params()
         if len(self.subscriber_list) == 0:
-            rospy.logwarn("No topics selected to subscribe to. Closing.")
-            rospy.signal_shutdown("No topics to subscribe to")
+            rclpy.logwarn("No topics selected to subscribe to. Closing.")
+            rclpy.signal_shutdown("No topics to subscribe to")
             return
         self.make_dicts()
 
@@ -72,7 +73,9 @@ def __init__(self):
             auto_start=False,
         )
         self.subscribe_loop()
-        rospy.loginfo(f"Remaining Failed Topics: {self.get_subscriber_list(False)}\n")
+        self.get_logger().info(
+            f"Remaining Failed Topics: {self.get_subscriber_list(False)}\n",
+        )
         self._action_server.start()
 
     def get_subscriber_list(self, status):
@@ -99,20 +102,23 @@ def get_params(self):
         """
         Retrieve parameters from param server.
         """
-        self.dir = rospy.get_param("~bag_package_path", default=None)
+        self.dir = self.declare_parameter("~bag_package_path", default=None)
         # Handle bag directory for MIL bag script
         if self.dir is None and "BAG_DIR" in os.environ:
             self.dir = os.environ["BAG_DIR"]
 
-        self.stream_time = rospy.get_param("~stream_time", default=30)  # seconds
-        self.resubscribe_period = rospy.get_param(
+        self.stream_time = self.declare_parameter("~stream_time", default=30)  # seconds
+        self.resubscribe_period = self.declare_parameter(
             "~resubscribe_period",
             default=3.0,
         )  # seconds
-        self.dated_folder = rospy.get_param("~dated_folder", default=True)  # bool
+        self.dated_folder = self.declare_parameter(
+            "~dated_folder",
+            default=True,
+        )  # bool
 
         self.subscriber_list = {}
-        topics_param = rospy.get_param("~topics", default=[])
+        topics_param = self.declare_parameter("~topics", default=[])
 
         # Add topics from rosparam to subscribe list
         for topic in topics_param:
@@ -147,8 +153,8 @@ def add_env_var(var):
             if key[0:4] == "bag_":
                 add_env_var(os.environ[key])
 
-        rospy.loginfo(f"Default stream_time: {self.stream_time} seconds")
-        rospy.loginfo(f"Bag Directory: {self.dir}")
+        node.get_logger().info(f"Default stream_time: {self.stream_time} seconds")
+        node.get_logger().info(f"Bag Directory: {self.dir}")
 
     def make_dicts(self):
         """
@@ -193,7 +199,9 @@ def __getitem__(self, index):
         for topic in self.subscriber_list:
             self.topic_messages[topic] = SliceableDeque(deque())
 
-        rospy.loginfo(f"Initial subscriber_list: {self.get_subscriber_list(False)}")
+        node.get_logger().info(
+            f"Initial subscriber_list: {self.get_subscriber_list(False)}",
+        )
 
     def subscribe_loop(self):
         """
@@ -205,25 +213,25 @@ def subscribe_loop(self):
         i = 0
         # if self.successful_subscription_count == 0 and not
         # rospy.is_shutdown():
-        while self.successful_subscription_count == 0 and not rospy.is_shutdown():
+        while self.successful_subscription_count == 0 and not rclpy.is_shutdown():
             self.subscribe()
-            rospy.sleep(0.1)
+            rclpy.sleep(0.1)
             i = i + 1
             if i % 1000 == 0:
-                rospy.logdebug("still subscribing!")
-        rospy.loginfo(
+                rclpy.logdebug("still subscribing!")
+        node.get_logger().info(
             "Subscribed to {} of {} topics, will try again every {} seconds".format(
                 self.successful_subscription_count,
                 len(self.subscriber_list),
                 self.resubscribe_period,
             ),
         )
-        self.resubscriber = rospy.Timer(
-            rospy.Duration(self.resubscribe_period),
+        self.resubscriber = rclpy.Timer(
+            rclpy.Duration(self.resubscribe_period),
             self.subscribe,
         )
 
-    def subscribe(self, _: rospy.timer.TimerEvent | None = None):
+    def subscribe(self, _: rclpy.timer.TimerEvent | None = None):
         """
         Subscribe to the topics defined in the yaml configuration file
 
@@ -239,16 +247,16 @@ def subscribe(self, _: rospy.timer.TimerEvent | None = None):
             self.resubscriber is not None
         ):
             self.resubscriber.shutdown()
-        rospy.loginfo("All topics subscribed too! Shutting down resubscriber")
+        node.get_logger().info("All topics subscribed too! Shutting down resubscriber")
 
         for topic, (time, subscribed) in self.subscriber_list.items():
             if not subscribed:
                 msg_class = rostopic.get_topic_class(topic)
                 if msg_class[1] is not None:
                     self.successful_subscription_count += 1
-                    rospy.Subscriber(
-                        topic,
+                    self.create_subscription(
                         msg_class[0],
+                        topic,
                         lambda msg, _topic=topic: self.bagger_callback(msg, _topic),
                     )
 
@@ -280,7 +288,7 @@ def get_header_time(self, msg: genpy.Message):
         if hasattr(msg, "header"):
             return msg.header.stamp
         else:
-            return rospy.get_rostime()
+            return rclpy.get_rostime()
 
     def get_time_index(self, topic, requested_seconds):
         """
@@ -338,7 +346,7 @@ def bagger_callback(self, msg: genpy.Message, topic: str):
 
         # verify streaming is popping off and recording topics
         if self.iteration_count % 100 == 0:
-            rospy.logdebug(
+            rclpy.logdebug(
                 "{} has {} messages spanning {} seconds".format(
                     topic,
                     self.get_topic_message_count(topic),
@@ -347,8 +355,8 @@ def bagger_callback(self, msg: genpy.Message, topic: str):
             )
 
         while (
-            time_diff > rospy.Duration(self.subscriber_list[topic][0])
-            and not rospy.is_shutdown()
+            time_diff > rclpy.Duration(self.subscriber_list[topic][0])
+            and not rclpy.is_shutdown()
         ):
             self.topic_messages[topic].popleft()
             time_diff = self.get_topic_duration(topic)
@@ -509,7 +517,7 @@ def start_bagging(self, req: BagOnlineGoal):
             if bag is not None:
                 bag.close()
             return
-        rospy.loginfo(f"Bag written to {result.filename}")
+        node.get_logger().info(f"Bag written to {result.filename}")
         result.success = True
         self._action_server.set_succeeded(result)
         self.streaming = True
@@ -539,7 +547,7 @@ def _feedback_cb(self, feedback):
             self.bar.refresh()
             self.total_it = percentage
 
-    def bag(self, timeout=rospy.Duration(0)):
+    def bag(self, timeout=rclpy.Duration(0)):
         self.client.wait_for_server()
         self.result = None
         self.total_it = 0
@@ -554,8 +562,8 @@ def bag(self, timeout=rospy.Duration(0)):
             done_cb=self._done_cb,
             feedback_cb=self._feedback_cb,
         )
-        while self.result is None and not rospy.is_shutdown():
-            rospy.sleep(0.1)
+        while self.result is None and not rclpy.is_shutdown():
+            rclpy.sleep(0.1)
         self.bar.refresh()
         self.bar.close()
         (status, result) = self.result
@@ -566,7 +574,7 @@ def bag(self, timeout=rospy.Duration(0)):
 
 
 if __name__ == "__main__":
-    argv = rospy.myargv()
+    argv = rclpy.myargv()
     parser = argparse.ArgumentParser(
         description="ROS node to maintain buffers to create bags of the past\
                                                   and client to call this node.",
@@ -605,11 +613,15 @@ def bag(self, timeout=rospy.Duration(0)):
     )
     args = parser.parse_args(argv[1:])
     if args.client:  # Run as actionclient
-        rospy.init_node("online_bagger_client", anonymous=True)
+        rclpy.init(args=sys.argv)
+        node = rclpy.create_node("online_bagger_client")
+
         topics = "" if args.topics is None else "".join(args.topics)
         client = OnlineBaggerClient(name=args.name, topics=topics, time=args.time)
         client.bag()
     else:  # Run as OnlineBagger server
-        rospy.init_node("online_bagger")
+        rclpy.init(args=sys.argv)
+        node = rclpy.create_node("online_bagger")
+
         bagger = OnlineBagger()
-        rospy.spin()
+        rclpy.spin()
diff --git a/mil_common/utils/mil_tools/nodes/tf_fudger.py b/mil_common/utils/mil_tools/nodes/tf_fudger.py
index 0ae508c98..cd10bad1f 100755
--- a/mil_common/utils/mil_tools/nodes/tf_fudger.py
+++ b/mil_common/utils/mil_tools/nodes/tf_fudger.py
@@ -5,11 +5,13 @@
 
 import cv2
 import numpy as np
-import rospy
+import rclpy
 import tf
 import tf.transformations as trns
 
-rospy.init_node("tf_fudger", anonymous=True)
+rclpy.init(args=sys.argv)
+node = rclpy.create_node("tf_fudger")
+
 br = tf.TransformBroadcaster()
 
 usage_msg = "Useful to test transforms between two frames."
@@ -93,13 +95,13 @@ def toCvAng(x):
     listener.waitForTransform(
         args.tf_parent,
         args.tf_child,
-        rospy.Time(0),
-        rospy.Duration(1),
+        rclpy.Time(0),
+        rclpy.Duration(1),
     )
     (trans, rot) = listener.lookupTransform(
         args.tf_parent,
         args.tf_child,
-        rospy.Time(0),
+        rclpy.Time(0),
     )
     euler = trns.euler_from_quaternion(rot)
     p_original = trans
@@ -127,7 +129,7 @@ def reset():
 p_last = p_original
 rpy_last = rpy_original
 
-while not rospy.is_shutdown():
+while not rclpy.is_shutdown():
     x, y, z = (
         toTfLin(cv2.getTrackbarPos("x", "tf")),
         toTfLin(cv2.getTrackbarPos("y", "tf")),
@@ -211,7 +213,7 @@ def reset():
         break
     """
 
-    br.sendTransform(p, q, rospy.Time.now(), args.tf_child, args.tf_parent)
+    br.sendTransform(p, q, rclpy.Time.now(), args.tf_child, args.tf_parent)
 
 # Print out the tf static transform line with the fudged tf
 print(
diff --git a/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py b/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py
index 9363d3c4f..8e5e147ac 100755
--- a/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py
+++ b/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py
@@ -1,24 +1,28 @@
 #!/usr/bin/env python3
-import rospy
+import sys
+
+import rclpy
 import tf
 
 
 def main():
-    rospy.init_node("tf_to_gazebo")
-    do_cam_fix = rospy.get_param("~cam_fix", False)
-    tf_parent = rospy.get_param("~tf_parent", "/measurement")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("tf_to_gazebo")
+
+    do_cam_fix = node.declare_parameter("~cam_fix", False)
+    tf_parent = node.declare_parameter("~tf_parent", "/measurement")
     listener = tf.TransformListener()
-    rate = rospy.Rate(10.0)
+    rate = rclpy.Rate(10.0)
     cam_fix_quat = (-0.5, 0.5, -0.5, -0.5)
 
-    while not rospy.is_shutdown():
+    while not rclpy.is_shutdown():
         try:
             print("============= TRANSFORMS ================")
             for frame_id in listener.getFrameStrings():
                 (trans, rot) = listener.lookupTransform(
                     tf_parent,
                     frame_id,
-                    rospy.Time(0),
+                    rclpy.Time(0),
                 )
                 print("--")
                 print(f"Transform {tf_parent} {frame_id}")
diff --git a/mil_common/utils/mil_tools/nodes/video_player.py b/mil_common/utils/mil_tools/nodes/video_player.py
index 7b07ec3e7..703d10096 100755
--- a/mil_common/utils/mil_tools/nodes/video_player.py
+++ b/mil_common/utils/mil_tools/nodes/video_player.py
@@ -25,7 +25,7 @@
 import sys
 
 import cv2
-import rospy
+import rclpy
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
 
@@ -33,27 +33,27 @@
 class RosVideoPlayer:
     def __init__(self):
         self.bridge = CvBridge()
-        self.filename = rospy.get_param("~filename", sys.argv[1])
+        self.filename = self.declare_parameter("~filename", sys.argv[1])
         image_topic = (
             "video_player/"
             + os.path.splitext(os.path.basename(self.filename))[0]
             + "/image_raw",
         )
-        self.image_pub = rospy.Publisher(image_topic, Image, queue_size=10)
-        self.slider = rospy.get_param("~slider", True)
-        self.start_frame = rospy.get_param("~start_frames", 0)
+        self.image_pub = self.create_publisher(Image, image_topic, 10)
+        self.slider = self.declare_parameter("~slider", True)
+        self.start_frame = self.declare_parameter("~start_frames", 0)
         self.cap = cv2.VideoCapture(self.filename)
 
         self.width = self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)
         self.height = self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)
 
-        self.roi_y_offset = rospy.get_param("~y_offset", 0)
-        self.roi_x_offset = rospy.get_param("~x_offset", 0)
-        self.roi_height = rospy.get_param("~height", self.height)
-        self.roi_width = rospy.get_param("~width", self.width)
+        self.roi_y_offset = self.declare_parameter("~y_offset", 0)
+        self.roi_x_offset = self.declare_parameter("~x_offset", 0)
+        self.roi_height = self.declare_parameter("~height", self.height)
+        self.roi_width = self.declare_parameter("~width", self.width)
 
         self.num_frames = self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT)
-        self.fps = rospy.get_param("~fps", self.cap.get(cv2.cv.CV_CAP_PROP_FPS))
+        self.fps = self.declare_parameter("~fps", self.cap.get(cv2.cv.CV_CAP_PROP_FPS))
         self.cap.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, self.start_frame)
         self.num_frames = self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT)
         if self.num_frames < 1:
@@ -100,7 +100,7 @@ def __init__(self):
                 int(self.width),
                 self.roi_width_cb,
             )
-        rospy.loginfo(
+        self.get_logger().info(
             "Playing {} at {}fps starting at frame {} ({} Total Frames)".format(
                 self.filename,
                 self.fps,
@@ -110,8 +110,8 @@ def __init__(self):
         )
 
     def run(self):
-        r = rospy.Rate(self.fps)  # 10hz
-        while not rospy.is_shutdown() and self.cap.isOpened() and not self.ended:
+        r = rclpy.Rate(self.fps)  # 10hz
+        while not rclpy.is_shutdown() and self.cap.isOpened() and not self.ended:
             if self.slider:
                 k = cv2.waitKey(1) & 0xFF
                 if k != self.last_key:
@@ -140,7 +140,7 @@ def one_frame(self):
             print(f"Exception: {e}")
         if not ret:
             if not self.ended:
-                rospy.loginfo(f"File {self.filename} ended")
+                self.get_logger().info(f"File {self.filename} ended")
             self.ended = True
             return
         else:
@@ -175,7 +175,8 @@ def pause(self):
 
 
 def main():
-    rospy.init_node("video_player", anonymous=True)
+    rclpy.init(args=sys.argv)
+
     player = RosVideoPlayer()
     player.run()
 
diff --git a/mil_common/utils/mil_tools/scripts/param_sever.py b/mil_common/utils/mil_tools/scripts/param_sever.py
index f9044a4af..363ae364d 100755
--- a/mil_common/utils/mil_tools/scripts/param_sever.py
+++ b/mil_common/utils/mil_tools/scripts/param_sever.py
@@ -1,11 +1,13 @@
 #!/usr/bin/env python3
 import os
 import random
+import sys
 
-import rospy
+import rclpy
 import yaml
 
-rospy.init_node("param_saver", anonymous=True)
+rclpy.init(args=sys.argv)
+node = rclpy.create_node("param_saver")
 
 
 class MyDumper(yaml.Dumper):
@@ -13,16 +15,16 @@ def represent_mapping(self, tag, mapping, flow_style=False):
         return yaml.Dumper.represent_mapping(self, tag, mapping, flow_style)
 
 
-while not rospy.is_shutdown():
-    rospy.sleep(rospy.Duration(3))
+while not rclpy.is_shutdown():
+    rclpy.sleep(rclpy.Duration(3))
 
-    entries = rospy.get_param("~")
+    entries = node.declare_parameter("~")
     for entry in entries.values():
         filename = entry["filename"]
         file_basepath = entry["file_basepath"]
         param_paths = entry["param_paths"]
 
-        p = rospy.get_param(file_basepath)
+        p = node.declare_parameter(file_basepath)
         data = yaml.dump(
             {k: v for k, v in p.items() if k in param_paths},
             Dumper=MyDumper,
diff --git a/mil_common/utils/mil_tools/test/test_ros_tools.py b/mil_common/utils/mil_tools/test/test_ros_tools.py
index 2c0eeeca5..4596ef995 100644
--- a/mil_common/utils/mil_tools/test/test_ros_tools.py
+++ b/mil_common/utils/mil_tools/test/test_ros_tools.py
@@ -4,7 +4,7 @@
 import unittest
 
 import numpy as np
-import rospy
+import rclpy
 from geometry_msgs.msg import Pose2D, Quaternion, Vector3
 from mil_misc_tools import datetime_to_rospy, rospy_to_datetime
 from mil_ros_tools import (
@@ -161,7 +161,7 @@ def test_datetime_conv(self):
         """Test datetime to rospy.Time conversion."""
         for _ in range(10):
             unix_stamp = random.randrange(0, int(time.time()))
-            rp1 = rospy.Time(unix_stamp)
+            rp1 = rclpy.Time(unix_stamp)
             dt1 = rospy_to_datetime(rp1)
             self.assertEqual(datetime_to_rospy(dt1), rp1)
 

From 271a76144cbacafb5bf1c67e162c10b0bb07d3eb Mon Sep 17 00:00:00 2001
From: anvitD <anvitd@SubjuGator.myguest.virtualbox.org>
Date: Wed, 17 Apr 2024 19:28:52 -0400
Subject: [PATCH 4/6] made some corrections to mil_tools

---
 .../mil_tools/mil_ros_tools/image_helpers.py   | 18 ++++++++++--------
 .../mil_tools/mil_ros_tools/init_helpers.py    |  2 +-
 .../mil_tools/mil_ros_tools/mag_to_marker.py   |  2 +-
 .../mil_ros_tools/vector_to_marker.py          |  2 +-
 .../mil_tools/nodes/network_broadcaster.py     |  2 +-
 .../utils/mil_tools/nodes/online_bagger.py     |  2 +-
 6 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py
index d87d7f487..b99561c17 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py
@@ -24,14 +24,16 @@ def get_parameter_range(parameter_root: str):
     """
     low_param, high_param = parameter_root + "/hsv_low", parameter_root + "/hsv_high"
 
-    rclpy.logwarn(f"Blocking -- waiting for parameters {low_param} and {high_param}")
+    Node.get_logger().warn(
+        f"Blocking -- waiting for parameters {low_param} and {high_param}",
+    )
 
     wait_for_param(low_param)
     wait_for_param(high_param)
     low = Node.declare_parameter(low_param)
     high = Node.declare_parameter(high_param)
 
-    Node.get_logger.info()(f"Got {low_param} and {high_param}")
+    Node.get_logger().info()(f"Got {low_param} and {high_param}")
     return np.array([low, high]).transpose()
 
 
@@ -82,7 +84,7 @@ def publish(self, cv_image: np.ndarray):
             self.im_pub.publish(image_message)
         except CvBridgeError as e:
             # Intentionally absorb CvBridge Errors
-            rclpy.logerr(str(e))
+            self.get_logger().warn(str(e))
 
 
 class Image_Subscriber:
@@ -142,7 +144,7 @@ def wait_for_camera_info(self, timeout: int = 10):
         Raises:
             Exception: No camera info was found after the timeout had finished.
         """
-        rclpy.logwarn(
+        self.get_logger().warn(
             "Blocking -- waiting at most %d seconds for camera info." % timeout,
         )
 
@@ -152,11 +154,11 @@ def wait_for_camera_info(self, timeout: int = 10):
 
         while (rclpy.Time.now() - start_time < timeout) and (not rclpy.is_shutdown()):
             if self.camera_info is not None:
-                rclpy.loginfo("Camera info found!")
+                self.get_logger().info("Camera info found!")
                 return self.camera_info
             rclpy.sleep(0.2)
 
-        rclpy.logerr("Camera info not found.")
+        self.get_logger().warn("Camera info not found.")
         raise Exception("Camera info not found.")
 
     def wait_for_camera_model(self, **kwargs):
@@ -204,7 +206,7 @@ def convert(self, data: Image):
             self.callback(image)
         except CvBridgeError as e:
             # Intentionally absorb CvBridge Errors
-            rclpy.logerr(e)
+            self.get_logger().warn(e)
 
 
 class StereoImageSubscriber:
@@ -377,4 +379,4 @@ def _image_callback(self, left_img: Image, right_img: Image):
             self.callback(img_left, img_right)
         except CvBridgeError as e:
             # Intentionally absorb CvBridge Errors
-            rclpy.logerr(e)
+            self.get_logger().warn(e)
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py
index c1d10653e..2db7cc5df 100644
--- a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py
@@ -105,5 +105,5 @@ def wait_for_service(
     except rclpy.ROSException:
         if timeout is not None:
             timeout = timeout - warn_time
-        rclpy.logwarn(warn_msg)
+        Node.get_logger().warm(warn_msg)
         service.wait_for_service(timeout)
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py b/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py
index be4e78347..9163987bd 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py
@@ -60,7 +60,7 @@ def publish(self, vec: MagneticField):
         if self.length is not None:
             norm = np.linalg.norm(vec)
             if norm == 0:
-                rclpy.logwarn("Zero vector received, skipping")
+                self.get_logger().warn("Zero vector received, skipping")
                 return
             vec = (self.length / norm) * vec
         marker.points.append(numpy_to_point(vec))
diff --git a/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py b/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py
index 081619edf..25d75e211 100755
--- a/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py
+++ b/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py
@@ -63,7 +63,7 @@ def publish(self, vec):
         if self.length is not None:
             norm = np.linalg.norm(vec)
             if norm == 0:
-                rclpy.logwarn("Zero vector received, skipping")
+                self.get_logger().warn("Zero vector received, skipping")
                 return
             vec = (self.length / norm) * vec
         marker.points.append(numpy_to_point(vec))
diff --git a/mil_common/utils/mil_tools/nodes/network_broadcaster.py b/mil_common/utils/mil_tools/nodes/network_broadcaster.py
index ee328f64e..69b8a2a64 100755
--- a/mil_common/utils/mil_tools/nodes/network_broadcaster.py
+++ b/mil_common/utils/mil_tools/nodes/network_broadcaster.py
@@ -27,7 +27,7 @@ def __init__(self):
         hz = self.declare_parameter("~hz", 20)
         topic = self.declare_parameter("~topic", "network")
 
-        rclpy.loginfo(f"NETWORK BROADCASTER: publishing to {topic} at {hz}hz")
+        self.get_logger().info(f"NETWORK BROADCASTER: publishing to {topic} at {hz}hz")
         self.msg = Header()
         self.msg.seq = 0
         self.num_connections = -1
diff --git a/mil_common/utils/mil_tools/nodes/online_bagger.py b/mil_common/utils/mil_tools/nodes/online_bagger.py
index 5839afd38..288d9dcf9 100755
--- a/mil_common/utils/mil_tools/nodes/online_bagger.py
+++ b/mil_common/utils/mil_tools/nodes/online_bagger.py
@@ -61,7 +61,7 @@ def __init__(self):
         self.streaming = True
         self.get_params()
         if len(self.subscriber_list) == 0:
-            rclpy.logwarn("No topics selected to subscribe to. Closing.")
+            self.get_logger().warn("No topics selected to subscribe to. Closing.")
             rclpy.signal_shutdown("No topics to subscribe to")
             return
         self.make_dicts()

From 66e926ef06a520500d6e1eab48521297cb65696d Mon Sep 17 00:00:00 2001
From: anvitD <anvitd@SubjuGator.myguest.virtualbox.org>
Date: Wed, 17 Apr 2024 19:36:51 -0400
Subject: [PATCH 5/6] made corrections to mil_poi

---
 mil_common/utils/mil_poi/mil_poi/server.py |  2 +-
 mil_common/utils/mil_poi/nodes/poi_server  | 10 +++++++---
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py
index 8bdee09cf..0f401b8fc 100644
--- a/mil_common/utils/mil_poi/mil_poi/server.py
+++ b/mil_common/utils/mil_poi/mil_poi/server.py
@@ -122,7 +122,7 @@ def transform_position(self, ps: PointStamped) -> Optional[Point]:
             )
             return ps_tf.point
         except tf2_ros.TransformException as e:
-            rclpy.logwarn(
+            self.get_logger().warn(
                 'Error transforming "{}" to "{}": {}'.format(
                     ps.header.frame_id,
                     self.global_frame,
diff --git a/mil_common/utils/mil_poi/nodes/poi_server b/mil_common/utils/mil_poi/nodes/poi_server
index 2ad461514..2d45e8e29 100755
--- a/mil_common/utils/mil_poi/nodes/poi_server
+++ b/mil_common/utils/mil_poi/nodes/poi_server
@@ -1,8 +1,12 @@
 #!/usr/bin/env python3
-import rospy
+import sys
+
+import rclpy
 from mil_poi import POIServer
 
 if __name__ == "__main__":
-    rospy.init_node("poi_server")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("poi_server")
+
     server = POIServer()
-    rospy.spin()
+    rclpy.spin()

From 128e797ac22cceed5e54a0b568823e1394ec1f99 Mon Sep 17 00:00:00 2001
From: anvitD <anvitd@SubjuGator.myguest.virtualbox.org>
Date: Wed, 17 Apr 2024 20:10:32 -0400
Subject: [PATCH 6/6] updated navigator_gui to ROS2

---
 .../navigator_gui/navigator_gui/dashboard.py  | 22 +++++++++++--------
 .../navigator_gui/host_monitor.py             | 18 ++++++++-------
 .../navigator_gui/navigator_gui/shooter.py    | 10 ++++-----
 .../navigator_gui/stc_display.py              |  7 ++++--
 4 files changed, 33 insertions(+), 24 deletions(-)

diff --git a/NaviGator/utils/navigator_gui/navigator_gui/dashboard.py b/NaviGator/utils/navigator_gui/navigator_gui/dashboard.py
index 1a22d65db..bcf3ee976 100755
--- a/NaviGator/utils/navigator_gui/navigator_gui/dashboard.py
+++ b/NaviGator/utils/navigator_gui/navigator_gui/dashboard.py
@@ -8,8 +8,8 @@
 from copy import copy
 from typing import Any
 
+import rclpy
 import rospkg
-import rospy
 from mil_tools import thread_lock
 from navigator_msgs.msg import Host, Hosts
 from python_qt_binding import QtCore, QtWidgets, loadUi
@@ -79,7 +79,7 @@ def __init__(self, context: Any):
 
     @thread_lock(lock)
     def update_gui(self):
-        self.system_time["current"] = rospy.Time.now()
+        self.system_time["current"] = rclpy.Time.now()
         if self.system_time["current"] != self.system_time["displayed"]:
             self.update_system_time_status()
         if self.kill_status["current"] != self.kill_status["displayed"]:
@@ -204,18 +204,22 @@ def connect_ros(self) -> None:
         within this class.
         """
         # Attempts to read the battery voltage parameters (sets them to defaults if they have not been set)
-        self.battery_low_voltage = rospy.get_param(
+        self.battery_low_voltage = self.declare_parameter(
             "/battery_monitor/battery_low_voltage",
             24,
         )
-        self.battery_critical_voltage = rospy.get_param(
+        self.battery_critical_voltage = self.get_parameter(
             "/battery_monitor/battery_critical_voltage",
             20,
         )
 
-        rospy.Subscriber("/wrench/selected", String, self.cache_operating_mode)
-        rospy.Subscriber("/battery_monitor", Float32, self.cache_battery_voltage)
-        rospy.Subscriber("/host_monitor", Hosts, self.cache_hosts)
+        self.create_subscription(String, "/wrench/selected", self.cache_operating_mode)
+        self.create_subscription(
+            Float32,
+            "/battery_monitor",
+            self.cache_battery_voltage,
+        )
+        self.create_subscription(Hosts, "/host_monitor", self.cache_hosts)
 
         self.kill_listener = AlarmListener(
             "kill",
@@ -241,7 +245,7 @@ def cache_hosts(self, msg: Hosts) -> None:
         in the hosts receiving variable.
         """
         self.hosts["current"] = msg
-        self.hosts["stamp"] = rospy.Time.now()
+        self.hosts["stamp"] = rclpy.Time.now()
 
     def update_kill_status(self) -> None:
         if self.kill_status["current"] is False:
@@ -291,7 +295,7 @@ def monitor_battery_voltage(self) -> None:
 
         # Sets the battery voltage to 'Unknown' if no message has been current in 15s
         if (
-            (rospy.Time.now() - self.battery_voltage["stamp"]) > rospy.Duration(15)
+            (rclpy.Time.now() - self.battery_voltage["stamp"]) > rclpy.Duration(15)
         ) or (self.battery_voltage["current"] is None):
             self.battery_voltage["current"] = "Unknown"
 
diff --git a/NaviGator/utils/navigator_gui/navigator_gui/host_monitor.py b/NaviGator/utils/navigator_gui/navigator_gui/host_monitor.py
index 2c5660ce9..136b6eb8c 100755
--- a/NaviGator/utils/navigator_gui/navigator_gui/host_monitor.py
+++ b/NaviGator/utils/navigator_gui/navigator_gui/host_monitor.py
@@ -6,8 +6,9 @@
 
 import socket
 import subprocess
+import sys
 
-import rospy
+import rclpy
 from navigator_msgs.msg import Host, Hosts
 
 __author__ = "Anthony Olive"
@@ -17,15 +18,16 @@
 __license__ = "MIT"
 
 
-rospy.init_node("host_monitor")
+rclpy.init(args=sys.argv)
+node = rclpy.create_node("host_monitor")
 
 
 class HostMonitor:
     def __init__(self):
-        self.pub_hosts = rospy.Publisher(
-            "/host_monitor",
+        self.pub_hosts = self.create_publisher(
             Hosts,
-            queue_size=1,
+            "/host_monitor",
+            1,
             latch=True,
         )
         self.hosts = Hosts()
@@ -61,7 +63,7 @@ def check_hosts(self) -> None:
 
             self.hosts.hosts.append(host)
 
-    def publish(self, _: rospy.timer.TimerEvent) -> None:
+    def publish(self, _: rclpy.timer.TimerEvent) -> None:
         """
         Publishes the list of hosts and the information gathered about them.
         """
@@ -71,5 +73,5 @@ def publish(self, _: rospy.timer.TimerEvent) -> None:
 
 if __name__ == "__main__":
     monitor = HostMonitor()
-    rospy.Timer(rospy.Duration(10), monitor.publish, oneshot=False)
-    rospy.spin()
+    rclpy.Timer(rclpy.Duration(10), monitor.publish, oneshot=False)
+    rclpy.spin()
diff --git a/NaviGator/utils/navigator_gui/navigator_gui/shooter.py b/NaviGator/utils/navigator_gui/navigator_gui/shooter.py
index d55a36043..18ed087a4 100755
--- a/NaviGator/utils/navigator_gui/navigator_gui/shooter.py
+++ b/NaviGator/utils/navigator_gui/navigator_gui/shooter.py
@@ -8,8 +8,8 @@
 
 import os
 
+import rclpy
 import rospkg
-import rospy
 from python_qt_binding import QtCore, QtWidgets, loadUi
 from qt_gui.plugin import Plugin
 from remote_control_lib import RemoteControl
@@ -44,7 +44,7 @@ def __init__(self, context):
 
         self.shooter_status = {
             "received": "Unknown",
-            "stamp": rospy.Time.now(),
+            "stamp": rclpy.Time.now(),
             "cached": "Unknown",
         }
 
@@ -52,7 +52,7 @@ def __init__(self, context):
 
         self.connect_ui()
 
-        rospy.Subscriber("/shooter/status", String, self.cache_shooter_status)
+        self.create_subscription(String, "/shooter/status", self.cache_shooter_status)
 
         # Deals with problem of multiple instances of same plugin
         if context.serial_number() > 1:
@@ -142,7 +142,7 @@ def cache_shooter_status(self, msg: String) -> None:
         Stores the shooter status when it is published.
         """
         self.shooter_status["received"] = msg.data
-        self.shooter_status["stamp"] = rospy.Time.now()
+        self.shooter_status["stamp"] = rclpy.Time.now()
 
     def monitor_shooter_status(self) -> None:
         """
@@ -150,7 +150,7 @@ def monitor_shooter_status(self) -> None:
         when the received shooter status has changed. The connection to the
         status will time out if no message has been received in 1s.
         """
-        if (rospy.Time.now() - self.shooter_status["stamp"]) < rospy.Duration(1):
+        if (rclpy.Time.now() - self.shooter_status["stamp"]) < rclpy.Duration(1):
             self.remote.is_timed_out = False
 
         # Sets the remote control to timed out and the shooter status to 'Unknown' if no message has been received in 1s
diff --git a/NaviGator/utils/navigator_gui/navigator_gui/stc_display.py b/NaviGator/utils/navigator_gui/navigator_gui/stc_display.py
index 3ccc6fb3e..9569320f3 100644
--- a/NaviGator/utils/navigator_gui/navigator_gui/stc_display.py
+++ b/NaviGator/utils/navigator_gui/navigator_gui/stc_display.py
@@ -1,7 +1,6 @@
 import os
 
 import rospkg
-import rospy
 from navigator_msgs.msg import ScanTheCode
 from python_qt_binding import QtWidgets, loadUi
 from python_qt_binding.QtWidgets import QWidget
@@ -63,7 +62,11 @@ def __init__(self, context):
         # Add widget to the user interface
         context.add_widget(self._widget)
         self.connect_ui()
-        self.stc_sub = rospy.Subscriber(self.STC_TOPIC, ScanTheCode, self.update_gui)
+        self.stc_sub = self.create_subscription(
+            ScanTheCode,
+            self.STC_TOPIC,
+            self.update_gui,
+        )
 
     def connect_ui(self) -> None:
         self.color1_rect = self._widget.findChild(QtWidgets.QColumnView, "color1_rect")