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