From 440d35cb37e9869c202bc3f0f9eef010cd34a4c6 Mon Sep 17 00:00:00 2001 From: Alex Perez Date: Sat, 12 Nov 2022 12:16:53 +1100 Subject: [PATCH 1/9] Added robotx comms support for simulation --- .../navigator_missions/navigator.py | 12 ++++++++++++ .../navigator_robotx_comms/navigator_robotx_comms.py | 8 ++++++-- .../nodes/robotx_comms_client.py | 10 ++++++++++ 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py index 228d0627c..f6f26b36b 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py @@ -3,6 +3,7 @@ import asyncio import os +from enum import IntEnum from typing import Callable import genpy @@ -42,6 +43,16 @@ from .pose_editor import PoseEditor2 +class UAVModes(IntEnum): + """ + Enumerates constants of friendly uav mode names to ints + """ + + STOWED = 1 + DEPLOYED = 2 + FAULTED = 3 + + class MissionResult: NoResponse = 0 ParamNotFound = 1 @@ -127,6 +138,7 @@ async def _init(cls, mission_runner): ) cls.pose = None + cls.uav_status = UAVModes.STOWED def odom_set(odom: Odometry): return setattr(cls, "pose", mil_tools.odometry_to_numpy(odom)[0]) diff --git a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py index b441c1eec..6f88db2d4 100644 --- a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py +++ b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py @@ -107,8 +107,12 @@ def to_string( latitude = gps_array.point.x longitude = gps_array.point.y else: - latitude = "" - longitude = "" + # latitude = "" + # longitude = "" + + # use only for qualifying, delete later and uncomment above!! + latitude = odom.pose.pose.position.x + longitude = odom.pose.pose.position.y if odom is not None: quaternion = odom.pose.pose.orientation diff --git a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py index 7b76d4bdb..51e5136bc 100755 --- a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py +++ b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py @@ -66,6 +66,16 @@ class SystemModes(IntEnum): REMOTE_CONTROLLED = 1 +class UAVModes(IntEnum): + """ + Enumerates constants of friendly uav mode names to ints + """ + + STOWED = 1 + DEPLOYED = 2 + FAULTED = 3 + + class RobotXStartServices: """ Initializes services and subscribes to necessary publishers in order to facilitate From b057e75aabcdeaef36f5a9179a79b29089f6ac61 Mon Sep 17 00:00:00 2001 From: andrew-aj Date: Sun, 13 Nov 2022 12:16:23 +1100 Subject: [PATCH 2/9] killboard updates --- .../navigator_kill_board/constants.py | 80 +++++----- .../nodes/kill_board_driver.py | 141 ++++++++++++------ .../launch/hardware/test_board.launch | 20 +++ 3 files changed, 153 insertions(+), 88 deletions(-) create mode 100644 NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch diff --git a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py index 83663e034..a286239a3 100644 --- a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py +++ b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py @@ -20,57 +20,57 @@ constants = { "TIMEOUT_SECONDS": 8.0, # How often board must be pinged to not set HEARTBERAT_REMOTE True # Note: not official documented, this is just a guess - "RESPONSE_FALSE": "\x00", # True status for synchronous requests of individual addresses - "RESPONSE_TRUE": "\x01", # False status for synchronous requests of individual addresses - "PING": {"REQUEST": "\x20", "RESPONSE": "\x30"}, + "RESPONSE_FALSE": b"\x00", # True status for synchronous requests of individual addresses + "RESPONSE_TRUE": b"\x01", # False status for synchronous requests of individual addresses + "PING": {"REQUEST": b"\x20", "RESPONSE": b"\x30"}, "KILLS": [ "OVERALL", "BUTTON_FRONT_PORT", "BUTTON_AFT_PORT", "BUTTON_FRONT_STARBOARD", "BUTTON_AFT_STARBOARD", - "HEARTBEAT_COMPUTER", + # "HEARTBEAT_COMPUTER", "BUTTON_REMOTE", - "HEARTBEAT_REMOTE", + # "HEARTBEAT_REMOTE", "COMPUTER", ], "OVERALL": { # Should be True if any of the over are True - "REQUEST": "\x21", - "TRUE": "\x10", - "FALSE": "\x11", + "REQUEST": b"\x21", + "TRUE": b"\x10", + "FALSE": b"\x11", }, - "BUTTON_FRONT_PORT": {"REQUEST": "\x22", "TRUE": "\x12", "FALSE": "\x13"}, - "BUTTON_AFT_PORT": {"REQUEST": "\x23", "TRUE": "\x14", "FALSE": "\x15"}, - "BUTTON_FRONT_STARBOARD": {"REQUEST": "\x24", "TRUE": "\x16", "FALSE": "\x17"}, - "BUTTON_AFT_STARBOARD": {"REQUEST": "\x25", "TRUE": "\x18", "FALSE": "\x19"}, + "BUTTON_FRONT_PORT": {"REQUEST": b"\x22", "TRUE": b"\x12", "FALSE": b"\x13"}, + "BUTTON_AFT_PORT": {"REQUEST": b"\x23", "TRUE": b"\x14", "FALSE": b"\x15"}, + "BUTTON_FRONT_STARBOARD": {"REQUEST": b"\x24", "TRUE": b"\x16", "FALSE": b"\x17"}, + "BUTTON_AFT_STARBOARD": {"REQUEST": b"\x25", "TRUE": b"\x18", "FALSE": b"\x19"}, "HEARTBEAT_COMPUTER": { # Will return True if board is not pinged by mobo often enough - "REQUEST": "\x26", - "TRUE": "\x1A", - "FALSE": "\x1B", + "REQUEST": b"\x26", + "TRUE": b"\x1A", + "FALSE": b"\x1B", }, - "BUTTON_REMOTE": {"REQUEST": "\x28", "TRUE": "\x3A", "FALSE": "\x3B"}, + "BUTTON_REMOTE": {"REQUEST": b"\x28", "TRUE": b"\x3A", "FALSE": b"\x3B"}, "HEARTBEAT_REMOTE": { # Will return True if board is not pinged by controller often enough - "REQUEST": "\x29", - "TRUE": "\x3C", - "FALSE": "\x3D", + "REQUEST": b"\x29", + "TRUE": b"\x3C", + "FALSE": b"\x3D", }, "COMPUTER": { # Allows board to be killed over serial (like through ROS) - "KILL": {"REQUEST": "\x45", "RESPONSE": "\x55"}, - "CLEAR": {"REQUEST": "\x46", "RESPONSE": "\x56"}, - "REQUEST": "\x27", - "TRUE": "\x1C", - "FALSE": "\x1D", + "KILL": {"REQUEST": b"\x45", "RESPONSE": b"\x55"}, + "CLEAR": {"REQUEST": b"\x46", "RESPONSE": b"\x56"}, + "REQUEST": b"\x27", + "TRUE": b"\x1C", + "FALSE": b"\x1D", }, - "CONNECTED": {"TRUE": "\x1E", "FALSE": "\x1F"}, + "CONNECTED": {"TRUE": b"\x1E", "FALSE": b"\x1F"}, "LIGHTS": { # Note: YELLOW turns off GREEN and visa versa - "OFF_REQUEST": "\x40", - "OFF_RESPONSE": "\x50", - "YELLOW_REQUEST": "\x41", - "YELLOW_RESPONSE": "\x51", - "GREEN_REQUEST": "\x42", - "GREEN_RESPONSE": "\x52", + "OFF_REQUEST": b"\x40", + "OFF_RESPONSE": b"\x50", + "YELLOW_REQUEST": b"\x41", + "YELLOW_RESPONSE": b"\x51", + "GREEN_REQUEST": b"\x42", + "GREEN_RESPONSE": b"\x52", }, - "CONTROLLER": "\xA0", # Signifies the start of a controller message (joysticks & buttons) + "CONTROLLER": b"\xA0", # Signifies the start of a controller message (joysticks & buttons) # Immediately followed by 8 bytes: 6 joystick bytes, 2 button bytes # Joystick message is 3 signed ints from -2048 to 2047 # Button message is 16 bits signifying up to 16 buttons on/off @@ -86,13 +86,13 @@ "EMERGENCY_CONTROL", ], "CTRL_BUTTONS_VALUES": { # Amount of buttons and labels will be changed in the future - "STATION_HOLD": "\x00\x01", # Button 0 - "RAISE_KILL": "\x00\x02", # Button 1 - "CLEAR_KILL": "\x00\x04", # Button 2 - "THRUSTER_RETRACT": "\x00\x10", # Button 4 - "THRUSTER_DEPLOY": "\x00\x20", # Button 5 - "GO_INACTIVE": "\x00\x40", # Button 6 - "START": "\x00\x80", # Button 7 - "EMERGENCY_CONTROL": "\x20\x00", # Button 13 + "STATION_HOLD": b"\x00\x01", # Button 0 + "RAISE_KILL": b"\x00\x02", # Button 1 + "CLEAR_KILL": b"\x00\x04", # Button 2 + "THRUSTER_RETRACT": b"\x00\x10", # Button 4 + "THRUSTER_DEPLOY": b"\x00\x20", # Button 5 + "GO_INACTIVE": b"\x00\x40", # Button 6 + "START": b"\x00\x80", # Button 7 + "EMERGENCY_CONTROL": b"\x20\x00", # Button 13 }, } diff --git a/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py b/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py index 223318a65..374fb1b49 100755 --- a/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py +++ b/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py @@ -11,6 +11,7 @@ from mil_tools import thread_lock from navigator_alarm_handlers import NetworkLoss from navigator_kill_board import constants +from roboteq_msgs.msg import Command from ros_alarms import AlarmBroadcaster, AlarmListener from sensor_msgs.msg import Joy from std_msgs.msg import String @@ -82,6 +83,15 @@ def __init__(self): "CTRL_STICKS" ]: # These are 3 signed 16-bit values for stick positions self.sticks[stick] = 0x0000 + self.thrusters = {} + for thruster in ["BL", "BR", "FL", "FR"]: + self.thrusters[thruster] = 0x00 + self.rc_pub = rospy.Publisher("/wrench/selected", String) + self.motor_pubs = {} + for thruster in self.thrusters: + self.motor_pubs = rospy.Publisher( + f"/{thruster}_motor/cmd", Command, queue_size=1 + ) self.sticks_temp = 0x0000 self.buttons = {} for button in constants[ @@ -131,7 +141,8 @@ def update_ros(self): self.update_hw_kill() self.publish_diagnostics() if self.ctrl_msg_received is True: - self.publish_joy() + # self.publish_joy() + self.publish_thrust() self.ctrl_msg_received = False def handle_byte(self, msg): @@ -140,12 +151,14 @@ def handle_byte(self, msg): a known response to a recent request """ # If the controller message start byte is received, next 8 bytes are the controller data + # print("here") + # print(msg) if msg == constants["CONTROLLER"]: - self.ctrl_msg_count = 8 + self.ctrl_msg_count = 4 self.ctrl_msg_timeout = rospy.Time.now() return # If receiving the controller message, record the byte as stick/button data - if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 8): + if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 4): # If 1 second has passed since the message began, timeout and report warning if (rospy.Time.now() - self.ctrl_msg_timeout) >= rospy.Duration(1): self.ctrl_msg_received = False @@ -153,55 +166,65 @@ def handle_byte(self, msg): rospy.logwarn( "Timeout receiving controller message. Please disconnect controller." ) - if ( - self.ctrl_msg_count > 2 - ): # The first 6 bytes in the message are stick data bytes - if ( - self.ctrl_msg_count % 2 - ) == 0: # Even number byte: first byte in data word - self.sticks_temp = int(msg.encode("hex"), 16) << 8 - else: # Odd number byte: combine two bytes into a stick's data word - self.sticks_temp += int(msg.encode("hex"), 16) - if self.ctrl_msg_count > 6: - self.sticks["UD"] = self.sticks_temp - elif self.ctrl_msg_count > 4: - self.sticks["LR"] = self.sticks_temp - else: - self.sticks["TQ"] = self.sticks_temp - self.sticks_temp = 0x0000 - else: # The last 2 bytes are button data bytes - if (self.ctrl_msg_count % 2) == 0: - self.buttons_temp = int(msg.encode("hex"), 16) << 8 - else: # Combine two bytes into the button data word - self.buttons_temp += int(msg.encode("hex"), 16) - for ( - button - ) in ( - self.buttons - ): # Each of the 16 bits represents a button on/off state - button_check = int( - constants["CTRL_BUTTONS_VALUES"][button].encode("hex"), 16 - ) - self.buttons[button] = ( - self.buttons_temp & button_check - ) == button_check - self.buttons_temp = 0x0000 - self.ctrl_msg_received = ( - True # After receiving last byte, trigger joy update - ) + # [HEADER] [BL] [BR] [FL] [FR] + if self.ctrl_msg_count == 4: + self.thrusters["BL"] = int(msg.encode("hex", 16)) + elif self.ctrl_msg_count == 3: + self.thrusters["BR"] = int(msg.encode("hex", 16)) + elif self.ctrl_msg_count == 2: + self.thrusters["FL"] = int(msg.encode("hex", 16)) + elif self.ctrl_msg_count == 1: + self.thrusters["FR"] = int(msg.encode("hex", 16)) + self.ctrl_msg_received = True + # if ( + # self.ctrl_msg_count > 2 + # ): # The first 6 bytes in the message are stick data bytes + # if ( + # self.ctrl_msg_count % 2 + # ) == 0: # Even number byte: first byte in data word + # self.sticks_temp = int(msg.encode("hex"), 16) << 8 + # else: # Odd number byte: combine two bytes into a stick's data word + # self.sticks_temp += int(msg.encode("hex"), 16) + # if self.ctrl_msg_count > 6: + # self.sticks["UD"] = self.sticks_temp + # elif self.ctrl_msg_count > 4: + # self.sticks["LR"] = self.sticks_temp + # else: + # self.sticks["TQ"] = self.sticks_temp + # self.sticks_temp = 0x0000 + # else: # The last 2 bytes are button data bytes + # if (self.ctrl_msg_count % 2) == 0: + # self.buttons_temp = int(msg.encode("hex"), 16) << 8 + # else: # Combine two bytes into the button data word + # self.buttons_temp += int(msg.encode("hex"), 16) + # for ( + # button + # ) in ( + # self.buttons + # ): # Each of the 16 bits represents a button on/off state + # button_check = int( + # constants["CTRL_BUTTONS_VALUES"][button].encode("hex"), 16 + # ) + # self.buttons[button] = ( + # self.buttons_temp & button_check + # ) == button_check + # self.buttons_temp = 0x0000 + # self.ctrl_msg_received = ( + # True # After receiving last byte, trigger joy update + # ) self.ctrl_msg_count -= 1 return # If a response has been received to a requested status (button, remove, etc), update internal state if self.last_request is not None: if msg == constants["RESPONSE_FALSE"]: if self.board_status[self.last_request] is True: - rospy.logdebug(f"SYNC FALSE for {self.last_request}") + rospy.logerr(f"SYNC FALSE for {self.last_request}") self.board_status[self.last_request] = False self.last_request = None return if msg == constants["RESPONSE_TRUE"]: if self.board_status[self.last_request] is False: - rospy.logdebug(f"SYNC TRUE for {self.last_request}") + rospy.logerr(f"SYNC TRUE for {self.last_request}") self.board_status[self.last_request] = True self.last_request = None return @@ -209,12 +232,12 @@ def handle_byte(self, msg): for kill in self.board_status: if msg == constants[kill]["FALSE"]: if self.board_status[kill] is True: - rospy.logdebug(f"ASYNC FALSE for {self.last_request}") + rospy.logerr(f"ASYNC FALSE for {kill}") self.board_status[kill] = False return if msg == constants[kill]["TRUE"]: if self.board_status[kill] is False: - rospy.logdebug(f"ASYNC TRUE FOR {kill}") + rospy.logerr(f"ASYNC TRUE FOR {kill}") self.board_status[kill] = True return # If a response to another request, like ping or computer kill/clear is received @@ -249,6 +272,7 @@ def request_next(self): if self.request_index == len(self.kills): self.request_index = 0 self.last_request = self.kills[self.request_index] + rospy.logdebug(f"Requesting {self.last_request}") self.request(constants[self.last_request]["REQUEST"]) def wrench_cb(self, msg): @@ -320,6 +344,20 @@ def publish_diagnostics(self, err=None): msg.status.append(status) self.diagnostics_pub.publish(msg) + def publish_thrust(self): + data = String() + data.data = "/wrench/rc" + self.rc_pub.publish(data) + + def convert(int_val): + return ((int_val - 128) / 128) * 500 + + for thruster in self.thrusters: + converted = convert(self.thrusters[thruster]) + msg = Command() + msg.setpoint = converted + self.motor_pubs[thruster].publish(msg) + def publish_joy(self): """ Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node @@ -347,14 +385,21 @@ def publish_joy(self): current_joy.header.stamp = rospy.Time.now() self.joy_pub.publish(current_joy) + def get_hw_kills(self): + return [ + self.board_status[b] + for b in self.board_status + if b != "OVERALL" or b != "COMPUTER" + ] + def update_hw_kill(self): """ Raise/Clear hw-kill ROS Alarm is necessary (any kills on board are engaged) """ killed = self.board_status["OVERALL"] - if (killed and not self._hw_killed) or ( - killed and self.board_status != self._last_hw_kill_paramaters - ): + kills = self.get_hw_kills() + if killed and not self._hw_killed and any(kills): + rospy.logerr("here") self._hw_killed = True self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status) self._last_hw_kill_paramaters = copy.copy(self.board_status) @@ -369,10 +414,10 @@ def request(self, write_str, expected_response=None): Returns True or False depending on the response. With no `recv_str` passed in the raw result will be returned. """ - self.ser.write(write_str.encode()) + self.ser.write(write_str) if expected_response is not None: for byte in expected_response: - self.expected_responses.append(byte.encode()) + self.expected_responses.append(byte) def kill_alarm_cb(self, alarm): """ diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch new file mode 100644 index 000000000..9fde9779f --- /dev/null +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch @@ -0,0 +1,20 @@ + + + + + + + + + battery-voltage: + low: 26 + critical: 20 + + + + kill: ['odom-kill', 'hw-kill'] + + + kill, odom-kill, network-loss, station-hold, battery-voltage, hw-kill, thruster-fault + + From 9cdae721e6a20186216889a666ce3b13165d657c Mon Sep 17 00:00:00 2001 From: Alex Perez Date: Tue, 15 Nov 2022 18:12:52 -0500 Subject: [PATCH 3/9] Fix circular import from types.py --- mil_common/txros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mil_common/txros b/mil_common/txros index bd00d7366..d45fc0405 160000 --- a/mil_common/txros +++ b/mil_common/txros @@ -1 +1 @@ -Subproject commit bd00d7366b4e41e4af6c0f8a36e0433bdd6d1590 +Subproject commit d45fc04051c50b29468c1e087533ff3c76403a2f From 4c1042b642c67cd514d39b24ec12143a17ab5c59 Mon Sep 17 00:00:00 2001 From: andrew-aj Date: Wed, 16 Nov 2022 10:54:45 +1100 Subject: [PATCH 4/9] deleted test --- .../launch/hardware/test_board.launch | 20 ------------------- 1 file changed, 20 deletions(-) delete mode 100644 NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch deleted file mode 100644 index 9fde9779f..000000000 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/test_board.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - battery-voltage: - low: 26 - critical: 20 - - - - kill: ['odom-kill', 'hw-kill'] - - - kill, odom-kill, network-loss, station-hold, battery-voltage, hw-kill, thruster-fault - - From 77f5d1f43a53d0357b7ff2732bb00d6e57b085be Mon Sep 17 00:00:00 2001 From: Alex Perez Date: Wed, 16 Nov 2022 02:23:23 -0500 Subject: [PATCH 5/9] qual changes --- .../config/camera_calibration/dell.yaml | 4 +- .../launch/hardware/cameras/dell_right.launch | 13 +- .../hardware/cameras/front_cameras.launch | 2 +- .../launch/hardware/kill_board.launch | 2 +- .../navigator_launch/launch/master.launch | 2 +- .../launch/perception/classifier.launch | 117 +++++++++--------- 6 files changed, 69 insertions(+), 71 deletions(-) diff --git a/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml b/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml index ca760aabe..3f540b3d9 100644 --- a/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml +++ b/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml @@ -1,6 +1,6 @@ --- -image_width: 1920 -image_height: 1080 +image_width: 640 +image_height: 480 camera_name: head_camera camera_matrix: rows: 3 diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch index c9c48093a..f60658313 100644 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch @@ -1,19 +1,18 @@ - - + + - - - + + + - - + diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch index 9bd832f5c..c4c0de246 100644 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch @@ -1,6 +1,6 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch index 55cc23647..0cd3c3c01 100644 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch @@ -1,6 +1,6 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/master.launch b/NaviGator/mission_control/navigator_launch/launch/master.launch index 80c0cc849..639593a62 100644 --- a/NaviGator/mission_control/navigator_launch/launch/master.launch +++ b/NaviGator/mission_control/navigator_launch/launch/master.launch @@ -10,7 +10,7 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch index f5d795b50..c23cc954b 100644 --- a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch +++ b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch @@ -1,68 +1,67 @@ - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - + + + + + + + From 01bbfbb4c0710dc8376c4443278586395e7fb394 Mon Sep 17 00:00:00 2001 From: Daniel Parra Date: Fri, 6 Sep 2024 14:33:07 -0400 Subject: [PATCH 6/9] increased the confidence threshold of the YOLO model to prevent overlapping labels and prevented navigator from investigating large objects when running in the simulation evenironment --- .../launch/perception/classifier.launch | 2 +- .../navigator_missions/navigation.py | 33 ++++++++++++++++++- 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch index c23cc954b..6f365998e 100644 --- a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch +++ b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch @@ -13,7 +13,7 @@ - + diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py index b40136d8d..b3bf5df22 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py @@ -290,10 +290,26 @@ async def explore_closest_until(self, is_done, filter_and_sort): # and find closest one within 25 meters (max width of a gate). if cone_buoys_investigated > 0 and potential_candidate is None: for i in range(len(objects)): - print(positions[i]) + obj_vol = ( + objects[i].scale.x * objects[i].scale.y * objects[i].scale.z + ) + + # Catch case where we invesitegate the shore + if obj_vol > 3: + print( + f"Object {i} volume: ", + objects[i].scale.x + * objects[i].scale.y + * objects[i].scale.z, + ) + continue + else: + print(positions[i]) + if ( objects[i].id not in investigated and "round" not in objects[i].labeled_classification + and obj_vol < 3 ): distance = np.linalg.norm(positions[i] - self.pose[0]) if distance < shortest_distance and distance <= 25: @@ -308,9 +324,24 @@ async def explore_closest_until(self, is_done, filter_and_sort): # if that doesn't produce any results, literally just go to closest buoy if potential_candidate is None: for i in range(len(objects)): + obj_vol = ( + objects[i].scale.x * objects[i].scale.y * objects[i].scale.z + ) + + # Catch case where we invesitegate the shore + if obj_vol > 3: + print( + f"Object {i} volume: ", + objects[i].scale.x + * objects[i].scale.y + * objects[i].scale.z, + ) + continue + if ( objects[i].id not in investigated and "round" not in objects[i].labeled_classification + and obj_vol < 3 ): distance = np.linalg.norm(positions[i] - self.pose[0]) if distance < shortest_distance: From 61e1f33280a47a0189d70a8a26e447d58fbd31f8 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 10 Sep 2024 12:28:29 -0400 Subject: [PATCH 7/9] Remove print statements --- .../navigator_missions/navigation.py | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py index b3bf5df22..f51865b0b 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py @@ -296,15 +296,7 @@ async def explore_closest_until(self, is_done, filter_and_sort): # Catch case where we invesitegate the shore if obj_vol > 3: - print( - f"Object {i} volume: ", - objects[i].scale.x - * objects[i].scale.y - * objects[i].scale.z, - ) continue - else: - print(positions[i]) if ( objects[i].id not in investigated @@ -330,12 +322,6 @@ async def explore_closest_until(self, is_done, filter_and_sort): # Catch case where we invesitegate the shore if obj_vol > 3: - print( - f"Object {i} volume: ", - objects[i].scale.x - * objects[i].scale.y - * objects[i].scale.z, - ) continue if ( From 5fd75611f68c6675c00b3751f96d9f4e98482e04 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 10 Sep 2024 16:21:07 -0400 Subject: [PATCH 8/9] Remove navigator_kill_board changes which won't be used in 2024 --- .../navigator_kill_board/constants.py | 80 +++++----- .../nodes/kill_board_driver.py | 144 ++++++------------ 2 files changed, 89 insertions(+), 135 deletions(-) diff --git a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py index c1d9e1a49..d5eaef7e4 100644 --- a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py +++ b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py @@ -21,57 +21,57 @@ constants = { "TIMEOUT_SECONDS": 8.0, # How often board must be pinged to not set HEARTBERAT_REMOTE True # Note: not official documented, this is just a guess - "RESPONSE_FALSE": b"\x00", # True status for synchronous requests of individual addresses - "RESPONSE_TRUE": b"\x01", # False status for synchronous requests of individual addresses - "PING": {"REQUEST": b"\x20", "RESPONSE": b"\x30"}, + "RESPONSE_FALSE": "\x00", # True status for synchronous requests of individual addresses + "RESPONSE_TRUE": "\x01", # False status for synchronous requests of individual addresses + "PING": {"REQUEST": "\x20", "RESPONSE": "\x30"}, "KILLS": [ "OVERALL", "BUTTON_FRONT_PORT", "BUTTON_AFT_PORT", "BUTTON_FRONT_STARBOARD", "BUTTON_AFT_STARBOARD", - # "HEARTBEAT_COMPUTER", + "HEARTBEAT_COMPUTER", "BUTTON_REMOTE", - # "HEARTBEAT_REMOTE", + "HEARTBEAT_REMOTE", "COMPUTER", ], "OVERALL": { # Should be True if any of the over are True - "REQUEST": b"\x21", - "TRUE": b"\x10", - "FALSE": b"\x11", + "REQUEST": "\x21", + "TRUE": "\x10", + "FALSE": "\x11", }, - "BUTTON_FRONT_PORT": {"REQUEST": b"\x22", "TRUE": b"\x12", "FALSE": b"\x13"}, - "BUTTON_AFT_PORT": {"REQUEST": b"\x23", "TRUE": b"\x14", "FALSE": b"\x15"}, - "BUTTON_FRONT_STARBOARD": {"REQUEST": b"\x24", "TRUE": b"\x16", "FALSE": b"\x17"}, - "BUTTON_AFT_STARBOARD": {"REQUEST": b"\x25", "TRUE": b"\x18", "FALSE": b"\x19"}, + "BUTTON_FRONT_PORT": {"REQUEST": "\x22", "TRUE": "\x12", "FALSE": "\x13"}, + "BUTTON_AFT_PORT": {"REQUEST": "\x23", "TRUE": "\x14", "FALSE": "\x15"}, + "BUTTON_FRONT_STARBOARD": {"REQUEST": "\x24", "TRUE": "\x16", "FALSE": "\x17"}, + "BUTTON_AFT_STARBOARD": {"REQUEST": "\x25", "TRUE": "\x18", "FALSE": "\x19"}, "HEARTBEAT_COMPUTER": { # Will return True if board is not pinged by mobo often enough - "REQUEST": b"\x26", - "TRUE": b"\x1A", - "FALSE": b"\x1B", + "REQUEST": "\x26", + "TRUE": "\x1A", + "FALSE": "\x1B", }, - "BUTTON_REMOTE": {"REQUEST": b"\x28", "TRUE": b"\x3A", "FALSE": b"\x3B"}, + "BUTTON_REMOTE": {"REQUEST": "\x28", "TRUE": "\x3A", "FALSE": "\x3B"}, "HEARTBEAT_REMOTE": { # Will return True if board is not pinged by controller often enough - "REQUEST": b"\x29", - "TRUE": b"\x3C", - "FALSE": b"\x3D", + "REQUEST": "\x29", + "TRUE": "\x3C", + "FALSE": "\x3D", }, "COMPUTER": { # Allows board to be killed over serial (like through ROS) - "KILL": {"REQUEST": b"\x45", "RESPONSE": b"\x55"}, - "CLEAR": {"REQUEST": b"\x46", "RESPONSE": b"\x56"}, - "REQUEST": b"\x27", - "TRUE": b"\x1C", - "FALSE": b"\x1D", + "KILL": {"REQUEST": "\x45", "RESPONSE": "\x55"}, + "CLEAR": {"REQUEST": "\x46", "RESPONSE": "\x56"}, + "REQUEST": "\x27", + "TRUE": "\x1C", + "FALSE": "\x1D", }, - "CONNECTED": {"TRUE": b"\x1E", "FALSE": b"\x1F"}, + "CONNECTED": {"TRUE": "\x1E", "FALSE": "\x1F"}, "LIGHTS": { # Note: YELLOW turns off GREEN and visa versa - "OFF_REQUEST": b"\x40", - "OFF_RESPONSE": b"\x50", - "YELLOW_REQUEST": b"\x41", - "YELLOW_RESPONSE": b"\x51", - "GREEN_REQUEST": b"\x42", - "GREEN_RESPONSE": b"\x52", + "OFF_REQUEST": "\x40", + "OFF_RESPONSE": "\x50", + "YELLOW_REQUEST": "\x41", + "YELLOW_RESPONSE": "\x51", + "GREEN_REQUEST": "\x42", + "GREEN_RESPONSE": "\x52", }, - "CONTROLLER": b"\xA0", # Signifies the start of a controller message (joysticks & buttons) + "CONTROLLER": "\xA0", # Signifies the start of a controller message (joysticks & buttons) # Immediately followed by 8 bytes: 6 joystick bytes, 2 button bytes # Joystick message is 3 signed ints from -2048 to 2047 # Button message is 16 bits signifying up to 16 buttons on/off @@ -87,13 +87,13 @@ "EMERGENCY_CONTROL", ], "CTRL_BUTTONS_VALUES": { # Amount of buttons and labels will be changed in the future - "STATION_HOLD": b"\x00\x01", # Button 0 - "RAISE_KILL": b"\x00\x02", # Button 1 - "CLEAR_KILL": b"\x00\x04", # Button 2 - "THRUSTER_RETRACT": b"\x00\x10", # Button 4 - "THRUSTER_DEPLOY": b"\x00\x20", # Button 5 - "GO_INACTIVE": b"\x00\x40", # Button 6 - "START": b"\x00\x80", # Button 7 - "EMERGENCY_CONTROL": b"\x20\x00", # Button 13 + "STATION_HOLD": "\x00\x01", # Button 0 + "RAISE_KILL": "\x00\x02", # Button 1 + "CLEAR_KILL": "\x00\x04", # Button 2 + "THRUSTER_RETRACT": "\x00\x10", # Button 4 + "THRUSTER_DEPLOY": "\x00\x20", # Button 5 + "GO_INACTIVE": "\x00\x40", # Button 6 + "START": "\x00\x80", # Button 7 + "EMERGENCY_CONTROL": "\x20\x00", # Button 13 }, } diff --git a/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py b/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py index fd049faec..fa1e029fe 100755 --- a/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py +++ b/NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py @@ -11,7 +11,6 @@ from mil_tools import thread_lock from navigator_alarm_handlers import NetworkLoss from navigator_kill_board import constants -from roboteq_msgs.msg import Command from ros_alarms import AlarmBroadcaster, AlarmListener from sensor_msgs.msg import Joy from std_msgs.msg import String @@ -86,17 +85,6 @@ def __init__(self): "CTRL_STICKS" ]: # These are 3 signed 16-bit values for stick positions self.sticks[stick] = 0x0000 - self.thrusters = {} - for thruster in ["BL", "BR", "FL", "FR"]: - self.thrusters[thruster] = 0x00 - self.rc_pub = rospy.Publisher("/wrench/selected", String) - self.motor_pubs = {} - for thruster in self.thrusters: - self.motor_pubs = rospy.Publisher( - f"/{thruster}_motor/cmd", - Command, - queue_size=1, - ) self.sticks_temp = 0x0000 self.buttons = {} for button in constants[ @@ -148,8 +136,7 @@ def update_ros(self): self.update_hw_kill() self.publish_diagnostics() if self.ctrl_msg_received is True: - # self.publish_joy() - self.publish_thrust() + self.publish_joy() self.ctrl_msg_received = False def handle_byte(self, msg): @@ -158,14 +145,12 @@ def handle_byte(self, msg): a known response to a recent request """ # If the controller message start byte is received, next 8 bytes are the controller data - # print("here") - # print(msg) if msg == constants["CONTROLLER"]: - self.ctrl_msg_count = 4 + self.ctrl_msg_count = 8 self.ctrl_msg_timeout = rospy.Time.now() return # If receiving the controller message, record the byte as stick/button data - if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 4): + if (self.ctrl_msg_count > 0) and (self.ctrl_msg_count <= 8): # If 1 second has passed since the message began, timeout and report warning if (rospy.Time.now() - self.ctrl_msg_timeout) >= rospy.Duration(1): self.ctrl_msg_received = False @@ -173,65 +158,56 @@ def handle_byte(self, msg): rospy.logwarn( "Timeout receiving controller message. Please disconnect controller.", ) - # [HEADER] [BL] [BR] [FL] [FR] - if self.ctrl_msg_count == 4: - self.thrusters["BL"] = int(msg.encode("hex", 16)) - elif self.ctrl_msg_count == 3: - self.thrusters["BR"] = int(msg.encode("hex", 16)) - elif self.ctrl_msg_count == 2: - self.thrusters["FL"] = int(msg.encode("hex", 16)) - elif self.ctrl_msg_count == 1: - self.thrusters["FR"] = int(msg.encode("hex", 16)) - self.ctrl_msg_received = True - # if ( - # self.ctrl_msg_count > 2 - # ): # The first 6 bytes in the message are stick data bytes - # if ( - # self.ctrl_msg_count % 2 - # ) == 0: # Even number byte: first byte in data word - # self.sticks_temp = int(msg.encode("hex"), 16) << 8 - # else: # Odd number byte: combine two bytes into a stick's data word - # self.sticks_temp += int(msg.encode("hex"), 16) - # if self.ctrl_msg_count > 6: - # self.sticks["UD"] = self.sticks_temp - # elif self.ctrl_msg_count > 4: - # self.sticks["LR"] = self.sticks_temp - # else: - # self.sticks["TQ"] = self.sticks_temp - # self.sticks_temp = 0x0000 - # else: # The last 2 bytes are button data bytes - # if (self.ctrl_msg_count % 2) == 0: - # self.buttons_temp = int(msg.encode("hex"), 16) << 8 - # else: # Combine two bytes into the button data word - # self.buttons_temp += int(msg.encode("hex"), 16) - # for ( - # button - # ) in ( - # self.buttons - # ): # Each of the 16 bits represents a button on/off state - # button_check = int( - # constants["CTRL_BUTTONS_VALUES"][button].encode("hex"), 16 - # ) - # self.buttons[button] = ( - # self.buttons_temp & button_check - # ) == button_check - # self.buttons_temp = 0x0000 - # self.ctrl_msg_received = ( - # True # After receiving last byte, trigger joy update - # ) + if ( + self.ctrl_msg_count > 2 + ): # The first 6 bytes in the message are stick data bytes + if ( + self.ctrl_msg_count % 2 + ) == 0: # Even number byte: first byte in data word + self.sticks_temp = int(msg.encode("hex"), 16) << 8 + else: # Odd number byte: combine two bytes into a stick's data word + self.sticks_temp += int(msg.encode("hex"), 16) + if self.ctrl_msg_count > 6: + self.sticks["UD"] = self.sticks_temp + elif self.ctrl_msg_count > 4: + self.sticks["LR"] = self.sticks_temp + else: + self.sticks["TQ"] = self.sticks_temp + self.sticks_temp = 0x0000 + else: # The last 2 bytes are button data bytes + if (self.ctrl_msg_count % 2) == 0: + self.buttons_temp = int(msg.encode("hex"), 16) << 8 + else: # Combine two bytes into the button data word + self.buttons_temp += int(msg.encode("hex"), 16) + for ( + button + ) in ( + self.buttons + ): # Each of the 16 bits represents a button on/off state + button_check = int( + constants["CTRL_BUTTONS_VALUES"][button].encode("hex"), + 16, + ) + self.buttons[button] = ( + self.buttons_temp & button_check + ) == button_check + self.buttons_temp = 0x0000 + self.ctrl_msg_received = ( + True # After receiving last byte, trigger joy update + ) self.ctrl_msg_count -= 1 return # If a response has been received to a requested status (button, remove, etc), update internal state if self.last_request is not None: if msg == constants["RESPONSE_FALSE"]: if self.board_status[self.last_request] is True: - rospy.logerr(f"SYNC FALSE for {self.last_request}") + rospy.logdebug(f"SYNC FALSE for {self.last_request}") self.board_status[self.last_request] = False self.last_request = None return if msg == constants["RESPONSE_TRUE"]: if self.board_status[self.last_request] is False: - rospy.logerr(f"SYNC TRUE for {self.last_request}") + rospy.logdebug(f"SYNC TRUE for {self.last_request}") self.board_status[self.last_request] = True self.last_request = None return @@ -239,12 +215,12 @@ def handle_byte(self, msg): for kill in self.board_status: if msg == constants[kill]["FALSE"]: if self.board_status[kill] is True: - rospy.logerr(f"ASYNC FALSE for {kill}") + rospy.logdebug(f"ASYNC FALSE for {self.last_request}") self.board_status[kill] = False return if msg == constants[kill]["TRUE"]: if self.board_status[kill] is False: - rospy.logerr(f"ASYNC TRUE FOR {kill}") + rospy.logdebug(f"ASYNC TRUE FOR {kill}") self.board_status[kill] = True return # If a response to another request, like ping or computer kill/clear is received @@ -279,7 +255,6 @@ def request_next(self): if self.request_index == len(self.kills): self.request_index = 0 self.last_request = self.kills[self.request_index] - rospy.logdebug(f"Requesting {self.last_request}") self.request(constants[self.last_request]["REQUEST"]) def wrench_cb(self, msg): @@ -351,20 +326,6 @@ def publish_diagnostics(self, err=None): msg.status.append(status) self.diagnostics_pub.publish(msg) - def publish_thrust(self): - data = String() - data.data = "/wrench/rc" - self.rc_pub.publish(data) - - def convert(int_val): - return ((int_val - 128) / 128) * 500 - - for thruster in self.thrusters: - converted = convert(self.thrusters[thruster]) - msg = Command() - msg.setpoint = converted - self.motor_pubs[thruster].publish(msg) - def publish_joy(self): """ Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node @@ -392,21 +353,14 @@ def publish_joy(self): current_joy.header.stamp = rospy.Time.now() self.joy_pub.publish(current_joy) - def get_hw_kills(self): - return [ - self.board_status[b] - for b in self.board_status - if b != "OVERALL" or b != "COMPUTER" - ] - def update_hw_kill(self): """ Raise/Clear hw-kill ROS Alarm is necessary (any kills on board are engaged) """ killed = self.board_status["OVERALL"] - kills = self.get_hw_kills() - if killed and not self._hw_killed and any(kills): - rospy.logerr("here") + if (killed and not self._hw_killed) or ( + killed and self.board_status != self._last_hw_kill_paramaters + ): self._hw_killed = True self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status) self._last_hw_kill_paramaters = copy.copy(self.board_status) @@ -421,10 +375,10 @@ def request(self, write_str, expected_response=None): Returns True or False depending on the response. With no `recv_str` passed in the raw result will be returned. """ - self.ser.write(write_str) + self.ser.write(write_str.encode()) if expected_response is not None: for byte in expected_response: - self.expected_responses.append(byte) + self.expected_responses.append(byte.encode()) def kill_alarm_cb(self, alarm): """ From a51522f0fac4b0e2c81650997b4cba1f9e81f1f3 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Fri, 13 Sep 2024 15:26:27 -0400 Subject: [PATCH 9/9] Remove breaking temporary change in navigator_robotx_comms --- .../navigator_robotx_comms/navigator_robotx_comms.py | 8 ++------ .../nodes/robotx_comms_client.py | 10 ---------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py index f2e8f1508..6a20ad08a 100644 --- a/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py +++ b/NaviGator/utils/navigator_robotx_comms/navigator_robotx_comms/navigator_robotx_comms.py @@ -107,12 +107,8 @@ def to_string( latitude = gps_array.point.x longitude = gps_array.point.y else: - # latitude = "" - # longitude = "" - - # use only for qualifying, delete later and uncomment above!! - latitude = odom.pose.pose.position.x - longitude = odom.pose.pose.position.y + latitude = "" + longitude = "" if odom is not None: quaternion = odom.pose.pose.orientation diff --git a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py index 194ea6426..59e6139e3 100755 --- a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py +++ b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py @@ -66,16 +66,6 @@ class SystemModes(IntEnum): REMOTE_CONTROLLED = 1 -class UAVModes(IntEnum): - """ - Enumerates constants of friendly uav mode names to ints - """ - - STOWED = 1 - DEPLOYED = 2 - FAULTED = 3 - - class RobotXStartServices: """ Initializes services and subscribes to necessary publishers in order to facilitate