Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Robotx qual task #1272

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
Open

Robotx qual task #1272

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
440d35c
Added robotx comms support for simulation
alexperez33 Nov 12, 2022
b057e75
killboard updates
andrew-aj Nov 13, 2022
9cdae72
Fix circular import from types.py
alexperez33 Nov 15, 2022
7351bd7
Fix circular import from types.py
alexperez33 Nov 15, 2022
4c1042b
deleted test
andrew-aj Nov 15, 2022
353dd82
Merge branch 'robotx_merge_branch' of github.com:uf-mil/mil into robo…
alexperez33 Nov 15, 2022
77f5d1f
qual changes
alexperez33 Nov 16, 2022
20c1958
merged master into robotx_qual_task
DaniParr Sep 6, 2024
01bbfbb
increased the confidence threshold of the YOLO model to prevent overl…
DaniParr Sep 6, 2024
61e1f33
Remove print statements
cbrxyz Sep 10, 2024
5fd7561
Remove navigator_kill_board changes which won't be used in 2024
cbrxyz Sep 10, 2024
a51522f
Remove breaking temporary change in navigator_robotx_comms
cbrxyz Sep 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 40 additions & 40 deletions NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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": "\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
Expand All @@ -87,13 +87,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
},
}
144 changes: 95 additions & 49 deletions NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -85,6 +86,17 @@ 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[
Expand Down Expand Up @@ -136,7 +148,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):
Expand All @@ -145,82 +158,93 @@ 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
self.ctrl_msg_count = 0
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
# If an async update was received, update internal state
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
Expand Down Expand Up @@ -255,6 +279,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):
Expand Down Expand Up @@ -326,6 +351,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
Expand Down Expand Up @@ -353,14 +392,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)
Expand All @@ -375,10 +421,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):
"""
Expand Down
4 changes: 2 additions & 2 deletions NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
---
image_width: 1920
image_height: 1080
image_width: 640
image_height: 480
camera_name: head_camera
camera_matrix:
rows: 3
Expand Down
13 changes: 6 additions & 7 deletions NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,18 @@
<?xml version="1.0"?>
<launch>
<arg name="simulation" default="False" />
<group ns="camera/front/right" >
<node pkg="usb_cam" type="usb_cam_node" name="dell_driver" unless="$(arg simulation)" >
<group ns="camera/front/right">
<node pkg="usb_cam" type="usb_cam_node" name="dell_driver" unless="$(arg simulation)">
<param name="video_device" value="/dev/v4l/by-id/usb-Alpha_Imaging_Tech._Corp._Dell_Webcam_WB7022_A7D19B26B27A-video-index0" />
<param name="camera_frame_id" value="wamv/front_right_cam_link_optical" />
<param name="camera_info_url" value="file://$(find navigator_launch)/config/camera_calibration/dell.yaml" />
<param name="pixel_format" value="yuyv" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="framerate" value="7" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="framerate" value="30" />
<remap from="dell_driver/image_raw" to="image_raw" />
<remap from="dell_driver/camera_info" to="camera_info" />
</node>
<node pkg="image_proc" type="image_proc" name="dell_right_image_proc">
</node>
<node pkg="image_proc" type="image_proc" name="dell_right_image_proc"></node>
</group>
</launch>
2 changes: 1 addition & 1 deletion NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
<launch>
<arg name="front_left" default="True" />
<arg name="front_left" default="False" />
<arg name="front_right" default="True" />
<arg name="simulation" default="False" />
<include if="$(arg front_left)" file="$(find navigator_launch)/launch/hardware/cameras/dell_left.launch">
Expand Down
Loading
Loading