def update_kb(self):
kus = KnowledgeUpdateServiceArrayRequest()
# Get info from KB functions and propositions (to know type of variable)
# Fill update
self.mutex.acquire(True)
sensed_predicates = self.sensed_topics.copy()
self.mutex.release()
self.srv_mutex.acquire(True)
sensed_predicates.update(self.sensed_services.copy())
self.srv_mutex.release()
for predicate, (val, changed) in sensed_predicates.iteritems():
if not changed:
continue
if predicate in self.sensed_topics:
self.mutex.acquire(True)
self.sensed_topics[predicate] = (
self.sensed_topics[predicate][0], False
) # Set to not changed
self.mutex.release()
else:
self.srv_mutex.acquire(True)
self.sensed_services[predicate] = (
self.sensed_services[predicate][0], False
) # Set to not changed
self.srv_mutex.release()
predicate_info = predicate.split(':')
ki = KnowledgeItem()
ki.attribute_name = predicate_info.pop(0)
if type(val) is bool:
ki.knowledge_type = ki.FACT
ki.is_negative = not val
else:
ki.knowledge_type = ki.FUNCTION
ki.function_value = val
# TODO what to do if no parameters specified? iterate over all the instantiated parameters and add them?
for i, param in enumerate(predicate_info):
kv = KeyValue()
kv.key = 'p' + str(i)
kv.value = param
ki.values.append(kv)
kus.update_type += np.array(kus.ADD_KNOWLEDGE).tostring()
kus.knowledge.append(ki)
# Update the KB with the full array
if len(kus.update_type) > 0:
try:
self.update_kb_srv.call(kus)
except Exception as e:
rospy.logerr(
"KCL (SensingInterface) Failed to update knowledge base: %s"
% e.message)
def __init__(self, platform=None):
smach.State.__init__(self,
outcomes=['success', 'failed'],
input_keys=['goal'])
self.client = SimpleActionClient('unstage_object_server',
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
if platform is not None:
self.goal.parameters.append(
KeyValue(key='platform', value=str(platform).upper()))
def publish_motor_status(self, event=None):
status = self.motor.get_status()
data_list = []
for key in status:
data_list.append(KeyValue(key, str(status[key])))
msg = DiagnosticStatus()
msg.values = data_list
self.motor_status_pub.publish(msg)
def do_step(self):
path_id, direction = self.rhbp_agent.local_map.get_go_to_goal_area_move(
self.path_to_goal_area_id)
self.path_to_goal_area_id = path_id
if direction is not None:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name +
" executing move to " + str(direction))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_MOVE,
params=params)
def predicate_maker(attr_name, attr_key, attr_value, is_negative=None):
ki = KnowledgeItem()
ki.knowledge_type = 1
ki.attribute_name = attr_name
ki_values = []
if type(attr_key) == list:
for key, value in zip(attr_key, attr_value):
kv = KeyValue()
kv.key = key
kv.value = value
ki_values.append(kv)
else:
kv = KeyValue()
kv.key = attr_key
kv.value = attr_value
ki_values.append(kv)
ki.values = ki_values
if is_negative:
ki.is_negative = is_negative
return ki
def gpu_status(hardware, gpu):
"""
Decode and build a diagnostic status message
Fields:
* min_freq - Minimum frequency in kHz
* max_freq - Maximum frequency in kHz
* frq - Running frequency in kHz
* val - Status GPU, value between [0, 100]
"""
d_gpu = DiagnosticStatus(
name='jetson_stats gpu',
message='{val}%'.format(val=gpu['val']),
hardware_id=hardware,
#values=[KeyValue('Val', '{val}%'.format(val=gpu['val'])), KeyValue("Freq", "{frq}".format(frq=gpu['frq'])), KeyValue("Unit", "khz")])
values=[
KeyValue(key='val', value=str(gpu['val'])),
KeyValue(key='Freq', value=str(gpu['frq'])),
KeyValue(key='Unit', value="khz")
])
return d_gpu
def _laptop_charge_to_diag(laptop_msg):
rv = DiagnosticStatus()
rv.level = DiagnosticStatus.OK
rv.message = 'OK'
rv.name = rospy.get_name()
if rv.name.startswith('/'):
rv.name = rv.name[1:]
if not laptop_msg.present:
rv.level = DiagnosticStatus.ERROR
rv.message = 'Laptop battery missing'
rv.values.append(KeyValue('Voltage (V)', str(laptop_msg.voltage)))
rv.values.append(KeyValue('Current (A)', str(laptop_msg.rate)))
rv.values.append(KeyValue('Charge (Ah)', str(laptop_msg.charge)))
rv.values.append(KeyValue('Capacity (Ah)', str(laptop_msg.capacity)))
rv.values.append(
KeyValue('Design Capacity (Ah)', str(laptop_msg.design_capacity)))
return rv
def predicate_obj_to_ros(predicate):
"""
:predicate: Predicate
:returns: PredicateROS
"""
return PredicateROS(name=predicate.name,
params=[
KeyValue(key=param.name, value=param.value)
for param in predicate.params
])
def cb_diagnostics_update(self):
tabpage_status = DiagnosticStatus()
tabpage_status.level = tabpage_status.OK
tabpage_status.name = self.handler_name + " Handler"
tabpage_status.hardware_id = self.ros_name
tabpage_status.message = "OK"
tabpage_status.values = []
tabpage_status.values.append(
KeyValue(key="Number of Clients",
value=str(len(self.osc_clients))))
return tabpage_status
def __init__(self, platform=None):
smach.State.__init__(self,
outcomes=["success", "failed"],
input_keys=["goal"])
self.client = SimpleActionClient("unstage_object_server",
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
if platform is not None:
self.goal.parameters.append(
KeyValue(key="platform", value=str(platform).upper()))
def execute(self, userdata):
result = GenericExecuteResult()
for i, obj in enumerate(self.perceived_cavity_names):
result.results.append(
KeyValue(key="cavity_" + str(i + 1), value=obj))
rospy.loginfo(result)
userdata.result = result
self.perceived_cavity_names = [
] # clear perceived objects for next call
return "succeeded"
def add_mission(self, WTs):
"""
Add (inspection) missions for ASVs to do
"""
params = list()
pred_names = list()
update_types = list()
inspect_wps = ['asv_wp' + str(self.wt_to_wp[wt]) for wt in WTs]
for wp in inspect_wps:
pred_names.append('turbine_inspected_at')
params.append([KeyValue('wp', wp)])
update_types.append(KnowledgeUpdateServiceRequest.ADD_GOAL)
pred_names.append('inspect_post')
params.append([KeyValue('wp', wp)])
update_types.append(KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE)
self.goal_state[0].extend(pred_names)
self.goal_state[1].extend(params)
succeed = self.update_predicates(pred_names, params, update_types)
succeed = succeed and self.home_mission()
return succeed
def _fuel_update(self, event):
"""
Update fuel value on ROSPlan knowledge base
"""
for asv in self.asvs:
# fuel status update
self.update_functions(
['fuel'], [[KeyValue('v', asv.namespace)]], [asv.fuel], [
KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE,
])
self.lowfuel[asv.namespace] = asv.low_fuel
def __execute_turn_valve__(self, action_id, params):
if float(params[1]) != 0.0:
feedback = ActionFeedback()
feedback.action_id = action_id
feedback.status = 'action enabled'
# disable valve angle update
self.disable_valve_orientation_srv(EmptyRequest())
goal = ValveTurningGoal()
goal.valve_id = int(params[0])
goal.long_approach = False
desired_increment = float(params[1])
desired_increment = np.sign(
desired_increment) * 0.2 + desired_increment
goal.desired_increment = -1.0 * desired_increment
self.turn_valve_action.send_goal(goal)
self.pub_feedback.publish(feedback)
rospy.loginfo('%s: turn valve action enabled', self.name)
self.turn_valve_action.wait_for_result()
action_result = self.turn_valve_action.get_result()
# if action_result.valve_turned:
# feedback.status = 'action achieved'
# else:
# feedback.status = 'action failed'
# if action_result.error_code == 1:
# # Valve is blocked
# element = KeyValue()
# element.key = 'valve_' + params[0] + '_state'
# element.value = "valve_blocked"
# feedback.information.append(element)
if not self.is_valve_blocked[int(params[0])]:
feedback.status = 'action achieved'
else:
feedback.status = 'action failed'
#if action_result.error_code == 1:
# Valve is blocked
element = KeyValue()
element.key = 'valve_' + params[0] + '_state'
element.value = "valve_blocked"
feedback.information.append(element)
self.pub_feedback.publish(feedback)
rospy.loginfo('%s: turn valve action finished', self.name)
# enable valve angle update
self.enable_valve_orientation_srv(EmptyRequest())
else:
feedback = ActionFeedback()
feedback.action_id = action_id
feedback.status = 'action achieved'
self.pub_feedback.publish(feedback)
def cpu_status(hardware, name, cpu):
"""
Decode a cpu stats
Fields:
* min_freq - Minimum frequency in kHz
* max_freq - Maximum frequency in kHz
* frq - Running frequency in kHz
* governor - Governor selected
* val - Status CPU, value between [0, 100]
* model - Model Architecture
* IdleStates
"""
message = 'OFF'
values = []
if cpu:
if 'val' in cpu:
# read value
val = cpu['val']
message = '{val}%'.format(val=val)
# Make Diagnostic Status message with cpu info
values = [
KeyValue("Val", "{val}%".format(val=val)),
KeyValue("Freq", "{frq}".format(frq=cpu['frq'])),
KeyValue("Unit", "khz")
]
if 'governor' in cpu:
values += [
KeyValue("Governor",
"{governor}".format(governor=cpu['governor']))
]
if 'model' in cpu:
values += [
KeyValue("Model", "{model}".format(model=cpu['model']))
]
# Make Diagnostic message
d_cpu = DiagnosticStatus(name='jetson_stats cpu {name}'.format(name=name),
message=message,
hardware_id=hardware,
values=values)
return d_cpu
def check_uptime(load1_threshold, load5_threshold):
level = DiagnosticStatus.OK
vals = []
load_dict = {0: 'OK', 1: 'High Load', 2: 'Very High Load'}
try:
p = subprocess.Popen('uptime',
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
shell=True)
stdout, stderr = p.communicate()
retcode = p.returncode
if retcode != 0:
vals.append(KeyValue(key='uptime Failed', value=stderr))
return DiagnosticStatus.ERROR, vals
upvals = stdout.split()
load1 = upvals[-3].rstrip(',').replace(',', '.')
load5 = upvals[-2].rstrip(',').replace(',', '.')
load15 = upvals[-1].replace(',', '.')
num_users = upvals[-7]
# Give warning if we go over load limit
if float(load1) > load1_threshold or float(load5) > load5_threshold:
level = DiagnosticStatus.WARN
vals.append(KeyValue(key='Load Average Status',
value=load_dict[level]))
vals.append(KeyValue(key='1 min Load Average', value=load1))
vals.append(
KeyValue(key='1 min Load Average Threshold',
value=str(load1_threshold)))
vals.append(KeyValue(key='5 min Load Average', value=load5))
vals.append(
KeyValue(key='5 min Load Average Threshold',
value=str(load5_threshold)))
vals.append(KeyValue(key='15 min Load Average', value=load15))
vals.append(KeyValue(key='Number of Users', value=num_users))
except Exception, e:
rospy.logerr(traceback.format_exc())
level = DiagnosticStatus.ERROR
vals.append(
KeyValue(key='Load Average Status', value=traceback.format_exc()))
def ram_status(hardware, ram, dgtype):
"""
Make a RAM diagnostic status message
Fields:
* use - status ram used
* shared - status of shared memory used from GPU
* tot - Total size RAM
* unit - Unit size RAM, usually in kB
* lfb - Largest Free Block (lfb) is a statistic about the memory allocator
* nblock - Number of block used
* size - Size of the largest free block
* unit - Unit size lfb
"""
lfb_status = ram['lfb']
tot_ram, divider, unit_name = size_min(ram.get('tot', 0),
start=ram.get('unit', 'M'))
# Make ram diagnostic status
d_ram = DiagnosticStatus(
name='jetson_stats {type} ram'.format(type=dgtype),
message=
'{use:2.1f}{unit_ram}B/{tot:2.1f}{unit_ram}B (lfb {nblock}x{size}{unit}B)'
.format(use=ram['use'] / divider,
unit_ram=unit_name,
tot=tot_ram,
nblock=lfb_status['nblock'],
size=lfb_status['size'],
unit=lfb_status['unit']),
hardware_id=hardware,
values=[
KeyValue("Use", "{use}".format(use=ram.get('use', 0))),
KeyValue("Shared", "{shared}".format(shared=ram.get('shared', 0))),
KeyValue("Total", "{tot}".format(tot=ram.get('tot', 0))),
KeyValue("Unit", "{unit}B".format(unit=ram.get('unit', 'M'))),
KeyValue(
"lfb",
"{nblock}x{size}{unit}B".format(nblock=lfb_status['nblock'],
size=lfb_status['size'],
unit=lfb_status['unit']))
])
return d_ram
def sensor_status_to_diag(sensorList, hostname='localhost', ignore_fans=False):
stat = DiagnosticStatus()
stat.name = '%s Sensor Status' % hostname
stat.message = 'OK'
stat.level = DiagnosticStatus.OK
stat.hardware_id = hostname
for sensor in sensorList:
if sensor.getType() == "Temperature":
if sensor.getInput() > sensor.getCrit():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "Critical Temperature"
elif sensor.getInput() > sensor.getHigh():
stat.level = max(stat.level, DiagnosticStatus.WARN)
stat.message = "High Temperature"
stat.values.append(
KeyValue(key=" ".join([sensor.getName(),
sensor.getType()]),
value=str(sensor.getInput())))
elif sensor.getType() == "Voltage":
if sensor.getInput() < sensor.getMin():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "Low Voltage"
elif sensor.getInput() > sensor.getMax():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "High Voltage"
stat.values.append(
KeyValue(key=" ".join([sensor.getName(),
sensor.getType()]),
value=str(sensor.getInput())))
elif sensor.getType() == "Speed":
if not ignore_fans:
if sensor.getInput() < sensor.getMin():
stat.level = max(stat.level, DiagnosticStatus.ERROR)
stat.message = "No Fan Speed"
stat.values.append(
KeyValue(key=" ".join([sensor.getName(),
sensor.getType()]),
value=str(sensor.getInput())))
return stat
def __init__(self, hostname, diag_hostname, home_dir=''):
self._mutex = threading.Lock()
self._no_temp_warn = rospy.get_param('~no_hd_temp_warn', False)
if self._no_temp_warn:
rospy.logwarn(
'Not warning for HD temperatures is deprecated. This will be removed in D-turtle'
)
self._home_dir = home_dir
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
self._last_temp_time = 0
self._last_usage_time = 0
self._last_publish_time = 0
self._temp_timer = None
self._usage_timer = None
self._temp_stat = DiagnosticStatus()
self._temp_stat.name = "%s HD Temperature" % diag_hostname
self._temp_stat.level = DiagnosticStatus.ERROR
self._temp_stat.hardware_id = hostname
self._temp_stat.message = 'No Data'
self._temp_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
if self._home_dir != '':
self._usage_stat = DiagnosticStatus()
self._usage_stat.level = DiagnosticStatus.ERROR
self._usage_stat.hardware_id = hostname
self._usage_stat.name = '%s HD Usage' % diag_hostname
self._usage_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self.check_disk_usage()
def __init__(self, hostname, diag_hostname):
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=100)
self._mutex = threading.Lock()
self._net_level_warn = rospy.get_param('~net_level_warn',
net_level_warn)
self._net_capacity = rospy.get_param('~net_capacity', net_capacity)
self._usage_timer = None
self._usage_stat = DiagnosticStatus()
self._usage_stat.name = 'Network Usage'
self._usage_stat.level = 1
self._usage_stat.hardware_id = hostname
self._usage_stat.message = 'No Data'
self._usage_stat.values = [
KeyValue(key='Update Status', value='No Data'),
KeyValue(key='Time Since Last Update', value='N/A')
]
self._last_usage_time = 0
self._last_publish_time = 0
self.check_usage()
def _battery_update(self, event):
"""
Update battery value on ROSPlan knowledge base
"""
for uav in self.uavs:
# battery status update
self.update_functions(
['fuel'], [[KeyValue('v', uav.namespace)]],
[np.mean(uav.battery_voltages)], [
KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE,
])
self.lowbat[uav.namespace] = uav.low_battery
def create_node_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: Node Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = socket.gethostname()
for r_node in node_list:
# self.node_state[r_node] = rosnode.rosnode_ping(r_node, 1, False)
node_case_kv = KeyValue()
node_case_kv.key = r_node
node_case_kv.value = str(rosnode.rosnode_ping(r_node, 1, False))
diag_status.values.append(node_case_kv)
return diag_status
def get_basic_can_msg(self, name, diag_data):
values = []
for (key, value) in diag_data.items():
if key != 'utime':
values.append(KeyValue(key=key, value=str(round(value, 4))))
msg = DiagnosticArray()
msg.header.stamp = self.get_utime(diag_data)
msg.status.append(
DiagnosticStatus(name=name, level=0, message='OK', values=values))
return (msg.header.stamp, '/diagnostics', msg)
def stats(self):
values = [KeyValue("args", " ".join(self.args))]
# launcher check
message = "inactive"
if self.process is not None:
if self.process.is_alive():
# Update message
message = "running [{pid}]".format(pid=self.process.pid)
# Update list nodes
for item in self.nodes:
if item.value:
status = "active"
# Log status nodes in diagnostic
values += [KeyValue(str(item.value), status)]
# Orchestrator diagnostic
stats = DiagnosticStatus(
name="orchestrator {name}".format(name=self.name),
message=message,
hardware_id=platform.system(),
values=values)
return stats
def do_step(self):
if self.perception_provider.is_submit_agent:
params = [
KeyValue(key="task",
value=self.perception_provider.selected_task.name)
]
rospy.logdebug(self._agent_name + "::" + self._name +
" submiting task " + str(params))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_SUBMIT,
params=params)
def get_kb_update(instances, d):
kua = KnowledgeUpdateServiceArrayRequest()
n = len(instances)
assert (n == len(d) and n == len(d[0]))
for i in xrange(n):
for j in xrange(n):
ki = KnowledgeItem()
ki.knowledge_type = ki.FUNCTION
ki.attribute_name = 'distance'
kv = KeyValue()
kv.key = 'a'
kv.value = instances[i]
ki.values.append(kv)
kv = KeyValue()
kv.key = 'b'
kv.value = instances[j]
ki.values.append(kv)
ki.function_value = d[i][j]
kua.update_type += np.array(kua.ADD_KNOWLEDGE).tostring()
kua.knowledge.append(ki)
return kua
def do_step(self):
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
direction = self.rhbp_agent.local_map.get_direction_to_close_block(
active_subtask.type)
if direction is not None and direction is not False:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name +
"attaching to block in direction " + str(direction))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_ATTACH,
params=params)
def add(self, key, value):
found = False
i = 0
while i < len(self.diagnostic.values) and not found:
if self.diagnostic.values[i].key == key:
self.diagnostic.values[i].value = value
found = True
else:
i = i + 1
if not found:
self.diagnostic.values.append(KeyValue(key, value))
def inject_surge_thruster_fault(self, new_surge_health, index_of_fault):
"""
:param new_surge_health: 17 is 20% health, 85 is 100% health
:param index_of_fault: (fwd-port, fwd-std, lat-rear, lat-front, vert-rear, vert-front)
:return:
"""
fault_type = KeyValue(key='fault_type',value='hard_limit')
print("Injecting Fault of {0} into thruster {1}".format(new_surge_health, index_of_fault))
if index_of_fault == 0:
th_min = KeyValue(key='th_min', value='-{0}, -85, -85, -85, -85, -85'.format(str(new_surge_health)))
th_max = KeyValue(key='th_max', value='{0}, 85, 85, 85, 85, 85'.format(str(new_surge_health)))
elif index_of_fault == 1:
th_min = KeyValue(key='th_min', value='-85, -{0}, -85, -85, -85, -85'.format(str(new_surge_health)))
th_max = KeyValue(key='th_max', value='85, {0}, 85, 85, 85, 85'.format(str(new_surge_health)))
thruster_fault_response = self.srv_fault(request=[fault_type, th_min, th_max])
last_thruster_failure_index = index_of_fault
last_thruster_health = new_surge_health
print("Thruster Health changed to:\n{0}".format(thruster_fault_response))
print(thruster_fault_response.response[-1].value)
def createPlanMsg(self, args):
complete_plan = CompletePlan()
for i, action in enumerate(args):
msg = ActionDispatch()
msg.action_id = i
msg.name = action.pop(0).lower()
#print action
for j, parameter in enumerate(action):
msg.parameters.append(KeyValue(str(j + 1), parameter.lower()))
#pub.publish(msg)
complete_plan.plan.append(msg)
return complete_plan