Ros 2 migration plan anvit

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 = self.create_publisher(Odometry, "/odom", 2)
         self.odom = None
-        self.carrot_sub = rospy.Subscriber("/trajectory/cmd", Odometry, self.set_odom)
+        self.carrot_sub = self.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)
+        self.create_service(ObjectDBQuery, "/database/requests", self.got_request)
+        self.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()

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"
 

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

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,15 +44,15 @@ def __init__(self, context):
 
         self.shooter_status = {
             "received": "Unknown",
-            "stamp": rospy.Time.now(),
+            "stamp": rclpy.Time.now(),
             "cached": "Unknown",
         }
 
         self.disc_speed_setting = 0
 
         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,15 +142,15 @@ 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:
         """
         Monitors the shooter status on a 1s interval. Only updates the display
         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

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

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(
+            self.get_logger().warn(
                 'Error transforming "{}" to "{}": {}'.format(
                     ps.header.frame_id,
                     self.global_frame,
@@ -163,15 +167,15 @@ 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] = [
                 float(poi.position.x),
                 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()
 

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