From 87349354b31e06d6e9dea1a8e219030d301fa3a5 Mon Sep 17 00:00:00 2001 From: Nihar Devireddy Date: Sun, 28 Apr 2024 16:07:45 -0400 Subject: [PATCH] migrated navigator_emergency_control, navigator_joystick_control, and navigator_keyboard_control to ros2 --- .../nodes/navigator_emergency.py | 37 ++++++---- .../nodes/navigator_joystick.py | 30 ++++---- .../nodes/navigator_keyboard_client.py | 28 +++++--- .../nodes/navigator_keyboard_server.py | 14 ++-- .../remote_control_lib/remote_control_lib.py | 72 ++++++++++--------- 5 files changed, 102 insertions(+), 79 deletions(-) diff --git a/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py b/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py index 49b799fa4..01502a555 100755 --- a/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py +++ b/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 -import rospy +import sys + +import rclpy +from rclpy.duration import Duration from remote_control_lib import RemoteControl from sensor_msgs.msg import Joy @@ -32,11 +35,11 @@ class Joystick: """ def __init__(self): - self.force_scale = rospy.get_param("/joystick_wrench/force_scale", 600) - self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale", 500) + self.force_scale = self.declare_parameter("/joystick_wrench/force_scale", 600) + self.torque_scale = self.declare_parameter("/joystick_wrench/torque_scale", 500) self.remote = RemoteControl("emergency", "/wrench/emergency") - rospy.Subscriber("joy_emergency", Joy, self.joy_recieved) + self.create_subscription(Joy, "joy_emergency", self.joy_recieved) self.active = False self.reset() @@ -80,15 +83,16 @@ def check_for_timeout(self, joy: Joy): # No change in state # The controller times out after 15 minutes if ( - rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60) + self.get_clock().now() - self.last_joy.header.stamp + > Duration(seconds=(15 * 60)) and self.active ): - rospy.logwarn("Controller Timed out. Hold start to resume.") + self.get_logger().warn("Controller Timed out. Hold start to resume.") self.reset() else: joy.header.stamp = ( - rospy.Time.now() + self.get_clock().now() ) # In the sim, stamps weren't working right self.last_joy = joy @@ -102,7 +106,7 @@ def joy_recieved(self, joy: Joy) -> None: Args: joy (Joy): The Joy message. """ - self.last_time = rospy.Time.now() + self.last_time = self.get_clock().now() self.check_for_timeout(joy) # Assigns readable names to the buttons that are used @@ -116,14 +120,14 @@ def joy_recieved(self, joy: Joy) -> None: thruster_deploy = bool(joy.buttons[5]) if go_inactive and not self.last_go_inactive: - rospy.loginfo("Go inactive pressed. Going inactive") + self.get_logger().info("Go inactive pressed. Going inactive") self.reset() return # Reset controller state if only start is pressed down about 1 seconds self.start_count += start if self.start_count > 5: - rospy.loginfo("Resetting controller state") + self.get_logger().info("Resetting controller state") self.reset() self.active = True @@ -170,19 +174,22 @@ def joy_recieved(self, joy: Joy) -> None: rotation = joy.axes[3] * self.torque_scale self.remote.publish_wrench(x, y, rotation, joy.header.stamp) - def die_check(self, _: rospy.timer.TimerEvent) -> None: + def die_check(self, _: rclpy.timer.TimerEvent) -> None: """ Publishes zeros after 2 seconds of no update in case node dies. """ # No new instructions after 2 seconds - if self.active and rospy.Time.now() - self.last_time > rospy.Duration(2): + if self.active and self.get_clock().now() - self.last_time > Duration( + seconds=2, + ): # Zero the wrench, reset self.reset() if __name__ == "__main__": - rospy.init_node("emergency") + rclpy.init(args=sys.argv) + node = rclpy.create_node("emergency") emergency = Joystick() - rospy.Timer(rospy.Duration(1), emergency.die_check, oneshot=False) - rospy.spin() + node.create_timer(Duration(seconds=1.0), emergency.die_check) + rclpy.spin(node) diff --git a/NaviGator/utils/remote_control/navigator_joystick_control/nodes/navigator_joystick.py b/NaviGator/utils/remote_control/navigator_joystick_control/nodes/navigator_joystick.py index 3d9ad194b..dd4af9b46 100755 --- a/NaviGator/utils/remote_control/navigator_joystick_control/nodes/navigator_joystick.py +++ b/NaviGator/utils/remote_control/navigator_joystick_control/nodes/navigator_joystick.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 -import rospy +import sys + +import rclpy +from rclpy.duration import Duration from remote_control_lib import RemoteControl from sensor_msgs.msg import Joy @@ -11,12 +14,12 @@ class Joystick: def __init__(self): - self.force_scale = rospy.get_param("~force_scale", 600) - self.torque_scale = rospy.get_param("~torque_scale", 500) + self.force_scale = self.declare_parameter("~force_scale", 600) + self.torque_scale = self.declare_parameter("~torque_scale", 500) self.remote = RemoteControl("joystick", "/wrench/rc") self.reset() - rospy.Subscriber("joy", Joy, self.joy_recieved) + self.create_subscription(Joy, "joy", self.joy_recieved) def reset(self): """ @@ -49,15 +52,16 @@ def check_for_timeout(self, joy): if ( joy.axes == self.last_joy.axes and joy.buttons == self.last_joy.buttons - and rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(15 * 60) + and self.get_clock().now() - self.last_joy.header.stamp + > Duration(seconds=(15 * 60)) and self.active ): - rospy.logwarn("Controller Timed out. Hold start to resume.") + self.get_logger().warn("Controller Timed out. Hold start to resume.") self.reset() else: joy.header.stamp = ( - rospy.Time.now() + self.get_clock().now() ) # In the sim, stamps weren't working right self.last_joy = joy @@ -78,14 +82,14 @@ def joy_recieved(self, joy): thruster_deploy = bool(joy.buttons[5]) # RB if back and not self.last_back: - rospy.loginfo("Back pressed. Going inactive") + self.get_logger().info("Back pressed. Going inactive") self.reset() return # Reset controller state if only start is pressed down about 1 second self.start_count += start if self.start_count > 5: - rospy.loginfo("Resetting controller state") + self.get_logger().info("Resetting controller state") self.reset() self.active = True @@ -143,11 +147,13 @@ def joy_recieved(self, joy): x = joy.axes[1] * self.force_scale y = joy.axes[0] * self.force_scale rotation = joy.axes[3] * self.torque_scale - self.remote.publish_wrench(x, y, rotation, rospy.Time.now()) + self.remote.publish_wrench(x, y, rotation, self.get_clock().now()) if __name__ == "__main__": - rospy.init_node("joystick") + rclpy.init(args=sys.argv) + node = rclpy.create_node("joystick") joystick = Joystick() - rospy.spin() + + rclpy.spin(node) diff --git a/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_client.py b/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_client.py index da34ee22c..7285b32db 100755 --- a/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_client.py +++ b/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_client.py @@ -7,18 +7,20 @@ calls. Curses is used to display a basic UI in the terminal that gives the user useful feedback and captures key presses to be sent to the server. """ - import contextlib import curses +import sys from typing import Optional -import rospy +import rclpy from navigator_msgs.srv import KeyboardControl +from rclpy.duration import Duration __author__ = "Anthony Olive" -rospy.init_node("keyboard_client", anonymous=True) +rclpy.init(args=sys.argv) +node = rclpy.create_node("keyboard_client", anonymous=True) class KeyboardClient: @@ -31,7 +33,7 @@ def __init__(self, stdscr): self.uuid = "" self.is_locked = False - self.keyboard_server = rospy.ServiceProxy("/keyboard_control", KeyboardControl) + self.keyboard_server = self.create_client(KeyboardControl, "/keyboard_control") self.help_menu = [ "Lock: L ", @@ -64,19 +66,19 @@ def read_key(self) -> Optional[int]: new_keycode = self.screen.getch() # This eliminates building a buffer of keys that takes forever to process - while (new_keycode != -1) and (not rospy.is_shutdown()): + while (new_keycode != -1) and (not self.is_shutdown()): keycode = new_keycode new_keycode = self.screen.getch() # The 'q' key can be used to quit the program if keycode == ord("q"): - rospy.signal_shutdown("The user has closed the keyboard client") + rclpy.shutdown("The user has closed the keyboard client") elif keycode == ord("H"): raise NotImplementedError("Kevin, you just threw away your shot!") return keycode if keycode != -1 else None - def send_key(self, _: rospy.timer.TimerEvent) -> None: + def send_key(self, _: rclpy.timer.TimerEvent) -> None: """ Sends the key to the keyboard server and stores the returned locked status and generated UUID (if one was received). @@ -138,12 +140,16 @@ def clear(self) -> None: def main(stdscr): - rospy.wait_for_service("/keyboard_control") + while not rclpy.wait_for_service(node, "/keyboard_control", timeout_sec=1.0): + node.get_logger().info("service not available, waiting again...") tele = KeyboardClient(stdscr) - rospy.Timer(rospy.Duration(0.05), tele.send_key, oneshot=False) - rospy.spin() + node.create_timer(Duration(seconds=0.05), tele.send_key) + rclpy.spin(node) if __name__ == "__main__": - with contextlib.suppress(rospy.ROSInterruptException): + # with contextlib.suppress(rospy.ROSInterruptException): + # curses.wrapper(main) + with contextlib.suppress(rclpy.ROSInterruptException): curses.wrapper(main) + # not sure about this change, could not find any migration solution diff --git a/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_server.py b/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_server.py index 8da84467a..1337839c9 100755 --- a/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_server.py +++ b/NaviGator/utils/remote_control/navigator_keyboard_control/nodes/navigator_keyboard_server.py @@ -10,25 +10,27 @@ import curses +import sys import uuid -import rospy +import rclpy from navigator_msgs.srv import KeyboardControl, KeyboardControlRequest from remote_control_lib import RemoteControl __author__ = "Anthony Olive" -rospy.init_node("keyboard_server") +rclpy.init(args=sys.argv) +node = rclpy.create_node("keyboard_server") class KeyboardServer: def __init__(self): - self.force_scale = rospy.get_param("/joystick_wrench/force_scale", 600) - self.torque_scale = rospy.get_param("/joystick_wrench/torque_scale", 500) + self.force_scale = self.declare_parameter("/joystick_wrench/force_scale", 600) + self.torque_scale = self.declare_parameter("/joystick_wrench/torque_scale", 500) self.remote = RemoteControl("keyboard", "/wrench/keyboard") - rospy.Service("/keyboard_control", KeyboardControl, self.key_recieved) + self.create_service(KeyboardControl, "/keyboard_control", self.key_recieved) # Initialize this to a random UUID so that a client without a UUID cannot authenticate self.locked_uuid = uuid.uuid4().hex @@ -115,4 +117,4 @@ def execute_key(self, key: int): if __name__ == "__main__": keyboard = KeyboardServer() - rospy.spin() + rclpy.spin(node) diff --git a/NaviGator/utils/remote_control/navigator_keyboard_control/remote_control_lib/remote_control_lib.py b/NaviGator/utils/remote_control/navigator_keyboard_control/remote_control_lib/remote_control_lib.py index 4b0aa11f0..9e9688b60 100755 --- a/NaviGator/utils/remote_control/navigator_keyboard_control/remote_control_lib/remote_control_lib.py +++ b/NaviGator/utils/remote_control/navigator_keyboard_control/remote_control_lib/remote_control_lib.py @@ -11,12 +11,12 @@ import actionlib import genpy -import rospy from actionlib import TerminalState from geometry_msgs.msg import WrenchStamped from mil_missions_core import MissionClient from navigator_msgs.msg import ShooterDoAction, ShooterDoActionGoal from navigator_msgs.srv import ShooterManual +from rclpy.duration import Duration from ros_alarms import AlarmBroadcaster, AlarmListener from std_srvs.srv import Trigger, TriggerRequest from topic_tools.srv import MuxSelect @@ -39,7 +39,7 @@ def __init__(self, controller_name: str, wrench_pub: Optional[str] = None): self.kill_broadcaster = AlarmBroadcaster("kill") self.station_hold_broadcaster = AlarmBroadcaster("station-hold") - self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect) + self.wrench_changer = self.create_client(MuxSelect, "/wrench/select") self.task_client = MissionClient() self.kill_listener = AlarmListener( "kill", @@ -49,7 +49,7 @@ def __init__(self, controller_name: str, wrench_pub: Optional[str] = None): if wrench_pub is None: self.wrench_pub = wrench_pub else: - self.wrench_pub = rospy.Publisher(wrench_pub, WrenchStamped, queue_size=1) + self.wrench_pub = self.create_publisher(WrenchStamped, wrench_pub, 1) self.shooter_load_client = actionlib.SimpleActionClient( "/shooter/load", @@ -59,12 +59,12 @@ def __init__(self, controller_name: str, wrench_pub: Optional[str] = None): "/shooter/fire", ShooterDoAction, ) - self.shooter_cancel_client = rospy.ServiceProxy("/shooter/cancel", Trigger) - self.shooter_manual_client = rospy.ServiceProxy( - "/shooter/manual", + self.shooter_cancel_client = self.create_client(Trigger, "/shooter/cancel") + self.shooter_manual_client = self.create_client( ShooterManual, + "/shooter/manual", ) - self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset", Trigger) + self.shooter_reset_client = self.create_client(Trigger, "/shooter/reset") self.is_killed = False self.is_timed_out = False @@ -95,7 +95,7 @@ def kill(self, *args, **kwargs) -> None: """ Kills the system regardless of what state it is in. """ - rospy.loginfo("Killing") + self.get_logger().info("Killing") self.kill_broadcaster.raise_alarm( problem_description="System kill from user remote control", parameters={"location": self.name}, @@ -106,7 +106,7 @@ def clear_kill(self, *args, **kwargs) -> None: """ Clears the system kill regardless of what state it is in. """ - rospy.loginfo("Reviving") + self.get_logger().info("Reviving") self.kill_broadcaster.clear_alarm() @_timeout_check @@ -114,7 +114,7 @@ def toggle_kill(self, *args, **kwargs) -> None: """ Toggles the kill status when the toggle_kill_button is pressed. """ - rospy.loginfo("Toggling Kill") + self.get_logger().info("Toggling Kill") # Responds to the kill broadcaster and checks the status of the kill alarm if self.is_killed: @@ -131,7 +131,7 @@ def station_hold(self, *args, **kwargs) -> None: Sets the goal point to the current location and switches to autonomous mode in order to stay at that point. """ - rospy.loginfo("Station Holding") + self.get_logger().info("Station Holding") # Trigger station holding at the current pose self.station_hold_broadcaster.raise_alarm( @@ -147,9 +147,9 @@ def deploy_thrusters(self, *args, **kwargs) -> None: def cb(terminal_state, result): if terminal_state == 3: - rospy.loginfo("Thrusters Deployed!") + self.get_logger().info("Thrusters Deployed!") else: - rospy.logwarn( + self.get_logger().warn( "Error deploying thrusters: {}, status: {}".format( TerminalState.to_string(terminal_state), result.status, @@ -166,9 +166,9 @@ def retract_thrusters(self, *args, **kwargs) -> None: def cb(terminal_state, result): if terminal_state == 3: - rospy.loginfo("Thrusters Retracted!") + self.get_logger().info("Thrusters Retracted!") else: - rospy.logwarn( + self.get_logger().warn( "Error rectracting thrusters: {}, status: {}".format( TerminalState.to_string(terminal_state), result.status, @@ -182,7 +182,7 @@ def select_autonomous_control(self, *args, **kwargs) -> None: """ Selects the autonomously generated trajectory as the active controller. """ - rospy.loginfo("Changing Control to Autonomous") + self.get_logger().info("Changing Control to Autonomous") self.wrench_changer("autonomous") @_timeout_check @@ -190,14 +190,14 @@ def select_rc_control(self, *args, **kwargs) -> None: """ Selects the Xbox remote joystick as the active controller. """ - rospy.loginfo("Changing Control to RC") + self.get_logger().info("Changing Control to RC") self.wrench_changer("rc") def select_emergency_control(self, *args, **kwargs) -> None: """ Selects the emergency controller as the active controller. """ - rospy.loginfo("Changing Control to Emergency Controller") + self.get_logger().info("Changing Control to Emergency Controller") self.wrench_changer("emergency") @_timeout_check @@ -205,7 +205,7 @@ def select_keyboard_control(self, *args, **kwargs) -> None: """ Selects the keyboard teleoperation service as the active controller. """ - rospy.loginfo("Changing Control to Keyboard") + self.get_logger().info("Changing Control to Keyboard") self.wrench_changer("keyboard") @_timeout_check @@ -214,14 +214,14 @@ def select_next_control(self, *args, **kwargs) -> None: Selects the autonomously generated trajectory as the active controller. """ mode = next(self.wrench_choices) - rospy.loginfo(f"Changing Control Mode: {mode}") + self.get_logger().info(f"Changing Control Mode: {mode}") self.wrench_changer(mode) def _shooter_load_feedback(self, status, result): """ Prints the feedback that is returned by the shooter load action client """ - rospy.loginfo( + self.get_logger().info( "Shooter Load Status={} Success={} Error={}".format( status, result.success, @@ -238,13 +238,13 @@ def shooter_load(self, *args, **kwargs) -> None: goal=ShooterDoActionGoal(), done_cb=self._shooter_load_feedback, ) - rospy.loginfo("Kip, do not throw away your shot.") + self.get_logger().info("Kip, do not throw away your shot.") def _shooter_fire_feedback(self, status, result) -> None: """ Prints the feedback that is returned by the shooter fire action client """ - rospy.loginfo( + self.get_logger().info( "Shooter Fire Status={} Success={} Error={}".format( status, result.success, @@ -262,7 +262,7 @@ def shooter_fire(self, *args, **kwargs) -> None: goal=ShooterDoActionGoal(), done_cb=self._shooter_fire_feedback, ) - rospy.loginfo( + self.get_logger().info( "One, two, three, four, five, six, seven, eight, nine. Number... TEN PACES! FIRE!", ) @@ -272,10 +272,10 @@ def shooter_cancel(self, *args, **kwargs) -> None: Cancels the process that the shooter action client is currently running. """ - rospy.loginfo("Canceling shooter requests") + self.get_logger().info("Canceling shooter requests") self.shooter_cancel_client(TriggerRequest()) - rospy.loginfo("I imaging death so much it feels more like a memory.") - rospy.loginfo( + self.get_logger().info("I imaging death so much it feels more like a memory.") + self.get_logger().info( "When's it gonna get me? While I'm blocked? Seven clocks ahead of me?", ) @@ -283,9 +283,9 @@ def _shooter_reset_helper(self, event): """ Used to actually call the shooter's reset service. """ - rospy.loginfo("Resetting the shooter service") + self.get_logger().info("Resetting the shooter service") self.shooter_reset_client(TriggerRequest()) - rospy.loginfo( + self.get_logger().info( "In New York you can be a new man! In New York you can be a new man!", ) @@ -296,7 +296,7 @@ def shooter_reset(self, *args, **kwargs) -> None: using a ~6s delay before calling the actual reset service. """ self.shooter_linear_retract() - rospy.Timer(rospy.Duration(6), self._shooter_reset_helper, oneshot=True) + self.create_timer(Duration(seconds=6), self._shooter_reset_helper) @_timeout_check def shooter_linear_extend(self, *args, **kwargs): @@ -304,7 +304,7 @@ def shooter_linear_extend(self, *args, **kwargs): Extends the shooter's linear actuator by setting it's speed to full forward. """ - rospy.loginfo("Extending the shooter's linear actuator") + self.get_logger().info("Extending the shooter's linear actuator") self.shooter_manual_client(1, 0) @_timeout_check @@ -313,7 +313,7 @@ def shooter_linear_retract(self, *args, **kwargs): Retracts the shooter's linear actuator by setting it's speed to full reverse. """ - rospy.loginfo("Retracting the shooter's linear actuator") + self.get_logger().info("Retracting the shooter's linear actuator") self.shooter_manual_client(-1, 0) @_timeout_check @@ -326,7 +326,9 @@ def set_disc_speed(self, speed: int, *args, **kwargs) -> None: Args: speed (int): The speed to set the shooter disc to. """ - rospy.loginfo(f"Setting the shooter's accelerator disc speed to {speed}") + self.get_logger().info( + f"Setting the shooter's accelerator disc speed to {speed}", + ) self.shooter_manual_client(0, float(speed) / -100) @_timeout_check @@ -351,7 +353,7 @@ def publish_wrench( If ``None``, then use the current time. """ if stamp is None: - stamp = rospy.Time.now() + stamp = self.get_clock().now() if self.wrench_pub is not None: wrench = WrenchStamped() @@ -371,7 +373,7 @@ def clear_wrench(self, *args, **kwargs) -> None: if self.wrench_pub is not None: wrench = WrenchStamped() wrench.header.frame_id = "/base_link" - wrench.header.stamp = rospy.Time.now() + wrench.header.stamp = self.get_clock().now() wrench.wrench.force.x = 0 wrench.wrench.force.y = 0 wrench.wrench.torque.z = 0