From 10cfae749d120086d9b52fdb149ec48d9b680bb5 Mon Sep 17 00:00:00 2001 From: Nihar Devireddy Date: Fri, 22 Mar 2024 14:12:00 -0400 Subject: [PATCH 1/5] navigator_battery_monitor package migrated to ros2 --- .../nodes/navigator_battery_monitor.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py b/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py index 13876a8ef..a7f03a5dd 100755 --- a/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py +++ b/NaviGator/utils/navigator_battery_monitor/nodes/navigator_battery_monitor.py @@ -6,8 +6,10 @@ """ +import sys + import message_filters -import rospy +import rclpy from roboteq_msgs.msg import Feedback, Status from ros_alarms import AlarmListener from ros_alarms_msgs.msg import Alarm @@ -49,7 +51,7 @@ def __init__(self): self._hw_kill_listener.wait_for_server() # The publisher for the averaged voltage - self.pub_voltage = rospy.Publisher("/battery_monitor", Float32, queue_size=1) + self.pub_voltage = node.create_publisher(Float32, "/battery_monitor", 1) # Subscribes to the feedback from each of the four thrusters motor_topics = ["/FL_motor", "/FR_motor", "/BL_motor", "/BR_motor"] @@ -117,7 +119,8 @@ def publish_voltage(self, _) -> None: if __name__ == "__main__": - rospy.init_node("battery_monitor") + rclpy.init(args=sys.argv) + node = rclpy.create_node("battery_monitor") monitor = BatteryMonitor() - rospy.Timer(rospy.Duration(1), monitor.publish_voltage, oneshot=False) - rospy.spin() + node.create_timer(1.0, monitor.publish_voltage) + rclpy.spin() From 4d5ac5f9811d40ab88fed691013ecf3ca4335afd Mon Sep 17 00:00:00 2001 From: Nihar Devireddy Date: Mon, 15 Apr 2024 09:36:52 -0400 Subject: [PATCH 2/5] sub8 thrust and kill board package migrated to ros2 --- .../sub8_thrust_and_kill_board/CMakeLists.txt | 7 ------- .../sub8_thrust_and_kill_board/package.xml | 3 +++ .../sub8_thrust_and_kill_board/handle.py | 20 +++++++++--------- .../sub8_thrust_and_kill_board/simulation.py | 21 ++++++++++--------- 4 files changed, 24 insertions(+), 27 deletions(-) delete mode 100644 SubjuGator/drivers/sub8_thrust_and_kill_board/CMakeLists.txt diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/CMakeLists.txt b/SubjuGator/drivers/sub8_thrust_and_kill_board/CMakeLists.txt deleted file mode 100644 index 699568ff0..000000000 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(sub8_thrust_and_kill_board) -find_package(catkin REQUIRED COMPONENTS - mil_usb_to_can -) -catkin_python_setup() -catkin_package() diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/package.xml b/SubjuGator/drivers/sub8_thrust_and_kill_board/package.xml index 5dfb49e92..debf6fc39 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/package.xml +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/package.xml @@ -10,4 +10,7 @@ mil_usb_to_can mil_usb_to_can mil_usb_to_can + + ament_python + diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py index 19df02c33..fe7591af1 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py @@ -1,9 +1,9 @@ #!/usr/bin/python3 -import rospy +import rclpy from mil_usb_to_can.sub8 import CANDeviceHandle +from rclpy.timer import TimerEvent from ros_alarms import AlarmBroadcaster, AlarmListener from ros_alarms_msgs.msg import Alarm -from rospy.timer import TimerEvent from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from subjugator_msgs.msg import Thrust @@ -27,7 +27,7 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) # Initialize thruster mapping from params self.thrusters, self.name_to_id = make_thruster_dictionary( - rospy.get_param("/thruster_layout/thrusters"), + self.declare_parameter("/thruster_layout/thrusters").value, ) # Tracks last hw-kill alarm update self._last_hw_kill = None @@ -43,19 +43,19 @@ def __init__(self, *args, **kwargs): callback_funct=self.on_hw_kill, ) # Provide service for alarm handler to set/clear the motherboard kill - self._unkill_service = rospy.Service( - "/set_mobo_kill", + self._unkill_service = self.create_service( SetBool, + "/set_mobo_kill", self.set_mobo_kill, ) # Sends heartbeat to board - self._heartbeat_timer = rospy.Timer(rospy.Duration(0.4), self.send_heartbeat) + self._heartbeat_timer = rclpy.create_timer(0.4, self.send_heartbeat) # Create a subscribe for thruster commands - self._sub = rospy.Subscriber( - "/thrusters/thrust", + self._sub = self.create_subscription( Thrust, + "/thrusters/thrust", self.on_command, - queue_size=10, + 10, ) def set_mobo_kill(self, req: SetBoolRequest) -> SetBoolResponse: @@ -108,7 +108,7 @@ def on_command(self, msg: Thrust) -> None: for cmd in msg.thruster_commands: # If we don't have a mapping for this thruster, ignore it if cmd.name not in self.thrusters: - rospy.logwarn( + self.get_logger().warn( f"Command received for {cmd.name}, but this is not a thruster.", ) continue diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py index 83c595815..66ade6750 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py @@ -1,6 +1,7 @@ #!/usr/bin/python3 -import rospy +import rclpy from mil_usb_to_can.sub8 import SimulatedCANDevice +from rclpy.duration import Duration from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from .packets import ( @@ -27,7 +28,7 @@ class ThrusterAndKillBoardSimulation(SimulatedCANDevice): soft_kill_mobo (bool): Whether the motherboard experienced a soft kill request. """ - HEARTBEAT_TIMEOUT_SECONDS = rospy.Duration(1.0) + HEARTBEAT_TIMEOUT_SECONDS = Duration(seconds=1.0) def __init__(self, *args, **kwargs): self.hard_kill_plug_pulled = False @@ -37,18 +38,18 @@ def __init__(self, *args, **kwargs): self.go_button = False self._last_heartbeat = None super().__init__(*args, **kwargs) - self._update_timer = rospy.Timer(rospy.Duration(1), self.send_updates) - self._soft_kill = rospy.Service( - "/simulate_soft_kill", + self._update_timer = rclpy.create_timer(1.0, self.send_updates) + self._soft_kill = self.create_service( SetBool, + "/simulate_soft_kill", self.set_soft_kill, ) - self._hard_kill = rospy.Service( - "/simulate_hard_kill", + self._hard_kill = self.create_service( SetBool, + "/simulate_hard_kill", self.set_hard_kill, ) - self._go_srv = rospy.Service("/simulate_go", SetBool, self._on_go_srv) + self._go_srv = self.create_service(SetBool, "/simulate_go", self._on_go_srv) def _on_go_srv(self, req): self.go_button = req.data @@ -102,7 +103,7 @@ def heartbeat_timedout(self) -> bool: """ return ( self._last_heartbeat is None - or (rospy.Time.now() - self._last_heartbeat) + or (self.get_clock().now() - self._last_heartbeat) > self.HEARTBEAT_TIMEOUT_SECONDS ) @@ -154,6 +155,6 @@ def on_data(self, data: bytes, can_id: int) -> None: packet = ThrustPacket.from_bytes(data) elif data[0] == HeartbeatMessage.IDENTIFIER: packet = HeartbeatMessage.from_bytes(data) - self._last_heartbeat = rospy.Time.now() + self._last_heartbeat = self.get_clock().now() else: raise Exception("No recognized identifier") From 27091f6a02b48487d1b886b0c73bae3a06f65bf8 Mon Sep 17 00:00:00 2001 From: Nihar Devireddy Date: Mon, 15 Apr 2024 20:26:39 -0400 Subject: [PATCH 3/5] navigator/mission_control/navigator_alarm package migrated to ros2 --- .../navigator_alarm/CMakeLists.txt | 14 -------------- .../navigator_alarm_handlers/battery_voltage.py | 11 +++++------ .../navigator_alarm_handlers/kill.py | 15 ++++++++------- .../navigator_alarm_handlers/odom_kill.py | 7 +++---- .../navigator_alarm_handlers/station_hold.py | 7 +++---- .../navigator_alarm_handlers/thruster_fault.py | 8 ++++++-- .../mission_control/navigator_alarm/package.xml | 5 ++++- 7 files changed, 29 insertions(+), 38 deletions(-) delete mode 100644 NaviGator/mission_control/navigator_alarm/CMakeLists.txt diff --git a/NaviGator/mission_control/navigator_alarm/CMakeLists.txt b/NaviGator/mission_control/navigator_alarm/CMakeLists.txt deleted file mode 100644 index 7594cbea8..000000000 --- a/NaviGator/mission_control/navigator_alarm/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(navigator_alarm) - -find_package(catkin REQUIRED - COMPONENTS - rospy - mil_tools - nav_msgs - ros_alarms -) - -catkin_python_setup() - -catkin_package() diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py index c10b13c91..4a5527c87 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py @@ -1,4 +1,3 @@ -import rospy from ros_alarms import AlarmBroadcaster, HandlerBase from std_msgs.msg import Float32 @@ -8,13 +7,13 @@ class BatteryVoltage(HandlerBase): def __init__(self): self.broadcaster = AlarmBroadcaster(self.alarm_name) - self.low_threshold = rospy.get_param("~battery-voltage/low") - self.critical_threshold = rospy.get_param("~battery-voltage/critical") - self.voltage_sub = rospy.Subscriber( - "/battery_monitor", + self.low_threshold = self.declare_parameter("~battery-voltage/low") + self.critical_threshold = self.declare_parameter("~battery-voltage/critical") + self.voltage_sub = self.create_subscription( Float32, + "/battery_monitor", self._check_voltage, - queue_size=3, + 3, ) self._raised = False self._severity = 0 diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py index 547c56fa8..540497f35 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import os -import rospy from actionlib import SimpleActionClient, TerminalState from mil_missions_core import MissionClient from mil_msgs.msg import BagOnlineAction, BagOnlineGoal @@ -25,17 +24,17 @@ def __init__(self): def _online_bagger_cb(self, status, result): if status == 3: - rospy.loginfo(f"KILL BAG WRITTEN TO {result.filename}") + self.get_logger().info(f"KILL BAG WRITTEN TO {result.filename}") else: - rospy.logwarn( + self.get_logger().warn( f"KILL BAG {TerminalState.to_string(status)}, status: {result.status}", ) def _kill_task_cb(self, status, result): if status == 3: - rospy.loginfo("Killed task success!") + self.get_logger().info("Killed task success!") return - rospy.logwarn( + self.get_logger().warn( f"Killed task failed ({TerminalState.to_string(status)}): {result.result}", ) @@ -45,7 +44,9 @@ def raised(self, alarm): self.first = False return if "BAG_ALWAYS" not in os.environ or "bag_kill" not in os.environ: - rospy.logwarn("BAG_ALWAYS or BAG_KILL not set. Not making kill bag.") + self.get_logger().warn( + "BAG_ALWAYS or BAG_KILL not set. Not making kill bag.", + ) else: goal = BagOnlineGoal(bag_name="kill.bag") goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["BAG_KILL"] @@ -72,7 +73,7 @@ def meta_predicate(self, meta_alarm, alarms): return Alarm( "kill", True, - node_name=rospy.get_name(), + node_name=self.get_name(), problem_description="Killed by meta alarm(s) " + ", ".join(raised), parameters={"Raised": raised}, ) diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py index d0fa63afd..803f53e10 100755 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 import numpy as np -import rospy from mil_ros_tools import rosmsg_to_numpy from nav_msgs.msg import Odometry from ros_alarms import AlarmBroadcaster, HandlerBase, HeartbeatMonitor @@ -32,12 +31,12 @@ def __init__(self): prd=self.TIMEOUT_SECONDS, ) self.MAX_JUMP = 0.5 - self.launch_time = rospy.Time.now() + self.launch_time = self.get_clock().now() self.last_time = self.launch_time self.last_position = None self._raised = False self.ab = AlarmBroadcaster("odom-kill", node_name="odom-kill") - rospy.Subscriber("/odom", Odometry, self.check_continuity, queue_size=5) + self.create_subscription(Odometry, "/odom", self.check_continuity, 5) def check_continuity(self, odom): """ @@ -49,7 +48,7 @@ def check_continuity(self, odom): jump = np.linalg.norm(position - self.last_position) if jump > self.MAX_JUMP and not self._raised: self._raised = True # Avoid raising multiple times - rospy.logwarn("ODOM DISCONTINUITY DETECTED") + self.get_logger().warn("ODOM DISCONTINUITY DETECTED") self.ab.raise_alarm( problem_description=f"ODOM DISCONTINUITY DETECTED. JUMPED {jump} METERS", severity=5, diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/station_hold.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/station_hold.py index 82e11ccdd..11f50821d 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/station_hold.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/station_hold.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import rospy from actionlib import TerminalState from mil_missions_core import MissionClient from ros_alarms import AlarmBroadcaster, HandlerBase @@ -14,18 +13,18 @@ def __init__(self): def _client_cb(self, terminal_state, result): if terminal_state != 3: - rospy.logwarn( + self.get_logger().warn( "Station hold goal failed (Status={}, Result={})".format( TerminalState.to_string(terminal_state), result.result, ), ) return - rospy.loginfo("Station holding!") + self.get_logger().info("Station holding!") self.broadcaster.clear_alarm() def raised(self, alarm): - rospy.loginfo("Attempting to station hold") + self.get_logger().info("Attempting to station hold") self.task_client.run_mission("StationHold", done_cb=self._client_cb) def cleared(self, alarm): diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py index ce89ac3a0..0b870bf67 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py @@ -1,4 +1,3 @@ -import rospy from roboteq_msgs.msg import Status from ros_alarms import AlarmBroadcaster, HandlerBase @@ -29,7 +28,12 @@ def __init__(self): # Subscribe to the status message for all thruster topics [ - rospy.Subscriber(topic + "/status", Status, self._check_faults, topic) + self.create_subscription( + Status, + topic + "/status", + self._check_faults, + topic, + ) for topic in motor_topics ] diff --git a/NaviGator/mission_control/navigator_alarm/package.xml b/NaviGator/mission_control/navigator_alarm/package.xml index e7b8c4be6..669eafdc0 100644 --- a/NaviGator/mission_control/navigator_alarm/package.xml +++ b/NaviGator/mission_control/navigator_alarm/package.xml @@ -8,6 +8,9 @@ catkin mil_tools nav_msgs + rclpy ros_alarms - rospy + + ament_python + From f6786ea62175f0fb38b26c1a3a1e5e6233f6c15b Mon Sep 17 00:00:00 2001 From: Nihar Devireddy Date: Wed, 17 Apr 2024 13:41:51 -0400 Subject: [PATCH 4/5] added CMakeLists.txt file back to navigator_alarm package --- .../mission_control/navigator_alarm/CMakeLists.txt | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 NaviGator/mission_control/navigator_alarm/CMakeLists.txt diff --git a/NaviGator/mission_control/navigator_alarm/CMakeLists.txt b/NaviGator/mission_control/navigator_alarm/CMakeLists.txt new file mode 100644 index 000000000..7594cbea8 --- /dev/null +++ b/NaviGator/mission_control/navigator_alarm/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.0.2) +project(navigator_alarm) + +find_package(catkin REQUIRED + COMPONENTS + rospy + mil_tools + nav_msgs + ros_alarms +) + +catkin_python_setup() + +catkin_package() From 87349354b31e06d6e9dea1a8e219030d301fa3a5 Mon Sep 17 00:00:00 2001 From: Nihar Devireddy Date: Sun, 28 Apr 2024 16:07:45 -0400 Subject: [PATCH 5/5] 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