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