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 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):
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 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):
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)
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):
# 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
AlarmListener('hw-kill', self.hw_kill_cb)
# 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):
# 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, *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, 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, *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, 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, 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):
# 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, 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)
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 __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)
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)
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)
#!/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"
#!/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)
