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')
AlarmListener('hw-kill', self.hw_kill_alarm_cb)
AlarmListener('kill', self.kill_alarm_cb)
rospy.Subscriber("/wrench/selected", String, self.wrench_cb)
rospy.Subscriber("/network", Header, self.network_cb) # Passes along network hearbeat to kill board
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 __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
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 __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 __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 __init__(self):
self.station_holding = False
self.kill_alarm = False
self.voltage = None
self.wrench = None
self.markers_pub = rospy.Publisher('/boat_info', Marker, queue_size=10)
rospy.Subscriber("/wrench/selected", String, self.wrench_current_cb)
rospy.Subscriber("/battery_monitor", Float32, self.battery_monitor_cb)
self.kill_listener = AlarmListener('kill',
callback_funct=self.kill_alarm_cb)
self.station_hold_listner = AlarmListener(
'station-hold', callback_funct=self.station_alarm_cb)
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)
def __init__(self, thrusters, effort_ratio, effort_limit):
self.boat_cog = np.zeros(2)
self.des_force = np.zeros(3)
self.positions = []
self.angles = []
for thruster in thrusters:
self.positions.append(
np.array(([thruster.cog[0], thruster.cog[1]])))
self.angles.append(thruster.angle)
# conversion from newtons to firmware units, measured during thruster testing
self.effort_ratio = effort_ratio
# limit for the value sent to motor driver firmware
self.effort_limit = effort_limit
self.kill = False
self.kill_listener = AlarmListener('kill', self.kill_cb)
# ROS data
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 __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
def __init__(self, *args, **kwargs):
super(ThrusterAndKillBoard, self).__init__(*args, **kwargs)
# Initialize thruster mapping from params
self.thrusters = make_thruster_dictionary(
rospy.get_param('/thruster_layout/thrusters'))
# Tracks last hw-kill alarm update
self._last_hw_kill = None
# Tracks last soft kill status received from board
self._last_soft_kill = None
# Tracks last hard kill status received from board
self._last_hard_kill = None
# Tracks last go status broadcasted
self._last_go = None
# Used to raise/clear hw-kill when board updates
self._kill_broadcaster = AlarmBroadcaster('hw-kill')
# Alarm broadaster for GO command
self._go_alarm_broadcaster = AlarmBroadcaster('go')
# Listens to hw-kill updates to ensure another nodes doesn't manipulate it
self._hw_kill_listener = AlarmListener('hw-kill',
callback_funct=self.on_hw_kill)
# Provide service for alarm handler to set/clear the motherboard kill
self._unkill_service = rospy.Service('/set_mobo_kill', SetBool,
self.set_mobo_kill)
# Sends hearbeat to board
self._hearbeat_timer = rospy.Timer(rospy.Duration(0.4),
self.send_heartbeat)
# Create a subscribe for thruster commands
self._sub = rospy.Subscriber('/thrusters/thrust',
Thrust,
self.on_command,
queue_size=10)
def __init__(self, controller_name, wrench_pub=None):
self.name = controller_name
self.wrench_choices = itertools.cycle(
["rc", "emergency", "keyboard", "autonomous"])
self.kill_broadcaster = AlarmBroadcaster('kill')
self.station_hold_broadcaster = AlarmBroadcaster('station-hold')
self.wrench_changer = rospy.ServiceProxy("/wrench/select", MuxSelect)
self.kill_listener = AlarmListener(
'kill', callback_funct=self._update_kill_status)
if (wrench_pub is None):
self.wrench_pub = wrench_pub
else:
self.wrench_pub = rospy.Publisher(wrench_pub,
WrenchStamped,
queue_size=1)
self.shooter_load_client = actionlib.SimpleActionClient(
"/shooter/load", ShooterDoAction)
self.shooter_fire_client = actionlib.SimpleActionClient(
"/shooter/fire", ShooterDoAction)
self.shooter_cancel_client = rospy.ServiceProxy(
"/shooter/cancel", Trigger)
self.shooter_manual_client = rospy.ServiceProxy(
"/shooter/manual", ShooterManual)
self.shooter_reset_client = rospy.ServiceProxy("/shooter/reset",
Trigger)
self.is_killed = False
self.is_timed_out = False
self.clear_wrench()
def __init__(self, config_path, ports, thruster_definitions):
'''Thruster driver, an object for commanding all of the sub's thrusters
- Gather configuration data and make it available to other nodes
- Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
- Track a thrust_dict, which maps thruster names to the appropriate port
- Given a command message, route that command to the appropriate port/thruster
- Send a thruster status message describing the status of the particular thruster
'''
self.thruster_heartbeats = {}
self.failed_thrusters = []
# Bus voltage
self.bus_voltage_estimator = BusVoltageEstimator(self._window_duration)
self.bus_voltage_pub = rospy.Publisher('bus_voltage',
Float64,
queue_size=1)
self.bus_timer = rospy.Timer(rospy.Duration(0.1),
self.publish_bus_voltage)
self.warn_voltage = rospy.get_param("/battery/warn_voltage", 44.5)
self.kill_voltage = rospy.get_param("/battery/kill_voltage", 44.0)
self.bus_voltage_alarm = AlarmBroadcaster("bus-voltage")
self.make_fake = rospy.get_param('simulate', False)
if self.make_fake:
rospy.logwarn(
"Running fake thrusters for simulation, based on parameter '/simulate'"
)
# Individual thruster configuration data
newtons, thruster_input = self.load_effort_to_thrust_map(config_path)
self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input)
self.thrust_service = rospy.Service('thrusters/thruster_range',
ThrusterInfo,
self.get_thruster_info)
self.status_pub = rospy.Publisher('thrusters/thruster_status',
ThrusterStatus,
queue_size=8)
# Port and thruster layout
self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
AlarmListener("thruster-out",
self.check_alarm_status,
call_when_raised=False)
self.port_dict = self.load_thruster_layout(ports, thruster_definitions)
self.drop_check = rospy.Timer(rospy.Duration(0.5),
self.check_for_drops)
# The bread and bones
self.thrust_sub = rospy.Subscriber('thrusters/thrust',
Thrust,
self.thrust_cb,
queue_size=1)
# This is essentially only for testing
self.fail_thruster_server = rospy.Service('fail_thruster',
FailThruster,
self.fail_thruster)
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 test_callbacks(self):
al_a = AlarmListener("alarm_d")
ab_a = AlarmBroadcaster("alarm_d")
self.raised = False
self.cleared = False
self.both = False
ab_a.clear_alarm()
al_a.add_callback(self.raised_cb, call_when_cleared=False)
al_a.add_callback(self.cleared_cb, call_when_raised=False)
al_a.add_callback(self.both_cb)
rospy.sleep(0.5)
# Make sure callbacks are called on creation
self.assertFalse(self.raised)
self.assertTrue(self.cleared)
self.assertTrue(self.both)
self.raised = False
self.cleared = False
self.both = False
# Make sure callbacks run when they're suppsed to
ab_a.raise_alarm()
rospy.sleep(0.5)
self.assertTrue(self.raised)
self.assertFalse(self.cleared)
self.assertTrue(self.both)
self.raised = False
self.cleared = False
self.both = False
def __init__(self, ports_layout, thruster_definitions):
'''Thruster driver, an object for commanding all of the sub's thrusters
- Gather configuration data and make it available to other nodes
- Instantiate ThrusterPorts, (Either simulated or real), for communicating with thrusters
- Track a thrust_dict, which maps thruster names to the appropriate port
- Given a command message, route that command to the appropriate port/thruster
- Send a thruster status message describing the status of the particular thruster
'''
self.failed_thrusters = set() # This is only determined by comms
self.deactivated_thrusters = set(
) # These will not come back online even if comms are good (user managed)
# Alarms
self.thruster_out_alarm = AlarmBroadcaster("thruster-out")
AlarmListener("thruster-out",
self.check_alarm_status,
call_when_raised=False) # Prevent outside interference
# Create ThrusterPort objects in a dict indexed by port name
self.load_thruster_ports(ports_layout, thruster_definitions)
# Feedback on thrusters (thruster mapper blocks until it can use this service)
self.thruster_info_service = rospy.Service('thrusters/thruster_info',
ThrusterInfo,
self.get_thruster_info)
self.status_publishers = {
name: rospy.Publisher('thrusters/status/' + name,
ThrusterStatus,
queue_size=10)
for name in self.thruster_to_port_map.keys()
}
# These alarms require this service to be available before things will work
rospy.wait_for_service("update_thruster_layout")
self.update_thruster_out_alarm()
# Bus voltage
self.bus_voltage_monitor = BusVoltageMonitor(self._window_duration)
# Command thrusters
self.thrust_sub = rospy.Subscriber('thrusters/thrust',
Thrust,
self.thrust_cb,
queue_size=1)
# To programmatically deactivate thrusters
self.fail_thruster_server = rospy.Service('fail_thruster',
FailThruster,
self.fail_thruster)
self.unfail_thruster_server = rospy.Service('unfail_thruster',
UnfailThruster,
self.unfail_thruster)
def connect_ros(self):
'''
Connect ROS nodes, services, and alarms to variables and methods
within this class.
'''
# Attempts to read the battery voltage parameters (sets them to defaults if they have not been set)
self.battery_low_voltage = rospy.get_param(
"/battery_monitor/battery_low_voltage", 24)
self.battery_critical_voltage = rospy.get_param(
"/battery_monitor/battery_critical_voltage", 20)
rospy.Subscriber("/wrench/selected", String, self.cache_operating_mode)
rospy.Subscriber("/battery_monitor", Float32,
self.cache_battery_voltage)
rospy.Subscriber("/host_monitor", Hosts, self.cache_hosts)
self.kill_listener = AlarmListener(
"kill", callback_funct=self.cache_kill_status)
#!/usr/bin/env python
import rospy
from ros_alarms import AlarmListener, HeartbeatMonitor
from std_msgs.msg import String
def printit(alarm):
rospy.loginfo("Alarm {} raised".format(alarm.alarm_name))
if __name__ == "__main__":
rospy.init_node("heartbeat_test")
pub = rospy.Publisher("/heartbeat", String, queue_size=1)
al = AlarmListener("kill")
al.add_callback(printit, call_when_cleared=False)
hm = HeartbeatMonitor("kill", "/heartbeat", String, 1)
hm.clear_alarm()
rospy.loginfo("Timeout test")
for i in range(20):
pub.publish("testing 1, 2, 3")
rospy.sleep(0.1)
rospy.sleep(2)
assert al.is_raised()
rospy.loginfo("Revive test")
pub.publish("testing 1, 2, 3")
rospy.sleep(0.1)
assert al.is_cleared()
rospy.sleep(2)
def test_broadcaster_and_listener(self):
''' Simple broadcaster and listener tests, with arguments '''
ab_a = AlarmBroadcaster("alarm_a")
al_a = AlarmListener("alarm_a")
al_b = AlarmListener("alarm_b")
ab_b = AlarmBroadcaster("alarm_b")
al_c = AlarmListener("alarm_c")
ab_c = AlarmBroadcaster("alarm_c")
# Make sure all the alarm start off clear
self.assertFalse(al_a.is_raised())
self.assertFalse(al_b.is_raised())
self.assertFalse(al_b.is_raised())
# Same as above
self.assertTrue(al_a.is_cleared())
self.assertTrue(al_b.is_cleared())
self.assertTrue(al_c.is_cleared())
# Some args to pass in
_severity = 3
_blank_params = ''
_full_params = {'test': 1, 'test2': 2}
_problem_desc = "This is a test"
# Raise them all, with some arguments
ab_a.raise_alarm(severity=_severity)
ab_b.raise_alarm(severity=_severity, parameters=_blank_params)
ab_c.raise_alarm(problem_description=_problem_desc, parameters=_full_params)
# Make sure all the alarm start off clear
self.assertTrue(al_a.is_raised())
self.assertTrue(al_b.is_raised())
self.assertTrue(al_c.is_raised())
# Make sure alarm values are correct
self.assertEqual(al_a.get_alarm().severity, _severity)
self.assertEqual(al_b.get_alarm().severity, _severity)
self.assertEqual(al_b.get_alarm().parameters, _blank_params)
self.assertEqual(al_c.get_alarm().problem_description, _problem_desc)
self.assertEqual(al_c.get_alarm().parameters, _full_params)
# Now clear the alarms, some again with arguments
ab_a.clear_alarm()
ab_b.clear_alarm(parameters=_full_params)
ab_c.clear_alarm(parameters=_blank_params)
# Make sure all alarms are cleared
self.assertTrue(al_a.is_cleared())
self.assertTrue(al_b.is_cleared())
self.assertTrue(al_c.is_cleared())
# Make sure arugments were passed correctly
self.assertEqual(al_b.get_alarm().parameters, _full_params)
self.assertEqual(al_c.get_alarm().parameters, _blank_params)
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state",
visualization_msgs.Marker,
queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t",
visualization_msgs.Marker,
queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage",
visualization_msgs.Marker,
queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage',
44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage',
44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped,
self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped,
self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64,
self.voltage_callback)
class RvizVisualizer(object):
'''Cute tool for drawing both depth and height-from-bottom in RVIZ
'''
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state",
visualization_msgs.Marker,
queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t",
visualization_msgs.Marker,
queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage",
visualization_msgs.Marker,
queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage',
44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage',
44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped,
self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped,
self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64,
self.voltage_callback)
def update_kill_button(self):
if self.killed:
self.kill_marker.controls[0].markers[0].text = "KILLED"
self.kill_marker.controls[0].markers[0].color.r = 1
self.kill_marker.controls[0].markers[0].color.g = 0
else:
self.kill_marker.controls[0].markers[0].text = "UNKILLED"
self.kill_marker.controls[0].markers[0].color.r = 0
self.kill_marker.controls[0].markers[0].color.g = 1
self.kill_server.insert(self.kill_marker)
self.kill_server.applyChanges()
def kill_alarm_callback(self, alarm):
self.need_kill_update = False
self.killed = alarm.raised
self.update_kill_button()
def kill_buttton_callback(self, feedback):
if not feedback.event_type == 3:
return
if self.need_kill_update:
return
self.need_kill_update = True
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.kill_alarm.raise_alarm()
def voltage_callback(self, voltage):
self.voltage_marker.text = str(round(voltage.data, 2)) + ' volts'
self.voltage_marker.header.stamp = rospy.Time()
if voltage.data < self.low_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 0
elif voltage.data < self.warn_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 1
else:
self.voltage_marker.color.r = 0
self.voltage_marker.color.g = 1
self.rviz_pub_utils.publish(self.voltage_marker)
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.depth
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns = 'sub'
self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.range
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns = 'sub'
self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def make_cylinder_marker(self,
base,
length,
color,
frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]),
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs)
return marker
class killtest(unittest.TestCase):
def __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
def callback(self, msg):
self._current_alarm_state = msg
def test_AA_initial_state(self): # test the initial state of kill signal
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.1)
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
def test_AB_computer(self):
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
self.AlarmBroadcaster.raise_alarm() # raise the computer alarm
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(),
True,
msg='COMPUTER raise not worked')
self.AlarmBroadcaster.clear_alarm() # clear the computer alarm
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='COMPUTER shutdown not worked')
def test_AC_button_front_port(self):
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
# call the service of button
rospy.wait_for_service('/kill_board_interface/BUTTON_FRONT_PORT')
try:
bfp = rospy.ServiceProxy('/kill_board_interface/BUTTON_FRONT_PORT',
SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bfp(True) # set the button value to true
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(),
True,
msg='BUTTON_FRONT_PORT raise not worked')
bfp(False) # shut down the button
# when we shut down the button, we also need to clear the computer
# alarm
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_FRONT_PORT shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AD_button_aft_port(self):
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_interface/BUTTON_AFT_PORT')
try:
bap = rospy.ServiceProxy('/kill_board_interface/BUTTON_AFT_PORT',
SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bap(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(),
True,
msg='BUTTTON_AFT_PORT raise not worked')
bap(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTTON_AFT_PORT shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AE_button_front_starboard(self):
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_interface/BUTTON_FRONT_STARBOARD')
try:
bfs = rospy.ServiceProxy(
'/kill_board_interface/BUTTON_FRONT_STARBOARD', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bfs(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(),
True,
msg='BUTTON_FRONT_STARBOARD raise not worked')
bfs(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_FRONT_STARBOARD shutdown not worked. State = {}'.
format(self._current_alarm_state))
def test_AF_button_aft_starboard(self):
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_interface/BUTTON_AFT_STARBOARD')
try:
bas = rospy.ServiceProxy(
'/kill_board_interface/BUTTON_AFT_STARBOARD', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bas(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(),
True,
msg='BUTTON_AFT_STARBOARD raise not worked')
bas(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_AFT_STARBOARD shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AG_remote(self):
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
# publishing msg to network
pub = rospy.Publisher('/network', Header, queue_size=10)
rate = rospy.Rate(10)
t_end = rospy.Time.now() + rospy.Duration(3)
while rospy.Time.now() < t_end:
hello_header = Header()
hello_header.stamp = rospy.Time.now()
rospy.loginfo(hello_header)
pub.publish(hello_header)
rate.sleep()
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='REMOTE shutdown not worked. State = {}'.format(
self._current_alarm_state))
# stop sending the msg, the remote alarm will raise when stop recieving
# the msg for 8 secs
rospy.sleep(8.2)
self.assertEqual(self.AlarmListener.is_raised(),
True,
msg='REMOTE raised not worked. State = {}'.format(
self._current_alarm_state))
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
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
class RvizVisualizer(object):
'''Cute tool for drawing both depth and height-from-bottom in RVIZ
'''
def __init__(self):
rospy.init_node('revisualizer')
self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2)
self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2)
self.kill_server = InteractiveMarkerServer("interactive_kill")
# text marker
# TODO: Clean this up, there should be a way to set all of this inline
self.surface_marker = visualization_msgs.Marker()
self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING
self.surface_marker.color = ColorRGBA(1, 1, 1, 1)
self.surface_marker.scale.z = 0.1
self.depth_marker = visualization_msgs.Marker()
self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING
self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
self.depth_marker.scale.z = 0.1
# create marker for displaying current battery voltage
self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0)
self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5)
self.voltage_marker = visualization_msgs.Marker()
self.voltage_marker.header.frame_id = "base_link"
self.voltage_marker.lifetime = rospy.Duration(5)
self.voltage_marker.ns = 'sub'
self.voltage_marker.id = 22
self.voltage_marker.pose.position.x = -2.0
self.voltage_marker.scale.z = 0.2
self.voltage_marker.color.a = 1
self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING
# create an interactive marker to display kill status and change it
self.need_kill_update = True
self.kill_marker = InteractiveMarker()
self.kill_marker.header.frame_id = "base_link"
self.kill_marker.pose.position.x = -2.3
self.kill_marker.name = "kill button"
kill_status_marker = Marker()
kill_status_marker.type = Marker.TEXT_VIEW_FACING
kill_status_marker.text = "UNKILLED"
kill_status_marker.id = 55
kill_status_marker.scale.z = 0.2
kill_status_marker.color.a = 1.0
kill_button_control = InteractiveMarkerControl()
kill_button_control.name = "kill button"
kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON
kill_button_control.markers.append(kill_status_marker)
self.kill_server.insert(self.kill_marker, self.kill_buttton_callback)
self.kill_marker.controls.append(kill_button_control)
self.killed = False
# connect kill marker to kill alarm
self.kill_listener = AlarmListener("kill")
self.kill_listener.add_callback(self.kill_alarm_callback)
self.kill_alarm = AlarmBroadcaster("kill")
# distance to bottom
self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback)
# distance to surface
self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback)
# battery voltage
self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def update_kill_button(self):
if self.killed:
self.kill_marker.controls[0].markers[0].text = "KILLED"
self.kill_marker.controls[0].markers[0].color.r = 1
self.kill_marker.controls[0].markers[0].color.g = 0
else:
self.kill_marker.controls[0].markers[0].text = "UNKILLED"
self.kill_marker.controls[0].markers[0].color.r = 0
self.kill_marker.controls[0].markers[0].color.g = 1
self.kill_server.insert(self.kill_marker)
self.kill_server.applyChanges()
def kill_alarm_callback(self, alarm):
self.need_kill_update = False
self.killed = alarm.raised
self.update_kill_button()
def kill_buttton_callback(self, feedback):
if not feedback.event_type == 3:
return
if self.need_kill_update:
return
self.need_kill_update = True
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.kill_alarm.raise_alarm()
def voltage_callback(self, voltage):
self.voltage_marker.text = str(round(voltage.data, 2)) + ' volts'
self.voltage_marker.header.stamp = rospy.Time()
if voltage.data < self.low_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 0
elif voltage.data < self.warn_battery_threshold:
self.voltage_marker.color.r = 1
self.voltage_marker.color.g = 1
else:
self.voltage_marker.color.r = 0
self.voltage_marker.color.g = 1
self.rviz_pub_utils.publish(self.voltage_marker)
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.depth
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns = 'sub'
self.surface_marker.header = mil_ros_tools.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.range
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns = 'sub'
self.depth_marker.header = mil_ros_tools.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def make_cylinder_marker(self, base, length, color, frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(
position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def test_stress(self):
''' Stress test the server, lots of raises and clears '''
ab_a = AlarmBroadcaster("alarm_a")
al_a = AlarmListener("alarm_a")
ab_b = AlarmBroadcaster("alarm_b")
al_b = AlarmListener("alarm_b")
al_c = AlarmListener("alarm_c")
ab_c = AlarmBroadcaster("alarm_c")
actions = [ab_a.raise_alarm, ab_b.raise_alarm, ab_c.raise_alarm,
ab_a.clear_alarm, ab_b.clear_alarm, ab_c.clear_alarm]
for i in range(100):
random.choice(actions)()
rospy.sleep(0.01)
# Clear them all as a find state
ab_a.clear_alarm()
ab_b.raise_alarm()
ab_c.raise_alarm()
# Make sure all alarms are correct
self.assertTrue(al_a.is_cleared())
self.assertFalse(al_b.is_cleared())
self.assertFalse(al_c.is_cleared())
# Set everyhing cleared
ab_b.clear_alarm()
ab_c.clear_alarm()
# Make sure of that
self.assertTrue(al_b.is_cleared())
self.assertTrue(al_c.is_cleared())
def __init__(self, *args):
self.hw_kill_alarm = None
self.updated = False
self.AlarmListener = AlarmListener('hw-kill', self._hw_kill_cb)
self.AlarmBroadcaster = AlarmBroadcaster('kill')
super(killtest, self).__init__(*args)
print alarm.alarm_name, alarm.raised
cb1_ran = True
print alarm
print "Callback1!"
def cb2(alarm):
global cb2_ran
cb2_ran = True
print "Callback2!"
if __name__ == "__main__":
rospy.init_node("callback_test")
ab = AlarmBroadcaster("test_alarm")
al = AlarmListener("test_alarm")
ab.clear_alarm()
rospy.sleep(0.1)
al.add_callback(cb1)
print "Inited"
assert al.is_cleared()
rospy.loginfo("Raise Test")
ab.raise_alarm()
rospy.sleep(0.1)
assert al.is_raised()
assert cb1_ran
cb1_ran = False
def __init__(self):
self.controller_file = rospy.get_param('~controller/controller_serial',
'/dev/ttyUSB0')
use_sim = rospy.get_param('/is_simulation', False)
if use_sim:
rospy.loginfo("Shooter controller running in simulation mode")
print "Running in sim"
self.motor_controller = Sabertooth2x12(self.controller_file,
sim=True)
else:
self.motor_controller = Sabertooth2x12(self.controller_file)
rospy.loginfo("Shooter connecting to motor controller at: %s",
self.controller_file)
self.load_retract_time = rospy.Duration(
0,
rospy.get_param('~controller/load/retract_time_millis', 950) *
1000000)
self.load_extend_time = rospy.Duration(
0,
rospy.get_param('~controller/load/extend_time_millis', 500) *
1000000)
self.load_pause_time = rospy.Duration(
0,
rospy.get_param('~controller/load/pause_time_millis', 100) *
1000000)
self.load_total_time = self.load_retract_time + self.load_pause_time + self.load_extend_time
self.fire_retract_time = rospy.Duration(
0,
rospy.get_param('~controller/fire/retract_time_millis', 400) *
1000000)
self.fire_shoot_time = rospy.Duration(
0,
rospy.get_param('~controller/fire/shoot_time_millis', 1000) *
1000000)
self.total_fire_time = max(self.fire_shoot_time,
self.fire_retract_time)
self.loaded = False # Assume linear actuator starts forward
self.stop = False
self.motor1_stop = 0
self.motor2_stop = 0
self.fire_server = actionlib.SimpleActionServer(
'/shooter/fire', ShooterDoAction, self.fire_execute_callback,
False)
self.fire_server.start()
self.load_server = actionlib.SimpleActionServer(
'/shooter/load', ShooterDoAction, self.load_execute_callback,
False)
self.load_server.start()
self.cancel_service = rospy.Service('/shooter/cancel', Trigger,
self.cancel_callback)
self.manual_service = rospy.Service('/shooter/manual', ShooterManual,
self.manual_callback)
self.reset_service = rospy.Service('/shooter/reset', Trigger,
self.reset_callback)
self.status = "Standby"
self.status_pub = rospy.Publisher('/shooter/status',
String,
queue_size=5)
self.manual_used = False
self.killed = False
self.kill_listener = AlarmListener(
"kill", callback_funct=self.update_kill_status)
class killtest(unittest.TestCase):
def __init__(self, *args):
self._current_alarm_state = None
rospy.Subscriber("/diagnostics", DiagnosticArray, self.callback)
super(killtest, self).__init__(*args)
self.AlarmListener = AlarmListener('hw-kill')
self.AlarmBroadcaster = AlarmBroadcaster('kill')
self.AlarmBroadcaster.clear_alarm()
def callback(self, msg):
self._current_alarm_state = msg
def test_AA_initial_state(self): # test the initial state of kill signal
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.1)
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
def test_AB_computer(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
self.AlarmBroadcaster.raise_alarm() # raise the computer alarm
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_raised(),
True,
msg='COMPUTER raise not worked')
self.AlarmBroadcaster.clear_alarm() # clear the computer alarm
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='COMPUTER shutdown not worked')
def test_AC_button_front_port(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
# call the service of button
rospy.wait_for_service('/kill_board_driver/BUTTON_FRONT_PORT')
try:
bfp = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_FRONT_PORT', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bfp(True) # set the button value to true
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTON_FRONT_PORT raise not worked')
bfp(False) # shut down the button
# when we shut down the button, we also need to clear the computer
# alarm
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_FRONT_PORT shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AD_button_aft_port(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_driver/BUTTON_AFT_PORT')
try:
bap = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_AFT_PORT', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bap(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTTON_AFT_PORT raise not worked')
bap(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTTON_AFT_PORT shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AE_button_front_starboard(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_driver/BUTTON_FRONT_STARBOARD')
try:
bfs = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_FRONT_STARBOARD', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bfs(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTON_FRONT_STARBOARD raise not worked')
bfs(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_FRONT_STARBOARD shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AF_button_aft_starboard(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
rospy.wait_for_service('/kill_board_driver/BUTTON_AFT_STARBOARD')
try:
bas = rospy.ServiceProxy(
'/kill_board_driver/BUTTON_AFT_STARBOARD', SetBool)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
bas(True)
rospy.sleep(0.2)
self.assertEqual(self.AlarmListener.is_raised(), True,
msg='BUTTON_AFT_STARBOARD raise not worked')
bas(False)
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.2)
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='BUTTON_AFT_STARBOARD shutdown not worked. State = {}'.format(
self._current_alarm_state))
def test_AG_remote(self):
self.assertEqual(self.AlarmListener.is_cleared(), True,
msg='current state of kill signal is raised')
# publishing msg to network
pub = rospy.Publisher('/network', Header, queue_size=10)
rate = rospy.Rate(10)
t_end = rospy.Time.now() + rospy.Duration(3)
while rospy.Time.now() < t_end:
hello_header = Header()
hello_header.stamp = rospy.Time.now()
rospy.loginfo(hello_header)
pub.publish(hello_header)
rate.sleep()
self.assertEqual(
self.AlarmListener.is_cleared(),
True,
msg='REMOTE shutdown not worked. State = {}'.format(
self._current_alarm_state))
# stop sending the msg, the remote alarm will raise when stop recieving
# the msg for 8 secs
rospy.sleep(8.2)
self.assertEqual(
self.AlarmListener.is_raised(),
True,
msg='REMOTE raised not worked. State = {}'.format(
self._current_alarm_state))
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 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)
#!/usr/bin/env python
import rospy
from ros_alarms import AlarmListener, AlarmBroadcaster
if __name__ == "__main__":
rospy.init_node("broadcaster_listener_test")
ab = AlarmBroadcaster("test_alarm")
al = AlarmListener("test_alarm")
print "Inited"
assert al.is_cleared()
ab.raise_alarm()
assert al.is_raised()
rospy.sleep(0.5)
ab.clear_alarm()
assert al.is_cleared()
rospy.sleep(0.5)
ab.raise_alarm(parameters={
"int": 4,
"list": [1, 2, 3],
"str": "stringing"
})
print "All checks passed"
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)
