Ros2 navigator utils remote control

NaviGator/utils/remote_control/navigator_emergency_control/CMakeLists.txt

@@ -1,19 +1,39 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(navigator_emergency_controller)
 
-find_package(catkin REQUIRED COMPONENTS
-	roscpp
-	std_msgs
-	geometry_msgs
-	navigator_msg_multiplexer
-	nav_msgs
-)
+set(CMAKE_CXX_STANDARD 14)
 
-catkin_package(
-CATKIN_DEPENDS
-	std_msgs
-	nav_msgs
-	geometry_msgs
-	roscpp
-	navigator_msg_multiplexer
-)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+# find_package(catkin REQUIRED COMPONENTS
+# 	roscpp
+# 	std_msgs
+# 	geometry_msgs
+# 	navigator_msg_multiplexer
+# 	nav_msgs
+# )
+
+find_package(ament_cmake REQUIRED)
+find_package(roscpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(navigator_msg_multiplexer REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# catkin_package(
+# CATKIN_DEPENDS
+#	std_msgs
+#	nav_msgs
+#	geometry_msgs
+#	roscpp
+# 	navigator_msg_multiplexer
+# )
+
+ament_export_dependencies(std_msgs nav_msgs geometry_msgs roscpp navigator_msg_multiplexer)
+ament_package()

NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python3
-import rospy
+import sys
+
+import rclpy
 from remote_control_lib import RemoteControl
 from sensor_msgs.msg import Joy
 
@@ -32,11 +34,17 @@ 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 = node.declare_parameter(
+            "/joystick_wrench/force_scale",
+            600,
+        ).value
+        self.torque_scale = node.declare_parameter(
+            "/joystick_wrench/torque_scale",
+            500,
+        ).value
 
         self.remote = RemoteControl("emergency", "/wrench/emergency")
-        rospy.Subscriber("joy_emergency", Joy, self.joy_recieved)
+        node.create_subscription("joy_emergency", Joy, rclpy.qos.QoSProfile())
 
         self.active = False
         self.reset()
@@ -80,15 +88,15 @@ 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)
+                rclpy.Time.now() - self.last_joy.header.stamp > rclpy.Duration(15 * 60)
                 and self.active
             ):
-                rospy.logwarn("Controller Timed out. Hold start to resume.")
+                node.get_logger().warn("Controller Timed out. Hold start to resume.")
                 self.reset()
 
         else:
             joy.header.stamp = (
-                rospy.Time.now()
+                rclpy.Time.now()
             )  # In the sim, stamps weren't working right
             self.last_joy = joy
 
@@ -102,7 +110,7 @@ def joy_recieved(self, joy: Joy) -> None:
         Args:
             joy (Joy): The Joy message.
         """
-        self.last_time = rospy.Time.now()
+        self.last_time = rclpy.Time.now()
         self.check_for_timeout(joy)
 
         # Assigns readable names to the buttons that are used
@@ -116,14 +124,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")
+            node.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")
+            node.get_logger().info("Resetting controller state")
             self.reset()
             self.active = True
 
@@ -170,19 +178,20 @@ 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 rclpy.Time.now() - self.last_time > rclpy.Duration(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()
+    rclpy.Timer(rclpy.Duration(1), emergency.die_check, oneshot=False)
+    rclpy.spin()

NaviGator/utils/remote_control/navigator_emergency_control/package.xml

@@ -1,21 +1,20 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>navigator_emergency_controller</name>
   <version>4.0.0</version>
   <description>A server that controls navigator in response to the /joy_emergency node</description>
   <maintainer email="[email protected]">David Zobel</maintainer>
   <license>MIT</license>
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>message_runtime</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>navigator_msg_multiplexer</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>navigator_msg_multiplexer</run_depend>
-  <run_depend>nav_msgs</run_depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>geometry_msgs</depend>
+  <depend>message_runtime</depend>
+  <depend>nav_msgs</depend>
+  <depend>navigator_msg_multiplexer</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

NaviGator/utils/remote_control/navigator_joystick_control/CMakeLists.txt

@@ -1,19 +1,39 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(navigator_joystick_control)
+cmake_minimum_required(VERSION 3.5)
+project(navigator_emergency_controller)
 
-find_package(catkin REQUIRED COMPONENTS
-	roscpp
-	std_msgs
-	geometry_msgs
-	navigator_msg_multiplexer
-	nav_msgs
-)
+set(CMAKE_CXX_STANDARD 14)
 
-catkin_package(
-CATKIN_DEPENDS
-	std_msgs
-	nav_msgs
-	geometry_msgs
-	roscpp
-	navigator_msg_multiplexer
-)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+# find_package(catkin REQUIRED COMPONENTS
+# 	roscpp
+# 	std_msgs
+# 	geometry_msgs
+# 	navigator_msg_multiplexer
+# 	nav_msgs
+# )
+
+find_package(ament_cmake REQUIRED)
+find_package(roscpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(navigator_msg_multiplexer REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# catkin_package(
+# CATKIN_DEPENDS
+#	std_msgs
+#	nav_msgs
+#	geometry_msgs
+#	roscpp
+# 	navigator_msg_multiplexer
+# )
+
+ament_export_dependencies(std_msgs nav_msgs geometry_msgs roscpp navigator_msg_multiplexer)
+ament_package()

NaviGator/utils/remote_control/navigator_joystick_control/nodes/navigator_joystick.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python3
-import rospy
+import sys
+
+import rclpy
 from remote_control_lib import RemoteControl
 from sensor_msgs.msg import Joy
 
@@ -11,12 +13,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 = node.declare_parameter("~force_scale", 600).value
+        self.torque_scale = node.declare_parameter("~torque_scale", 500).value
 
         self.remote = RemoteControl("joystick", "/wrench/rc")
         self.reset()
-        rospy.Subscriber("joy", Joy, self.joy_recieved)
+        node.create_subscription("joy", Joy, self.joy_recieved, rclpy.qos.QoSProfile())
 
     def reset(self):
         """
@@ -49,15 +51,15 @@ 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 rclpy.Time.now() - self.last_joy.header.stamp > rclpy.Duration(15 * 60)
             and self.active
         ):
-            rospy.logwarn("Controller Timed out. Hold start to resume.")
+            node.get_logger().warn("Controller Timed out. Hold start to resume.")
             self.reset()
 
         else:
             joy.header.stamp = (
-                rospy.Time.now()
+                rclpy.Time.now()
             )  # In the sim, stamps weren't working right
             self.last_joy = joy
 
@@ -78,14 +80,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")
+            node.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")
+            node.get_logger().info("Resetting controller state")
             self.reset()
             self.active = True
 
@@ -143,11 +145,12 @@ 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, rclpy.Time.now())
 
 
 if __name__ == "__main__":
-    rospy.init_node("joystick")
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("joystick")
 
     joystick = Joystick()
-    rospy.spin()
+    rclpy.spin()

NaviGator/utils/remote_control/navigator_joystick_control/package.xml

@@ -1,22 +1,23 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>navigator_joystick_control</name>
   <version>4.0.0</version>
   <description>A server that controls navigator in response to the /Joy node</description>
   <maintainer email="[email protected]">Anthony Olive</maintainer>
   <license>MIT</license>
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>message_runtime</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>navigator_msg_multiplexer</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>navigator_msg_multiplexer</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>ros_alarms</run_depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>geometry_msgs</depend>
+  <depend>message_runtime</depend>
+  <depend>nav_msgs</depend>
+  <depend>navigator_msg_multiplexer</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+
+  <build_export_depend>ros_alarms</build_export_depend>
+  <exec_depend>ros_alarms</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

ruff.toml

@@ -20,6 +20,7 @@ extend-exclude = [
     "NaviGator/simulation/VRX/vrx",
     "NaviGator/simulation/VRX/vrx-docker",
     "mil_common/perception/yolov7-ros",
+    "mil_common/perception/vision_stack",
     # sub simulation
     "SubjuGator/simulation",
     # docker contents