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
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 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
#!/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 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 al.clear_callbacks() al.add_callback(cb1, severity_required=2) al.add_callback(cb2, call_when_raised=False)