From e515e0fd0efb42283ebb427bc50ad9c38e02d9f8 Mon Sep 17 00:00:00 2001 From: Jorge Ramirez Date: Thu, 2 May 2024 18:20:17 -0400 Subject: [PATCH 1/2] test commit --- .../navigator_joystick_control/CMakeLists.txt | 54 +++++++++++++------ 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/NaviGator/utils/remote_control/navigator_joystick_control/CMakeLists.txt b/NaviGator/utils/remote_control/navigator_joystick_control/CMakeLists.txt index 20d2de421..f72b67fd3 100644 --- a/NaviGator/utils/remote_control/navigator_joystick_control/CMakeLists.txt +++ b/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() From 2f5e024f8d4320e717466b3dc7e026089ed5a618 Mon Sep 17 00:00:00 2001 From: Jorge Ramirez Date: Thu, 2 May 2024 19:11:03 -0400 Subject: [PATCH 2/2] migrated joystick_control & emergency_control --- .../CMakeLists.txt | 52 +++++++++++++------ .../nodes/navigator_emergency.py | 39 ++++++++------ .../navigator_emergency_control/package.xml | 27 +++++----- .../nodes/navigator_joystick.py | 27 +++++----- .../navigator_joystick_control/package.xml | 31 +++++------ ruff.toml | 1 + 6 files changed, 105 insertions(+), 72 deletions(-) diff --git a/NaviGator/utils/remote_control/navigator_emergency_control/CMakeLists.txt b/NaviGator/utils/remote_control/navigator_emergency_control/CMakeLists.txt index 8452495ed..f72b67fd3 100644 --- a/NaviGator/utils/remote_control/navigator_emergency_control/CMakeLists.txt +++ b/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() 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..f4be26522 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,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() diff --git a/NaviGator/utils/remote_control/navigator_emergency_control/package.xml b/NaviGator/utils/remote_control/navigator_emergency_control/package.xml index 42483702b..b47dfa027 100644 --- a/NaviGator/utils/remote_control/navigator_emergency_control/package.xml +++ b/NaviGator/utils/remote_control/navigator_emergency_control/package.xml @@ -1,21 +1,20 @@ - + navigator_emergency_controller 4.0.0 A server that controls navigator in response to the /joy_emergency node David Zobel MIT - catkin - geometry_msgs - message_runtime - nav_msgs - navigator_msg_multiplexer - roscpp - std_msgs - roscpp - std_msgs - geometry_msgs - message_runtime - navigator_msg_multiplexer - nav_msgs + + ament_cmake + geometry_msgs + message_runtime + nav_msgs + navigator_msg_multiplexer + roscpp + std_msgs + + + ament_cmake + 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..69923663a 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,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() diff --git a/NaviGator/utils/remote_control/navigator_joystick_control/package.xml b/NaviGator/utils/remote_control/navigator_joystick_control/package.xml index 22861b990..7711e0441 100644 --- a/NaviGator/utils/remote_control/navigator_joystick_control/package.xml +++ b/NaviGator/utils/remote_control/navigator_joystick_control/package.xml @@ -1,22 +1,23 @@ - + navigator_joystick_control 4.0.0 A server that controls navigator in response to the /Joy node Anthony Olive MIT - catkin - geometry_msgs - message_runtime - nav_msgs - navigator_msg_multiplexer - roscpp - std_msgs - roscpp - std_msgs - geometry_msgs - message_runtime - navigator_msg_multiplexer - nav_msgs - ros_alarms + + ament_cmake + geometry_msgs + message_runtime + nav_msgs + navigator_msg_multiplexer + roscpp + std_msgs + + ros_alarms + ros_alarms + + + ament_cmake + diff --git a/ruff.toml b/ruff.toml index 56794e48d..dbbbbc597 100644 --- a/ruff.toml +++ b/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