示例#1
0
    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)
示例#2
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()
示例#3
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(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
        )
示例#5
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
示例#6
0
    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
示例#7
0
 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"]})
示例#8
0
 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"]}
     )
示例#9
0
    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
示例#10
0
    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
        )
示例#11
0
    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)
示例#13
0
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)