class ThrusterMapperNode(object):
'''
Node to publish individual thruster commands from the body frame wrench.
See thruster_map.py for more details on this process as this is simply the ROS wrapper.
'''
def __init__(self):
# Used for mapping wrench to individual thrusts
self.thruster_map = ThrusterMap.from_ros_params()
# To track kill state so no thrust is sent when killed (kill board hardware also ensures this)
self.kill = False
self.kill_listener = AlarmListener('kill', self.kill_cb)
self.kill_listener.wait_for_server()
# Start off with no wrench
self.wrench = np.zeros(3)
# ROS setup
self.BL_pub = rospy.Publisher("/BL_motor/cmd", Command, queue_size=1)
self.BR_pub = rospy.Publisher("/BR_motor/cmd", Command, queue_size=1)
self.FL_pub = rospy.Publisher("/FL_motor/cmd", Command, queue_size=1)
self.FR_pub = rospy.Publisher("/FR_motor/cmd", Command, queue_size=1)
rospy.Subscriber("/wrench/cmd", WrenchStamped, self.wrench_cb)
def kill_cb(self, alarm):
self.kill = alarm.raised
def wrench_cb(self, msg):
'''
Store last commanded wrench for later mapping to thrusts
'''
force = msg.wrench.force
torque = msg.wrench.torque
self.wrench = np.array((force.x, force.y, torque.z))
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
BL, BR, FL, FR = Command(), Command(), Command(), Command()
if not self.kill: # Only get thrust of not killed, otherwise publish default 0 thrust
BL.setpoint, BR.setpoint, FL.setpoint, FR.setpoint = self.thruster_map.wrench_to_thrusts(
self.wrench)
self.BL_pub.publish(BL)
self.BR_pub.publish(BR)
self.FL_pub.publish(FL)
self.FR_pub.publish(FR)
class KillInterface(object):
"""
Driver to interface with NaviGator's kill handeling board, which disconnects power to actuators
if any of 4 emergency buttons is pressed, a software kill command is sent, or the network hearbeat
stops. This driver enables the software kill option via ros_alarms and outputs diagnostics
data about the board to ROS. The driver can handle the board's asyncronous updates of kill statuses
and will also periodicly request updates in case the async is not working (it often doesn't).
"""
ALARM = 'hw-kill' # Alarm to raise when hardware kill is detected
YELLOW_WRENCHES = ['rc', '/wrench/rc', 'keyboard', '/wrench/keyboard'
] # Wrenches which activate YELLOW LED
GREEN_WRENCHES = ['autonomous', '/wrench/autonomous'
] # Wrenches which activate GREEN LED
def __init__(self):
self.port = rospy.get_param(
'~port',
"/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A104OWRY-if00-port0")
self.connected = False
self.diagnostics_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=3)
while not self.connected and not rospy.is_shutdown():
try:
self.connect()
self.connected = True
except serial.SerialException as e:
rospy.logerr('Cannot connect to kill board. {}'.format(e))
self.publish_diagnostics(e)
rospy.sleep(1)
rospy.loginfo('Board connected!')
self.board_status = {}
for kill in constants['KILLS']:
self.board_status[kill] = False
self.kills = self.board_status.keys()
self.expected_responses = []
self.network_msg = None
self.wrench = ''
self._hw_killed = False
self._last_hw_kill_paramaters = self.board_status
self.last_request = None
self.request_index = -1
self.hw_kill_broadcaster = AlarmBroadcaster('hw-kill')
self.hw_kill_broadcaster.wait_for_server()
self.joy_pub = rospy.Publisher("/joy_emergency", Joy, queue_size=1)
self.ctrl_msg_received = False
self.ctrl_msg_count = 0
self.ctrl_msg_timeout = 0
self.sticks = {}
for stick in constants[
'CTRL_STICKS']: # These are 3 signed 16-bit values for stick positions
self.sticks[stick] = 0x0000
self.sticks_temp = 0x0000
self.buttons = {}
for button in constants[
'CTRL_BUTTONS']: # These are the button on/off states (16 possible inputs)
self.buttons[button] = False
self.buttons_temp = 0x0000
self._hw_kill_listener = AlarmListener('hw-kill',
self.hw_kill_alarm_cb)
self._kill_listener = AlarmListener('kill', self.kill_alarm_cb)
self._hw_kill_listener.wait_for_server()
self._kill_listener.wait_for_server()
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber(
"/network", Header,
self.network_cb) # Passes along network hearbeat to kill board
def connect(self):
if rospy.get_param(
'/is_simulation', False
): # If in Gazebo, run fake serial class following board's protocol
from navigator_kill_board import SimulatedKillBoard
self.ser = SimulatedKillBoard()
else:
baud = rospy.get_param('~baud', 9600)
self.ser = serial.Serial(port=self.port, baudrate=baud)
def run(self):
'''
Main loop for driver, at a fixed rate updates alarms and diagnostics output with new
kill board output.
'''
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.last_request is None:
self.request_next()
self.receive()
self.update_ros()
rate.sleep()
def update_ros(self):
self.update_hw_kill()
self.publish_diagnostics()
if self.ctrl_msg_received is True:
self.publish_joy()
self.ctrl_msg_received = False
def handle_byte(self, msg):
'''
React to a byte recieved from the board. This could by an async update of a kill status or
a known response to a recent request
'''
# If the controller message start byte is received, next 8 bytes are the controller data
if msg == constants['CONTROLLER']:
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 <= 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
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
self.ctrl_msg_count -= 1
return
# If a response has been recieved 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('SYNC FALSE for {}'.format(
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('SYNC TRUE for {}'.format(
self.last_request))
self.board_status[self.last_request] = True
self.last_request = None
return
# If an async update was recieved, update internal state
for kill in self.board_status:
if msg == constants[kill]['FALSE']:
if self.board_status[kill] is True:
rospy.logdebug('ASYNC FALSE for {}'.format(
self.last_request))
self.board_status[kill] = False
return
if msg == constants[kill]['TRUE']:
if self.board_status[kill] is False:
rospy.logdebug('ASYNC TRUE FOR {}'.format(kill))
self.board_status[kill] = True
return
# If a response to another request, like ping or computer kill/clear is recieved
for index, byte in enumerate(self.expected_responses):
if msg == byte:
del self.expected_responses[index]
return
# Log a warning if an unexpected byte was recieved
rospy.logwarn(
'Recieved an unexpected byte {}, remaining expected_responses={}'.
format(hex(ord(msg)), len(self.expected_responses)))
@thread_lock(lock)
def receive(self):
'''
Recieve update bytes sent from the board without requests being sent, updating internal
state, raising alarms, etc in response to board updates. Clears the in line buffer.
'''
while self.ser.in_waiting > 0 and not rospy.is_shutdown():
msg = self.ser.read(1)
self.handle_byte(msg)
def request_next(self):
'''
Manually request status of the next kill, looping back to the first
once all have been responsded to.
'''
self.request_index += 1
if self.request_index == len(self.kills):
self.request_index = 0
self.last_request = self.kills[self.request_index]
self.request(constants[self.last_request]['REQUEST'])
def wrench_cb(self, msg):
'''
Updates wrench (autnomous vs teleop) diagnostic light if nessesary
on wrench changes
'''
wrench = msg.data
if wrench != self.wrench:
if wrench in self.YELLOW_WRENCHES:
self.request(constants['LIGHTS']['YELLOW_REQUEST'],
constants['LIGHTS']['YELLOW_RESPONSE'])
elif wrench in self.GREEN_WRENCHES:
self.request(constants['LIGHTS']['GREEN_REQUEST'],
constants['LIGHTS']['GREEN_RESPONSE'])
else:
self.request(constants['LIGHTS']['OFF_REQUEST'],
constants['LIGHTS']['OFF_RESPONSE'])
self.wrench = wrench
def network_cb(self, msg):
'''
Pings kill board on every network hearbeat message. Pretends to be the rf-based hearbeat because
real one does not work :(
'''
self.request(constants['PING']['REQUEST'],
constants['PING']['RESPONSE'])
def publish_diagnostics(self, err=None):
'''
Publishes current kill board state in ROS diagnostics package, making it easy to use in GUIs and other ROS tools
'''
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = 'kill_board'
status.hardware_id = self.port
if not self.connected:
status.level = DiagnosticStatus.ERROR
status.message = 'Cannot connect to kill board. Retrying every second.'
status.values.append(KeyValue("Exception", str(err)))
else:
status.level = DiagnosticStatus.OK
for key, value in self.board_status.items():
status.values.append(KeyValue(key, str(value)))
msg.status.append(status)
self.diagnostics_pub.publish(msg)
def publish_joy(self):
'''
Publishes current stick/button state as a Joy object, to be handled by navigator_emergency.py node
'''
current_joy = Joy()
current_joy.axes.extend([0] * 4)
current_joy.buttons.extend([0] * 16)
for stick in self.sticks:
if self.sticks[
stick] >= 0x8000: # Convert 2's complement hex to signed decimal if negative
self.sticks[stick] -= 0x10000
current_joy.axes[0] = np.float32(self.sticks['LR']) / 2048
current_joy.axes[1] = np.float32(self.sticks['UD']) / 2048
current_joy.axes[3] = np.float32(self.sticks['TQ']) / 2048
current_joy.buttons[0] = np.int32(self.buttons['STATION_HOLD'])
current_joy.buttons[1] = np.int32(self.buttons['RAISE_KILL'])
current_joy.buttons[2] = np.int32(self.buttons['CLEAR_KILL'])
current_joy.buttons[4] = np.int32(self.buttons['THRUSTER_RETRACT'])
current_joy.buttons[5] = np.int32(self.buttons['THRUSTER_DEPLOY'])
current_joy.buttons[6] = np.int32(self.buttons['GO_INACTIVE'])
current_joy.buttons[7] = np.int32(self.buttons['START'])
current_joy.buttons[13] = np.int32(self.buttons['EMERGENCY_CONTROL'])
current_joy.header.frame_id = "/base_link"
current_joy.header.stamp = rospy.Time.now()
self.joy_pub.publish(current_joy)
def update_hw_kill(self):
'''
Raise/Clear hw-kill ROS Alarm is nessesary (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):
self._hw_killed = True
self.hw_kill_broadcaster.raise_alarm(parameters=self.board_status)
self._last_hw_kill_paramaters = copy.copy(self.board_status)
if not killed and self._hw_killed:
self._hw_killed = False
self.hw_kill_broadcaster.clear_alarm()
@thread_lock(lock)
def request(self, write_str, expected_response=None):
"""
Deals with requesting data and checking if the response matches some `recv_str`.
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)
if expected_response is not None:
for byte in expected_response:
self.expected_responses.append(byte)
def kill_alarm_cb(self, alarm):
'''
Informs kill board about software kills through ROS Alarms
'''
if alarm.raised:
self.request(constants['COMPUTER']['KILL']['REQUEST'],
constants['COMPUTER']['KILL']['RESPONSE'])
else:
self.request(constants['COMPUTER']['CLEAR']['REQUEST'],
constants['COMPUTER']['CLEAR']['RESPONSE'])
def hw_kill_alarm_cb(self, alarm):
self._hw_killed = alarm.raised
class RobotXStartServices:
"""
Initializes services and subscribes to necessary publishers
"""
def __init__(self):
# define all variables for subscribers
self.gps_array = None
self.odom = None
self.auv_status = 1 # we don't have an AUV, so this will always be 1
self.system_mode = None
self.wrench = None
self.kill = False
# define delimiter for messages
self.delim = ','
# define parameters
self.team_id = rospy.get_param("~team_id")
self.td_ip = rospy.get_param("~td_ip")
self.td_port = rospy.get_param("~td_port")
self.use_test_data = rospy.get_param("~use_test_data")
# time last called
self.time_last_entrance_exit = None
self.time_last_scan_code = None
self.time_last_identify_symbols = None
self.time_last_detect_deliver = None
# initialize connection to server
self.robotx_client = RobotXClient(self.td_ip, self.td_port)
self.robotx_client.connect()
# setup all message types
self.robotx_heartbeat_message = RobotXHeartbeatMessage()
self.robotx_entrance_exit_gate_message = RobotXEntranceExitGateMessage(
)
self.robotx_scan_code_message = RobotXScanCodeMessage()
self.robotx_identify_symbols_dock_message = RobotXIdentifySymbolsDockMessage(
)
self.robotx_detect_deliver_message = RobotXDetectDeliverMessage()
# setup all subscribers
rospy.Subscriber("lla", PointStamped, self.gps_coord_callback)
rospy.Subscriber("odom", Odometry, self.gps_odom_callback)
rospy.Subscriber("/wrench/selected", String, self.wrench_callback)
rospy.Subscriber("/scan_the_code", ScanTheCode,
self.scan_the_code_callback)
# track kill state for inferring system mode
self.kill_listener = AlarmListener('kill', self.kill_callback)
self.kill_listener.wait_for_server()
# setup all services
self.service_entrance_exit_gate_message = rospy.Service(
"entrance_exit_gate_message", MessageExtranceExitGate,
self.handle_entrance_exit_gate_message)
self.service_identify_symbols_dock_message = rospy.Service(
"identify_symbols_dock_message", MessageIdentifySymbolsDock,
self.handle_identify_symbols_dock_message)
self.service_detect_deliver_message = rospy.Service(
"detect_deliver_message", MessageDetectDeliver,
self.handle_detect_deliver_message)
# start sending heartbeat every second
rospy.Timer(rospy.Duration(1), self.handle_heartbeat_message)
def update_system_mode(self):
if self.kill is True:
self.system_mode = 3
elif self.wrench == "autonomous" or self.wrench == "/wrench/autonomous":
self.system_mode = 2
else:
self.system_mode = 1
def wrench_callback(self, wrench):
self.wrench = wrench.data
def kill_callback(self, alarm):
self.kill = alarm.raised
def gps_coord_callback(self, lla):
self.gps_array = lla
def gps_odom_callback(self, odom):
self.odom = odom
def auv_status_callback(self, auv_status):
self.auv_status = auv_status
def system_mode_callback(self, system_mode):
self.system_mode = system_mode
def scan_the_code_callback(self, scan_the_code):
self.handle_scan_code_message(scan_the_code.color_pattern)
def handle_heartbeat_message(self, data):
self.update_system_mode()
hst_date_time = self.get_hst_date_time()
message = self.robotx_heartbeat_message.to_string(
self.delim, self.team_id, hst_date_time, self.gps_array, self.odom,
self.auv_status, self.system_mode, self.use_test_data)
self.robotx_client.send_message(message)
def handle_entrance_exit_gate_message(self, data):
if self.time_last_entrance_exit is not None:
seconds_elapsed = rospy.get_time() - self.time_last_entrance_exit
if seconds_elapsed < 1:
rospy.sleep(1 - seconds_elapsed)
self.time_last_entrance_exit = rospy.get_time()
hst_date_time = self.get_hst_date_time()
message = self.robotx_entrance_exit_gate_message.to_string(
self.delim, self.team_id, hst_date_time, data, self.use_test_data)
self.robotx_client.send_message(message.message)
return message
def handle_scan_code_message(self, color_pattern):
if self.time_last_scan_code is not None:
seconds_elapsed = rospy.get_time() - self.time_last_scan_code
if seconds_elapsed < 1:
rospy.sleep(1 - seconds_elapsed)
self.time_last_scan_code = rospy.get_time()
hst_date_time = self.get_hst_date_time()
message = self.robotx_scan_code_message.to_string(
self.delim, self.team_id, hst_date_time, color_pattern,
self.use_test_data)
self.robotx_client.send_message(message)
def handle_identify_symbols_dock_message(self, data):
if self.time_last_identify_symbols is not None:
seconds_elapsed = rospy.get_time(
) - self.time_last_identify_symbols
if seconds_elapsed < 1:
rospy.sleep(1 - seconds_elapsed)
self.time_last_identify_symbols = rospy.get_time()
hst_date_time = self.get_hst_date_time()
message = self.robotx_identify_symbols_dock_message.to_string(
self.delim, self.team_id, hst_date_time, data, self.use_test_data)
self.robotx_client.send_message(message.message)
return message
def handle_detect_deliver_message(self, data):
if self.time_last_detect_deliver is not None:
seconds_elapsed = rospy.get_time() - self.time_last_detect_deliver
if seconds_elapsed < 1:
rospy.sleep(1 - seconds_elapsed)
self.time_last_detect_deliver = rospy.get_time()
hst_date_time = self.get_hst_date_time()
message = self.robotx_detect_deliver_message.to_string(
self.delim, self.team_id, hst_date_time, data, self.use_test_data)
self.robotx_client.send_message(message.message)
return message
def get_hst_date_time(self):
# HST is 10 hours behind UTC
hst_time = datetime.datetime.utcnow() - datetime.timedelta(hours=10,
minutes=0)
date_string = hst_time.strftime("%d%m%y")
time_string = hst_time.strftime("%H%M%S")
return date_string + self.delim + time_string
class ThrusterMapperNode(object):
'''
Node to publish individual thruster commands from the body frame wrench.
See thruster_map.py for more details on this process as this is simply the ROS wrapper.
'''
def __init__(self):
# Used for mapping wrench to individual thrusts
urdf = rospy.get_param('robot_description', default=None)
if urdf is None or len(urdf) == 0:
raise Exception('robot description not set or empty')
self.thruster_map = ThrusterMap.from_urdf(urdf)
# To track kill state so no thrust is sent when killed (kill board hardware also ensures this)
self.kill = False
self.kill_listener = AlarmListener('kill', self.kill_cb)
self.kill_listener.wait_for_server()
# Start off with no wrench
self.wrench = np.zeros(3)
# Publisher for each thruster
self.publishers = [
rospy.Publisher("/{}_motor/cmd".format(name),
Command,
queue_size=1) for name in self.thruster_map.names
]
# Joint state publisher
# TODO(ironmig):
self.joint_state_pub = rospy.Publisher('/thruster_states',
JointState,
queue_size=1)
self.joint_state_msg = JointState()
for name in self.thruster_map.joints:
self.joint_state_msg.name.append(name)
self.joint_state_msg.position.append(0)
self.joint_state_msg.effort.append(0)
rospy.Subscriber("/wrench/cmd", WrenchStamped, self.wrench_cb)
def kill_cb(self, alarm):
self.kill = alarm.raised
def wrench_cb(self, msg):
'''
Store last commanded wrench for later mapping to thrusts
'''
force = msg.wrench.force
torque = msg.wrench.torque
self.wrench = np.array((force.x, force.y, torque.z))
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
commands = [Command() for i in range(len(self.publishers))]
if not self.kill:
thrusts = self.thruster_map.wrench_to_thrusts(self.wrench)
if len(thrusts) != len(self.publishers):
rospy.logfatal(
"Number of thrusts does not equal number of publishers")
return
for i in range(len(self.publishers)):
commands[i].setpoint = thrusts[i]
for i in range(len(self.publishers)):
self.joint_state_msg.effort[i] = commands[i].setpoint
self.publishers[i].publish(commands[i])
self.joint_state_pub.publish(self.joint_state_msg)
class BatteryMonitor():
'''
Monitors the battery voltage measured by the 4 motors on Navigator,
publishing a moving average of these measurements to /battery_monitor
at a regular interval
'''
def __init__(self):
# Initialize the average voltage to none for the case of no feedback
self.voltage = None
# Initialize a list to hold the 1000 most recent supply voltage readings
# Holding 1000 values ensures that changes in the average are gradual rather than sharp
self.supply_voltages = []
self.hw_kill_raised = None
self._hw_kill_listener = AlarmListener('hw-kill', self.hw_kill_cb)
self._hw_kill_listener.wait_for_server()
# The publisher for the averaged voltage
self.pub_voltage = rospy.Publisher(
"/battery_monitor", Float32, queue_size=1)
# Subscribes to the feedback from each of the four thrusters
motor_topics = ['/FL_motor', '/FR_motor', '/BL_motor', '/BR_motor']
feedback_sub = [message_filters.Subscriber(
topic + '/feedback', Feedback) for topic in motor_topics]
status_sub = [message_filters.Subscriber(
topic + '/status', Status) for topic in motor_topics]
[message_filters.ApproximateTimeSynchronizer([feedback, status], 1, 10).registerCallback(
self.add_voltage) for feedback, status in zip(feedback_sub, status_sub)]
def hw_kill_cb(self, msg):
self.hw_kill_raised = msg.raised
def add_voltage(self, feedback, status):
'''
This is the callback function for feedback from all four motors.
It appends the new readings to the end of the list and
ensures that the list stays under 1000 entries.
'''
# Check if 3rd bit is raised
if status.fault & 4 == 4 or self.hw_kill_raised is True:
return
self.supply_voltages.append(feedback.supply_voltage)
# Limits the list by removing the oldest readings when it contains more then 1000 readings
while (len(self.supply_voltages) > 1000):
del self.supply_voltages[0]
def publish_voltage(self, event):
'''
Publishes the average voltage across all four thrusters to the battery_voltage node
as a standard Float32 message and runs the voltage_check
'''
if (len(self.supply_voltages) > 0):
self.voltage = sum(self.supply_voltages) / \
len(self.supply_voltages)
self.pub_voltage.publish(self.voltage)
