def __init__(self):
# Initialize the average voltage to max
self.voltage = 29.4
# 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 = []
# The publisher for the averaged voltage
self.pub_voltage = rospy.Publisher("/battery_monitor",
Float32,
queue_size=1)
# Subscrives to the feedback from each of the four thrusters
rospy.Subscriber("/FL_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/FR_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/BL_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/BR_motor/feedback", Feedback, self.add_voltage)
# Sets up the battery voltage alarms
alarm_broadcaster = AlarmBroadcaster()
self.battery_low_alarm = alarm_broadcaster.add_alarm(
name='battery_low', action_required=False, severity=2)
self.battery_critical_alarm = alarm_broadcaster.add_alarm(
name='battery_critical', action_required=True, severity=1)
self.battery_kill_alarm = alarm_broadcaster.add_alarm(
name='kill', action_required=True, severity=0)
def __init__(self, port, baud=9600):
self.load_yaml()
rospy.init_node("actuator_driver")
alarm_broadcaster = AlarmBroadcaster()
self.disconnection_alarm = alarm_broadcaster.add_alarm(
name='actuator_board_disconnect', action_required=True, severity=0)
self.packet_error_alarm = alarm_broadcaster.add_alarm(
name='packet_error', action_required=False, severity=2)
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
# Reset all valves
rospy.loginfo("Resetting valves.")
for actuator_key in self.actuators:
actuator = self.actuators[actuator_key]
for valve_ports in actuator['ports']:
valve_port = actuator['ports'][valve_ports]
self.send_data(valve_port['id'], valve_port['default'])
rospy.loginfo("Valves ready to go.")
rospy.Service('~actuate', SetValve, self.got_service_request)
rospy.Service('~actuate_raw', SetValve, self.set_raw_valve)
r = rospy.Rate(.2) # hz
while not rospy.is_shutdown():
# Heartbeat to make sure the board is still connected.
r.sleep()
self.ping()
def __init__(self, port, baud=9600):
self.load_yaml()
rospy.init_node("actuator_driver")
alarm_broadcaster = AlarmBroadcaster()
self.disconnection_alarm = alarm_broadcaster.add_alarm(
name="actuator_board_disconnect", action_required=True, severity=0
)
self.packet_error_alarm = alarm_broadcaster.add_alarm(name="packet_error", action_required=False, severity=2)
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
# Reset all valves
rospy.loginfo("Resetting valves.")
for actuator_key in self.actuators:
actuator = self.actuators[actuator_key]
for valve_ports in actuator["ports"]:
valve_port = actuator["ports"][valve_ports]
self.send_data(valve_port["id"], valve_port["default"])
rospy.loginfo("Valves ready to go.")
rospy.Service("~actuate", SetValve, self.got_service_request)
rospy.Service("~actuate_raw", SetValve, self.set_raw_valve)
r = rospy.Rate(0.2) # hz
while not rospy.is_shutdown():
# Heartbeat to make sure the board is still connected.
r.sleep()
self.ping()
def __init__(self):
# Initialize the average voltage to max
self.voltage = 29.4
# 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 = []
# The publisher for the averaged voltage
self.pub_voltage = rospy.Publisher("/battery_monitor", Float32, queue_size=1)
# Subscrives to the feedback from each of the four thrusters
rospy.Subscriber("/FL_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/FR_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/BL_motor/feedback", Feedback, self.add_voltage)
rospy.Subscriber("/BR_motor/feedback", Feedback, self.add_voltage)
# Sets up the battery voltage alarms
alarm_broadcaster = AlarmBroadcaster()
self.battery_low_alarm = alarm_broadcaster.add_alarm(
name='battery_low',
action_required=False,
severity=2
)
self.battery_critical_alarm = alarm_broadcaster.add_alarm(
name='battery_critical',
action_required=True,
severity=1
)
self.battery_kill_alarm = alarm_broadcaster.add_alarm(
name='kill',
action_required=True,
severity=0
)
class ThrusterDriver(object):
def __init__(self, config_path, bus_layout):
'''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.alarm_broadcaster = AlarmBroadcaster()
self.thruster_out_alarm = self.alarm_broadcaster.add_alarm(
name='thruster_out',
action_required=True,
severity=2
)
self.bus_voltage_alarm = self.alarm_broadcaster.add_alarm(
name='bus_voltage',
action_required=False,
severity=2
)
self.failed_thrusters = []
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_config(config_path)
self.interpolate = scipy.interpolate.interp1d(newtons, thruster_input)
# Bus configuration
self.port_dict = self.load_bus_layout(bus_layout)
self.thrust_service = rospy.Service('thrusters/thruster_range', ThrusterInfo, self.get_thruster_info)
self.thrust_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=1)
self.status_pub = rospy.Publisher('thrusters/thruster_status', ThrusterStatus, queue_size=8)
# This is essentially only for testing
self.fail_thruster_server = rospy.Service('fail_thruster', FailThruster, self.fail_thruster)
def load_config(self, path):
'''Load the configuration data:
- Map force inputs from Newtons to [-1, 1] required by the thruster
'''
try:
_file = file(path)
except IOError, e:
rospy.logerr("Could not find thruster configuration file at {}".format(path))
raise(e)
json_data = json.load(_file)
newtons = json_data['calibration_data']['newtons']
thruster_input = json_data['calibration_data']['thruster_input']
return newtons, thruster_input
def __init__(self, hydrophone_locations, port, baud=9600):
rospy.init_node("sonar")
alarm_broadcaster = AlarmBroadcaster()
self.disconnection_alarm = alarm_broadcaster.add_alarm(
name='sonar_disconnect', action_required=True, severity=0)
self.packet_error_alarm = alarm_broadcaster.add_alarm(
name='sonar_packet_error', action_required=False, severity=2)
try:
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
except Exception, e:
print "\x1b[31mSonar serial connection error:\n\t", e, "\x1b[0m"
return None
def test_add_alarm(self):
'''Ensure that adding an alarm succeeds without errors'''
broadcaster = AlarmBroadcaster()
alarm = broadcaster.add_alarm(name='wake-up',
action_required=True,
severity=1,
problem_description='This is a problem',
parameters={"concern": ["a", "b", "c"]})
def test_add_alarm(self):
'''Ensure that adding an alarm succeeds without errors'''
broadcaster = AlarmBroadcaster()
alarm = broadcaster.add_alarm(
name='wake-up',
action_required=True,
severity=1,
problem_description='This is a problem',
parameters={"concern": ["a", "b", "c"]}
)
def __init__(self, method, c, hydrophone_locations, port, baud=19200):
rospy.init_node("sonar")
alarm_broadcaster = AlarmBroadcaster()
self.disconnection_alarm = alarm_broadcaster.add_alarm(
name='sonar_disconnect',
action_required=True,
severity=0
)
self.packet_error_alarm = alarm_broadcaster.add_alarm(
name='sonar_packet_error',
action_required=False,
severity=2
)
try:
self.ser = serial.Serial(port=port, baudrate=baud, timeout=None)
self.ser.flushInput()
except Exception, e:
print "\x1b[31mSonar serial connection error:\n\t", e, "\x1b[0m"
return None
def __init__(self, hydrophone_locations, wave_propagation_speed): # speed in m/s
self.hydrophone_locations = np.array([1, 1, 1]) # just for apending, will not be included
self.wave_propagation_speed = wave_propagation_speed
for key in hydrophone_locations:
sensor_location = np.array([hydrophone_locations[key]['x'], hydrophone_locations[key]['y'], hydrophone_locations[key]['z']])
self.hydrophone_locations = np.vstack((self.hydrophone_locations, sensor_location))
self.hydrophone_locations = self.hydrophone_locations[1:]
alarm_broadcaster = AlarmBroadcaster()
self.locate_pulse_error_alarm = alarm_broadcaster.add_alarm(
name='sonar_pulse_locating_error',
action_required=False,
severity=2
)
def __init__(self, hydrophone_locations,
wave_propagation_speed): # speed in m/s
self.hydrophone_locations = np.array(
[1, 1, 1]) # just for apending, will not be included
self.wave_propagation_speed = wave_propagation_speed
for key in hydrophone_locations:
sensor_location = np.array([
hydrophone_locations[key]['x'], hydrophone_locations[key]['y'],
hydrophone_locations[key]['z']
])
self.hydrophone_locations = np.vstack(
(self.hydrophone_locations, sensor_location))
self.hydrophone_locations = self.hydrophone_locations[1:]
alarm_broadcaster = AlarmBroadcaster()
self.locate_pulse_error_alarm = alarm_broadcaster.add_alarm(
name='sonar_pulse_locating_error',
action_required=False,
severity=2)
class Joystick(object):
def __init__(self):
self.force_scale = rospy.get_param("~force_scale", 600)
self.torque_scale = rospy.get_param("~torque_scale", 500)
self.wrench_choices = itertools.cycle(['rc', 'autonomous'])
self.current_pose = Odometry()
self.active = False
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(
name='kill', action_required=True, severity=0)
# self.docking_alarm = self.alarm_broadcaster.add_alarm(
# name='docking',
# action_required=True,
# severity=0
# )
self.wrench_pub = rospy.Publisher("/wrench/rc",
WrenchStamped,
queue_size=1)
self.kb = KillBroadcaster(id='station_hold', description='Resets Pose')
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy('/change_wrench',
WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
self.reset()
def reset(self):
'''
Used to reset the state of the controller.
Sometimes when it disconnects then comes back online, the settings are all
out of wack.
'''
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.start_count = 0
self.last_joy = None
self.active = False
self.killed = False
# self.docking = False
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.wrench.force.x = 0
wrench.wrench.force.y = 0
wrench.wrench.torque.z = 0
self.wrench_pub.publish(wrench)
def check_for_timeout(self, joy):
if self.last_joy is None:
self.last_joy = joy
return
if joy.axes == self.last_joy.axes and \
joy.buttons == self.last_joy.buttons:
# No change in state
if rospy.Time.now() - self.last_joy.header.stamp > rospy.Duration(
15 * 60):
# The controller times out after 15 minutes
if self.active:
rospy.logwarn(
"Controller Timed out. Hold start to resume.")
self.reset()
else:
joy.header.stamp = rospy.Time.now(
) # In the sim, stamps weren't working right
self.last_joy = joy
def joy_cb(self, joy):
self.check_for_timeout(joy)
# Handle Button presses
start = joy.buttons[7]
change_mode = bool(joy.buttons[3]) # Y
kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
# docking = bool(joy.buttons[1]) # B
rc_control = bool(joy.buttons[11]) # d-pad left
auto_control = bool(joy.buttons[12]) # d-pad right
# Reset controller state if only start is pressed down for awhile
self.start_count += start
if self.start_count > 10: # About 3 seconds
rospy.loginfo("Resetting controller state.")
self.reset()
self.active = True
self.kill_alarm.clear_alarm()
self.wrench_changer("rc")
if not self.active:
return
# Change vehicle mode
if change_mode == 1 and change_mode != self.last_change_mode:
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
if rc_control == 1 and rc_control != self.last_rc:
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
if auto_control == 1 and auto_control != self.last_auto:
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
# Station hold
if station_hold == 1 and station_hold != self.last_station_hold_state:
rospy.loginfo("Station Holding")
self.kb.send(active=True)
self.kb.send(
active=False) # Resets c3, this'll change when c3 is replaced
self.wrench_changer("autonomous")
# Turn on full system kill
if kill == 1 and kill != self.last_kill:
rospy.loginfo("Toggling Kill")
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(
problem_description='System kill from location: joystick')
self.killed = not self.killed
# # Turn on docking mode
# if docking == 1 and docking != self.last_docking_state:
# rospy.loginfo("Toggling Docking")
# if self.docking:
# self.docking_alarm.clear_alarm()
# else:
# self.docking_alarm.raise_alarm(
# problem_description='Docking kill from location: joystick'
# )
# self.docking = not self.docking
self.last_start = start
self.last_change_mode = change_mode
self.last_kill = kill
self.last_station_hold_state = station_hold
# self.last_docking_state = docking
self.last_auto_control = auto_control
self.last_rc = rc_control
self.last_auto = auto_control
# Handle joystick commands
left_stick_x = joy.axes[1]
left_stick_y = joy.axes[0]
right_stick_y = joy.axes[3]
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.header.stamp = joy.header.stamp
wrench.wrench.force.x = self.force_scale * left_stick_x
wrench.wrench.force.y = self.force_scale * left_stick_y
wrench.wrench.torque.z = self.torque_scale * right_stick_y
self.wrench_pub.publish(wrench)
class Joystick(object):
# Base class for whatever you are writing
def __init__(self):
self.force_scale = rospy.get_param("~force_scale")
self.torque_scale = rospy.get_param("~torque_scale")
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.killed = False
self.docking = False
self.wrench_choices = itertools.cycle(["rc", "autonomous"])
self.current_pose = Odometry()
self.alarm_broadcaster = AlarmBroadcaster()
self.kill_alarm = self.alarm_broadcaster.add_alarm(name="kill", action_required=True, severity=0)
self.docking_alarm = self.alarm_broadcaster.add_alarm(name="docking", action_required=True, severity=0)
self.wrench_pub = rospy.Publisher("/wrench/rc", WrenchStamped, queue_size=1)
self.kb = KillBroadcaster(id="station_hold", description="Reset Pose")
# rospy.wait_for_service('/change_wrench')
self.wrench_changer = rospy.ServiceProxy("/change_wrench", WrenchSelect)
rospy.Subscriber("joy", Joy, self.joy_cb)
rospy.Subscriber("odom", Odometry, self.odom_cb)
def odom_cb(self, msg):
self.current_pose = msg
def joy_cb(self, joy):
# Handle Button presses
change_mode = bool(joy.buttons[3]) # Y
kill = bool(joy.buttons[2]) # X
station_hold = bool(joy.buttons[0]) # A
docking = bool(joy.buttons[1]) # B
rc_control = bool(joy.buttons[11]) # d-pad left
auto_control = bool(joy.buttons[12]) # d-pad right
# Change vehicle mode
if change_mode == 1 and change_mode != self.last_change_mode:
mode = next(self.wrench_choices)
rospy.loginfo("Changing Control Mode: {}".format(mode))
self.wrench_changer(mode)
if rc_control == 1 and rc_control != self.last_rc:
rospy.loginfo("Changing Control to RC")
self.wrench_changer("rc")
if auto_control == 1 and auto_control != self.last_auto:
rospy.loginfo("Changing Control to Autonomous")
self.wrench_changer("autonomous")
# Station hold
if station_hold == 1 and station_hold != self.last_station_hold_state:
rospy.loginfo("Station Holding")
self.kb.send(active=True)
self.kb.send(active=False) # Resets c3, this'll change when c3 is replaced
self.wrench_changer("autonomous")
# Turn on full system kill
if kill == 1 and kill != self.last_kill:
rospy.loginfo("Toggling Kill")
if self.killed:
self.kill_alarm.clear_alarm()
else:
self.wrench_changer("rc")
self.kill_alarm.raise_alarm(problem_description="System kill from location: joystick")
self.killed = not self.killed
# Turn on docking mode
if docking == 1 and docking != self.last_docking_state:
rospy.loginfo("Toggling Docking")
if self.docking:
self.docking_alarm.clear_alarm()
else:
self.docking_alarm.raise_alarm(problem_description="Docking kill from location: joystick")
self.docking = not self.docking
self.last_change_mode = change_mode
self.last_kill = kill
self.last_station_hold_state = station_hold
self.last_docking_state = docking
self.last_auto_control = auto_control
self.last_rc = rc_control
self.last_auto = auto_control
# Handle joystick commands
left_stick_x = joy.axes[1]
left_stick_y = joy.axes[0]
right_stick_y = joy.axes[3]
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.wrench.force.x = self.force_scale * left_stick_x
wrench.wrench.force.y = self.force_scale * left_stick_y
wrench.wrench.torque.z = self.torque_scale * right_stick_y
self.wrench_pub.publish(wrench)