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)