def load_fact_message(self, attribute, uploaded_instance, item, domain):
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ''
fact_msg.instance_name = ''
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
output_value.value = uploaded_instance
fact_msg.values.append(output_value)
#write to knowledge base
## For insert domain, the attribute container needs to be added as a fact, without second term
if (('in' == attribute) or ('on' == attribute)):
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
output_value.value = self.msg_to_instance_name(item, domain)
fact_msg.values.append(output_value)
##Robocup hack for finals
## Removing instance of objects to be picked form rotation table CB02
if "CB02" == output_value.value or "CB01" == output_value.value:
print "Rotating table knowledge base not updateing for object",uploaded_instance
self.object_to_delete_goals.append(uploaded_instance)
return
if "SH01" == output_value.value or "SH02" == output_value.value:
print "Removing object tot pick from shelf ,", uploaded_instance
self.object_to_delete_goals.append(uploaded_instance)
return
#print (fact_msg)
self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_KNOWLEDGE'])
def publisher():
wifi_publisher = rospy.Publisher("wifi_signal_status", DiagnosticArray, queue_size = 1)
rospy.init_node("Wifi_Publisher_Node")
WifiArray = DiagnosticArray()
WifiArray.header.frame_id = "base_link" #Change Robot Name here
rospy.loginfo("Wifi Signal Publisher Node has started")
while not rospy.is_shutdown():
WifiArray.header.stamp = rospy.get_rostime()
del(WifiArray.status[:])
for Wifinums in WifiList():
keyvalue=KeyValue()
Status = DiagnosticStatus()
del(Status.values[:])
keyvalue.key = "Signal Level"
keyvalue.value = str(Wifinums[2])
Status.values.append(copy.copy(keyvalue))
keyvalue.key = "%"
keyvalue.value = str(Wifinums[3])
Status.values.append(copy.copy(keyvalue))
Status.level = DiagnosticStatus.OK
Status.name = Wifinums[1]
Status.hardware_id = Wifinums[0]
Status.message = "Device Ready"
WifiArray.status.append(copy.copy(Status))
wifi_publisher.publish(WifiArray)
#rospy.loginfo(WifiArray)
rospy.Rate(10).sleep()
def wifi_main():
wifi_pub = rospy.Publisher('wifi_status', DiagnosticArray, queue_size=1)
rospy.init_node('wifi_status')
rate = rospy.Rate(2) # 10hz
diagArrayWiFi = DiagnosticArray()
diagArrayWiFi.header.frame_id = "robot_name"
kv = KeyValue()
rospy.loginfo("wifi_status publisher node started")
while not rospy.is_shutdown():
diagArrayWiFi.header.stamp = rospy.get_rostime()
del (diagArrayWiFi.status[:])
for wifi_status in getWiFiList():
diagStatusWiFi = DiagnosticStatus()
del (diagStatusWiFi.values[:])
diagStatusWiFi.level = DiagnosticStatus.OK
diagStatusWiFi.name = wifi_status[1]
diagStatusWiFi.hardware_id = wifi_status[0]
diagStatusWiFi.message = "Device is up"
kv.key = "siglevel"
kv.value = str(wifi_status[2])
diagStatusWiFi.values.append(copy.copy(kv))
kv.key = "percentage"
kv.value = str(wifi_status[3])
diagStatusWiFi.values.append(copy.copy(kv))
diagArrayWiFi.status.append(copy.copy(diagStatusWiFi))
wifi_pub.publish(diagArrayWiFi)
rate.sleep()
def calibrate_imu(self):
"""
Parse the responses given by the sensor during calibration
:return Status, statusType where status is the accuracy of the game rotation vector/magnetic field output
and statusType is which mode is being reported (mag or grvec)
Accuracy is given as:
0 Unreliable
1 Accuracy Low
2 Accuracy Medium
3 Accuracy High
"""
dat = self.readLine(startChar='G', startChar2='M')
datStr = ''
for num in dat:
datStr += self.toHex(num, padded=True)
datStr = datStr.decode('hex')
idx = datStr.find(':')
ret = KeyValue()
try:
status = dat[idx + 1]
ret.key = datStr[:idx]
ret.value = str(status - 48)
except:
ret.key = ''
ret.value = '-1'
self.imu_cal_pub.publish(ret)
def _update_diagnostics_state(self):
md5_warnings = {}
ttype_warnings = {}
for mname, mth in self.masters.items():
warnings = mth.get_md5warnigs()
if warnings:
md5_warnings[mname] = warnings
twarnings = mth.get_topic_type_warnings()
if twarnings:
ttype_warnings[mname] = twarnings
level = 0
if md5_warnings or ttype_warnings:
level = 1
if self._current_diagnistic_level != level:
da = DiagnosticArray()
if md5_warnings:
# add warnings for all hosts with topic types with different md5sum
for mname, warnings in md5_warnings.items():
diag_state = DiagnosticStatus()
diag_state.level = level
diag_state.name = rospy.get_name()
diag_state.message = "Wrong topics md5sum @%s" % mname
diag_state.hardware_id = mname
for (topicname, _node,
_nodeuri), ttype in warnings.items():
key = KeyValue()
key.key = topicname
key.value = ttype
diag_state.values.append(key)
da.status.append(diag_state)
elif ttype_warnings:
# add warnings if a topic with different type is synchrinozied to local host
for mname, warnings in ttype_warnings.items():
diag_state = DiagnosticStatus()
diag_state.level = level
diag_state.name = rospy.get_name()
diag_state.message = "Wrong topics type @%s" % mname
diag_state.hardware_id = mname
for (topicname, _node,
_nodeuri), ttype in warnings.items():
key = KeyValue()
key.key = topicname
key.value = ttype
diag_state.values.append(key)
da.status.append(diag_state)
else:
# clear all warnings
diag_state = DiagnosticStatus()
diag_state.level = 0
diag_state.name = rospy.get_name()
diag_state.message = ""
diag_state.hardware_id = ""
da.status.append(diag_state)
da.header.stamp = rospy.Time.now()
self.pub_diag.publish(da)
self._current_diagnistic_level = level
def fill_order(self, order, attribute, first, second, count=1, container_name=None):
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ""
fact_msg.instance_name = ""
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
if 5 == order.destination.type.data:
fact_msg.attribute_name = "in"
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[
0
]
name_of_object = self.msg_to_instance_name(order, first, count)
output_value.value = name_of_object
##Robocup finals hack not adding goals for object to be picked from rotation table
if output_value.value in self.object_to_delete_goals:
print "Not adding goal for ", output_value.value
return
# if 5 == order.object.type.data or 12 == order.object.type.data:
# print "Not adding M20_100 and distance tube goal"
# return
fact_msg.values.append(output_value)
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[
1
]
if not container_name:
# in order the container or location doesnt have the count
output_value.value = self.msg_to_instance_name(order, second)
else:
output_value.value = container_name
# if 'SH01' == order.destination.type.data or 'SH02' == order.destination.type.data:
# print "not adding goal for shelfes"
# return
## For PP01 adding insert
if 5 == order.destination.type.data:
# Hack for PPT for Nagoya
#################
# print "Not adding Goals for PP01"
# return
#################
output_value.value = "PP01_CAVITY"
fact_msg.values.append(output_value)
self.write_to_knowledge_base(
fact_msg, self.knowledge_update_service["ADD_GOAL"]
)
return name_of_object
def __execute_check_panel__(self, action_id):
# Publish action enabled
feedback = ActionFeedback()
feedback.action_id = action_id
feedback.status = 'action enabled'
self.pub_feedback.publish(feedback)
rospy.loginfo('%s: check panel action enabled', self.name)
# forget old panel location
self.ekf_panel_centre = None
rospy.sleep(2.0)
element = KeyValue()
element.key = 'panel_0_in_fov'
print 'current time: ', rospy.Time.now().to_sec()
print 'last time we see the panel: ', self.last_panel_update
rospy.sleep(8)
if rospy.Time.now().to_sec() - self.last_panel_update < 2.0:
element.value = 'true'
else:
element.value = 'false'
rospy.loginfo('%s: Panel out of field of view.', self.name)
ret = list()
rospy.wait_for_service('/cola2_control/goto_holonomic', 20)
ret.append(element)
# Publish action response
if self.ekf_panel_centre != None:
element1 = KeyValue()
element1.key = 'panel_0_position'
position = [
self.ekf_panel_centre.position.x,
self.ekf_panel_centre.position.y,
self.ekf_panel_centre.position.z,
self.ekf_panel_centre.orientation.x,
self.ekf_panel_centre.orientation.y,
self.ekf_panel_centre.orientation.z,
self.ekf_panel_centre.orientation.w
]
element1.value = str(position)
ret.append(element1)
feedback.status = 'action achieved'
feedback.information = ret
rospy.loginfo('%s: check panel response: \n%s', self.name, feedback)
self.pub_feedback.publish(feedback)
def update_and_publish_skill_arg(self):
self.robot_cooking.update_skill_arg()
skill_arg_dict_array = DictArray()
skill_arg_dict_processed = {} # transform np.ndarray to list
for k, v in viewitems(self.robot_cooking.skill_arg):
kv = KeyValue()
kv.key = k
if isinstance(v, np.ndarray):
v_processed = list(v)
elif isinstance(v, dict):
v_processed = {}
for k_, v_ in viewitems(v):
if isinstance(v_, np.ndarray):
v_ = list(v_)
v_processed[k_] = v_
else:
v_processed = v
kv.value = str(v_processed)
skill_arg_dict_array.data.append(kv)
skill_arg_dict_processed[k] = v_processed
if self.zmq_pub is not None and self.RunRobotNode_config[
'msg_system'] == 'zmq':
self.zmq_pub.send_json((self.robot, skill_arg_dict_processed))
self.skill_arg_pub.publish(skill_arg_dict_array)
def spin(self):
r = rospy.Rate(self._diagnostic_freq)
while not rospy.is_shutdown():
diag = DiagnosticArray()
diag.header.stamp = rospy.get_rostime()
for mon in self._monitors:
d = DiagnosticStatus()
d.name=mon.get_name()
d.hardware_id="PC0"
d.message=mon.get_field_value("status")
d.level=mon.get_field_value("status_level")
for key in mon.get_fields():
p = KeyValue()
p.key = key
p.value = str(mon.get_field_value(key))
d.values.append(p)
diag.status.append(d)
self._diagnostic_pub.publish(diag)
r.sleep()
self._cpu_temp_mon.stop()
self._cpu_usage_mon.stop()
self._wlan_mon.stop()
self._bandwidth_mon.stop()
def diagnostic_status(cls, msg, obj):
for i in range(msg['_length']):
kv = KeyValue()
kv.key = msg['%s_key' % i]
kv.value = msg['%s_value' % i]
obj.values.append(kv)
return(obj)
def __set_params__(param_list):
ret = list()
for key in param_list:
element = KeyValue()
element.key = key
element.value = param_list[key]
ret.append(element)
return ret
def update_kb(self):
kus = KnowledgeUpdateServiceArrayRequest()
# Get info from KB functions and propositions (to know type of variable)
# Fill update
self.dump_cache_mutex.acquire(True)
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,
updated) in sensed_predicates.iteritems():
if updated:
continue
if predicate in self.sensed_topics:
self.mutex.acquire(True)
self.sensed_topics[predicate] = (
self.sensed_topics[predicate][0], False, True
) # Set to not changed and updated KB
self.mutex.release()
else:
self.srv_mutex.acquire(True)
self.sensed_services[predicate] = (
self.sensed_services[predicate][0], False, True
) # Set to not changed and updated KB
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)
self.dump_cache_mutex.release()
# 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 wait_for_sensing():
rospy.loginfo("KCL: (%s) Triggering the visibility update" %
rospy.get_name())
vispub.publish("update_1")
vispub.publish("update_2")
# wait for the sensing interface to catch up
rospy.loginfo("KCL: (%s) Waiting for visibility to be added to KB" %
rospy.get_name())
propcount = 0
start = time.time()
while propcount < 1 and (time.time() - start < 20):
vis = rospy.ServiceProxy('/rosplan_knowledge_base/state/propositions',
GetAttributeService)
vis_response = vis("doughnut_visible_from")
if not vis_response:
rospy.logerr("KCL: (%s) Failed to call the KB" % rospy.get_name())
quit()
propcount = len(vis_response.attributes)
rospy.sleep(0.5)
wp_id = "NO_WAYPOINT"
inst = rospy.ServiceProxy('/rosplan_knowledge_base/state/instances',
GetInstanceService)
inst_response = inst("waypoint")
if len(inst_response.instances):
wp_id = inst_response.instances[0]
kus = KnowledgeUpdateServiceRequest()
kus.knowledge.knowledge_type = 1
kus.knowledge.attribute_name = 'robot_at'
kv = KeyValue()
kv.key = 'r'
kv.value = 'kenny'
kus.knowledge.values.append(kv)
kv = KeyValue()
kv.key = 'wp'
kv.value = wp_id
kus.knowledge.values.append(kv)
kuc = rospy.ServiceProxy('/rosplan_knowledge_base/update',
KnowledgeUpdateService)
if not kuc(kus):
rospy.logerr("KCL: (%s) Robot at was not added!" % rospy.get_name())
def __execute_valve_state__(self, action_id):
# Publish action enabled
feedback = ActionFeedback()
feedback.action_id = action_id
feedback.status = 'action enabled'
self.pub_feedback.publish(feedback)
rospy.loginfo('%s: valve state action enabled', self.name)
#rospy.sleep(10.0)
rospy.sleep(3.0)
if rospy.Time.now().to_sec() - self.last_panel_update < 8.0:
rospy.loginfo('%s: We are looking at the panel right now!',
self.name)
# Publish action response
ret = list()
for i in range(4):
if self.valve_covariance[i] > 0 and self.valve_covariance[
i] < 1.0:
element = KeyValue()
element.key = 'valve_' + str(i) + '_angle'
element.value = str(self.valve_orientation[i])
ret.append(element)
# If we have more than one panel we indicated the panel id (now is always 0)
element1 = KeyValue()
element1.key = 'valve_' + str(i) + '_in_panel'
element1.value = '0'
ret.append(element1)
feedback.status = 'action achieved'
feedback.information = ret
rospy.loginfo('%s: valve state response: \n%s', self.name,
feedback)
self.pub_feedback.publish(feedback)
else:
rospy.loginfo('%s: Panel out of field of view.', self.name)
feedback.status = 'action failed'
element = KeyValue()
element.key = 'panel_0_state'
element.value = 'panel_missing'
feedback.information.append(element)
self.pub_feedback.publish(feedback)
def changeKMSFact(self, name, paramsValues, changeType):
predicateDetails = self.predicate_details_client.call(name)
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param, paramValue in zip(predicateDetails.predicate.typed_parameters, paramsValues):
pair = KeyValue()
pair.key = param.key
pair.value = paramValue
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
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 diagnostic_array(cls, msg, obj):
obj.header = interpret.Decode().header(msg, obj.header, "")
for i in range(msg['_length_s']):
ds = DiagnosticStatus()
for j in range(msg['%s_length_v' % i]):
kv = KeyValue()
kv.key = msg['%s_%s_key' % (i, j)]
kv.value = msg['%s_%s_value' % (i, j)]
ds.values.append(kv)
obj.status.append(ds)
return(obj)
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 changeKMSFact(self, name, paramsValues, changeType):
predicateDetails = self.predicate_details_client.call(name)
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param, paramValue in zip(
predicateDetails.predicate.typed_parameters, paramsValues):
pair = KeyValue()
pair.key = param.key
pair.value = paramValue
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
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 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 create_nuc_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: NUC Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = socket.gethostname()
temperature_case_kv = KeyValue()
temperature_case_kv.key = 'temperature_case'
temperature_case_kv.value = str(self.temperature_case)
diag_status.values.append(temperature_case_kv)
disk_usage_kv = KeyValue()
self.get_disk_usage()
disk_usage_kv.key = 'disk_usage'
disk_usage_kv.value = str(self.disk_usage)
diag_status.values.append(disk_usage_kv)
is_recording_kv = KeyValue()
is_recording_kv.key = 'is_recording'
is_recording_kv.value = str(self.is_recording)
diag_status.values.append(is_recording_kv)
return diag_status
def fill_order(self, order, attribute, first, second, count=-1):
#In transportationtask sometimes the destination is not there in inventory
#Needs to add fact about the inventory
if 'destination' == second:
name = self.load_instance_message(KnowledgeItem.INSTANCE, 'destination', order)
fact_msg = KnowledgeItem()
fact_msg.knowledge_type = KnowledgeItem.FACT
fact_msg.instance_type = ''
fact_msg.instance_name = ''
fact_msg.attribute_name = attribute
fact_msg.function_value = 0.0
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[0]
output_value.value = self.msg_to_instance_name(order, first, count)
fact_msg.values.append(output_value)
output_value = KeyValue()
output_value.key = (self._predicate_param_label_list[fact_msg.attribute_name])[1]
#in order the container or location doesnt have the count
output_value.value = self.msg_to_instance_name(order, second )
fact_msg.values.append(output_value)
self.write_to_knowledge_base(fact_msg, self.knowledge_update_service['ADD_GOAL'] )
def change_controller_state(self, state_new_name): new_state = KeyValue() if state_new_name in ControllerState.MAP: new_state.key = state_new_name else: new_state.key = 'Unknown' new_state.value = str( ControllerState.MAP.index( new_state.key ) ) self.publisher['controller_state'].publish(new_state) print "Controller New State is:", new_state.key
def publish_predicates(self, preds):
ka = []
for p in preds:
k = KnowledgeItem()
k.knowledge_type = k.FACT
k.attribute_name = p.head.lower()
for i in xrange(len(p.args)):
kv = KeyValue()
kv.key = self.predicate_key_name[(k.attribute_name, i)]
kv.value = p.args[i].lower()
k.values.append(kv)
ka.append(k)
k.is_negative = not p.isTrue
self.publish(ka)
def publish_predicates(self, preds): ka = [] for p in preds: k = KnowledgeItem() k.knowledge_type = k.FACT k.attribute_name = p.head.lower() for i in xrange(len(p.args)): kv = KeyValue() kv.key = self.predicate_key_name[(k.attribute_name, i)] kv.value = p.args[i].lower() k.values.append(kv) ka.append(k) k.is_negative = not p.isTrue self.publish(ka)
def changeKMSFact(self, name, params, changeType):
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param in params:
pair = KeyValue()
pair.key = param[0]
pair.value = param[1]
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed updating KMS with attribute: %s", knowledge.attribute_name)
else:
rospy.loginfo("Updated KMS with attribute: %s", knowledge.attribute_name)
def _handle_dispatch_clicked(self, checked):
pub = rospy.Publisher('/kcl_rosplan/action_dispatch', ActionDispatch, queue_size=10)
self.statusLabel.setText("")
msg = ActionDispatch()
msg.name = self.operatorNameComboBox.currentText()
root = self.operatorView.invisibleRootItem()
child_count = root.childCount()
for i in range(child_count):
item = root.child(i)
cmb = self.operatorView.itemWidget(item, 2)
kv = KeyValue()
kv.key = item.text(0)
kv.value = cmb.currentText()
msg.parameters.append(kv)
pub.publish(msg)
def _handle_dispatch_clicked(self, checked):
pub = rospy.Publisher('/kcl_rosplan/action_dispatch', ActionDispatch, queue_size=10)
self.statusLabel.setText("")
msg = ActionDispatch()
msg.name = self.operatorNameComboBox.currentText()
root = self.operatorView.invisibleRootItem()
child_count = root.childCount()
for i in range(child_count):
item = root.child(i)
cmb = self.operatorView.itemWidget(item, 2)
kv = KeyValue()
kv.key = item.text(0)
kv.value = cmb.currentText()
msg.parameters.append(kv)
pub.publish(msg)
def ground_simple_effect(self, effect, bound_parameters):
# TODO fetch predicate labels
# bind objects from action message to effect predicate
item = KnowledgeItem()
item.knowledge_type = KnowledgeItem.FACT
item.attribute_name = effect.name
for j in range(0, len(effect.typed_parameters)):
# set key (parameter label in predicate) and value (object bound to operator)
pair = KeyValue()
pair.key = "TODO THE LABEL" # self.predicates[self.op.at_start_del_effects[i].name].typed_parameters[j].key
pair.value = bound_parameters[effect.typed_parameters[j].key]
item.values.append(pair)
return item
def addFact(name, params, client):
add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param in params:
pair = KeyValue()
pair.key = param[0]
pair.value = param[1]
knowledge.values.append(pair)
update_response = client(add_knowledge_type, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed updating KMS with attribute: %s", knowledge.attribute_name)
else:
rospy.loginfo("Updated KMS with attribute: %s", knowledge.attribute_name)
def changeKMSFact(self, name, params, changeType):
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = name
for param in params:
pair = KeyValue()
pair.key = param[0]
pair.value = param[1]
knowledge.values.append(pair)
update_response = self.update_knowledge_client(changeType, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed updating KMS with attribute: %s",
knowledge.attribute_name)
else:
rospy.loginfo("Updated KMS with attribute: %s",
knowledge.attribute_name)
def add(self, key, val):
"""
Add a key-value pair.
This method adds a key-value pair. Any type that has a << stream
operator can be passed as the second argument. Formatting is done
using a std::stringstream.
@type key string
@param key Key to be added.
@type value string
@param value Value to be added.
"""
key_ = KeyValue()
key_.key = key
key_.value = val
self.values.append(key_)
def generate_hand_state(self):
"""
Generate the state of the hand based on the communication state and the number of sensors connected
:return:
"""
self.hand_state.status_codes = []
self.hand_state.hand_id = str(self.phand.hand_id)
self.hand_state.state = self.phand.com_state
self.hand_state.connected_sensor_ids = self.phand.connected_sensor_ids
self.hand_state.connected_sensor_names = self.phand.connected_sensor_names
for key_value in self.phand.status_codes:
state = KeyValue()
state.key = key_value[0]
state.value = key_value[1]
self.hand_state.status_codes.append(state)
def ros_msg(grpcmsg):
try:
result = DiagnosticArray()
result.header.stamp = rospy.Time(grpcmsg.timestamp)
for stat in grpcmsg.status:
ds = DiagnosticStatus()
ds.level = stat.level
ds.name = stat.name
ds.message = stat.message
ds.hardware_id = stat.hardware_id
for val in stat.values:
dv = KeyValue()
dv.key = val.key
dv.value = val.value
ds.values.append(dv)
result.status.append(ds)
return result
except Exception as _err:
import traceback
raise Exception(traceback.format_exc())
def createGoal():
rosplan_pub = rospy.Publisher("/kcl_rosplan/planning_commands", String, queue_size=10)
speech_pub = rospy.Publisher("/KomodoSpeech/rec_command", KomodoSpeechRecCommand, queue_size=10)
command = KomodoSpeechRecCommand()
command.header = Header()
command.cmd = 'start'
command.cat = 'cmd'
rospy.sleep(2.)
speech_pub.publish(command)
result = rospy.wait_for_message("/KomodoSpeech/rec_result", KomodoSpeechRecResult, timeout=30)
if result and result.cat == "cmd" and result.success is True and result.phrase_id == 8:
attribute_query_client = rospy.ServiceProxy("/kcl_rosplan/get_current_knowledge", GetAttributeService)
knowledge_items = attribute_query_client.call("robot_at")
if len(knowledge_items.attributes) > 1:
rospy.loginfo("Failed to add goal. Robot in two places (robot_at)")
return
current_location = knowledge_items.attributes[0].values[0].value
update_knowledge_client = rospy.ServiceProxy("/kcl_rosplan/update_knowledge_base", KnowledgeUpdateService)
add_knowledge_type = KnowledgeUpdateServiceRequest.ADD_GOAL
knowledge = KnowledgeItem()
knowledge.knowledge_type = KnowledgeItem.FACT
knowledge.attribute_name = "coke_at"
pair = KeyValue()
pair.key = "loc"
pair.value = current_location #room_2
knowledge.values.append(pair)
update_response = update_knowledge_client(add_knowledge_type, knowledge)
if (update_response.success is not True):
rospy.loginfo("Failed to add goal of attribute: %s", knowledge.attribute_name)
else:
rospy.loginfo("Added goal of attribute: %s", knowledge.attribute_name)
rosplan_pub.publish(String("plan"))
else:
rospy.loginfo("Couldn't get command. Closing")
def create_fcu_status(self):
diag_status = DiagnosticStatus()
diag_status.name = "BHG: FCU Status"
diag_status.message = "BHG: Normal"
diag_status.hardware_id = "Pixhawk"
gps_fix_type_kv = KeyValue()
gps_fix_type_kv.key = 'gps_fix_type'
gps_fix_type_kv.value = self.fixtype_convert(self.gps_fix_type)
diag_status.values.append(gps_fix_type_kv)
gps_num_sv_kv = KeyValue()
gps_num_sv_kv.key = 'gps_num_sv'
gps_num_sv_kv.value = str(self.gps_num_sv)
diag_status.values.append(gps_num_sv_kv)
fcu_mode_kv = KeyValue()
fcu_mode_kv.key = 'fcu_mode'
fcu_mode_kv.value = str(self.fcu_mode)
diag_status.values.append(fcu_mode_kv)
fcu_voltage_kv = KeyValue()
fcu_voltage_kv.key = 'fcu_voltage'
fcu_voltage_kv.value = str(self.fcu_voltage)
diag_status.values.append(fcu_voltage_kv)
fcu_amps_kv = KeyValue()
fcu_amps_kv.key = 'fcu_amps'
fcu_amps_kv.value = str(self.fcu_amps)
diag_status.values.append(fcu_amps_kv)
fcu_cpu_load_kv = KeyValue()
fcu_cpu_load_kv.key = 'fcu_cpu_load'
fcu_cpu_load_kv.value = str(self.fcu_cpu_load)
diag_status.values.append(fcu_cpu_load_kv)
return diag_status
def publish_diagnostics_info(self):
"""
Publishes at a rate of 1Hz ( because thats the rate real kobuki does it )
For this first version we only publish battery status.
:return:
"""
battery_charge = self.get_simulated_battery_status()
msg = DiagnosticArray()
info_msg = DiagnosticStatus()
header = Header()
header.frame_id = ""
header.stamp = rospy.Time.now()
msg.header = header
msg.status.append(info_msg)
values = []
percent_value = KeyValue()
percent_value.key = "Percent"
percent_value.value = str(battery_charge)
# TODO: Add all the other values
voltage_value = KeyValue()
voltage_value.key = "Voltage (V)"
charge_value = KeyValue()
charge_value.key = "Charge (Ah)"
capacity_value = KeyValue()
capacity_value.key = "Capacity (Ah)"
source_value = KeyValue()
source_value.key = "Source"
charging_state_value = KeyValue()
charging_state_value.key = "Charging State"
current_value = KeyValue()
current_value.key = "Current (A)"
if battery_charge <= 10.0:
level = DiagnosticStatus.ERROR
message = "Empty Battery"
elif 10.0 < battery_charge <= 20.0:
level = DiagnosticStatus.WARN
message = "Warning Low Battery"
elif battery_charge >= 100.0:
level = DiagnosticStatus.OK
message = "Maximum"
else:
level = DiagnosticStatus.OK
message = "NormalLevel"
info_msg.name = "mobile_base_nodelet_manager: Battery"
info_msg.hardware_id = "Kobuki"
info_msg.message = message
info_msg.level = level
values.append(percent_value)
values.append(voltage_value)
values.append(charge_value)
values.append(capacity_value)
values.append(source_value)
values.append(charging_state_value)
values.append(current_value)
info_msg.values = values
msg.status.append(info_msg)
self._pub.publish(msg)
rospy.get_name())
count = 0
for k in facts.attributes:
if not k.is_negative:
count = count + 1
# add goals to the KB
for i in range(6):
wp_goal = random.randint(0, max_prm_size - 1)
kus = KnowledgeUpdateServiceRequest()
kus.update_type = 1
kus.knowledge.knowledge_type = 1
kus.knowledge.attribute_name = 'visited'
kv = KeyValue()
kv.key = 'wp'
kv.value = 'wp' + str(wp_goal)
kus.knowledge.values.append(kv)
kuc = rospy.ServiceProxy('/rosplan_knowledge_base/update',
KnowledgeUpdateService)
if not kuc(kus):
rospy.logerr("KCL: (%s) Goal was not added!" % rospy.get_name())
plan_found = generate_problem_and_plan()
rospy.sleep(1)
if plan_found:
execute_plan()
except rospy.ServiceException, e:
rospy.logerr("KCL: (%s) Service call failed: %s" % (rospy.get_name(), e))
def hwboard(self):
# initialize local variables
send_channel = 0
read_channel = 0
send_specifier = 0
read_specifier = 0
read_status = 0
read_data = 0
read_id = 0
read_crc = 0
# init ros-node
pub = rospy.Publisher('diagnostics',DiagnosticArray)
rospy.init_node('hwboard')
while not rospy.is_shutdown():
# init publisher message
pub_message = DiagnosticArray()
# init array for storing data
status_array = []
# init local variable for error detection
error_while_reading = 0
for send_specifier in range(0,7,3):
if send_specifier == 0:
count_range = range(6)
elif send_specifier == 3:
count_range = [0,1,2,3,6,7]
else:
count_range = [1,2,3,6,7]
for send_channel in count_range:
# init message and local variables
send_buff_array = [send_channel,send_specifier,0x00,0x00,0x00]
message = ""
preamble_bytes = 4
preamble_error = 1
crc_error = 1
retry = 0
# calculate crc
crc = 0x00
for i in range(4):
data = send_buff_array[i]
for k in range(8):
feedback_bit = (crc^data) & 0x80
feedback_bit = (feedback_bit>>7) & 0xFF
if feedback_bit == 1:
crc = (crc<<1) & 0xFF
crc = crc^0x31
else:
crc = (crc<<1) & 0xFF
data = (data<<1) & 0xFF
send_buff_array[4] = crc
# send message
while (preamble_error == 1 or crc_error == 1) and retry < 8:
message= ""
for i in range(preamble_bytes):
message += chr(0x55)
for i in send_buff_array:
message += chr(i)
self.s.write(message)
# receive message
# check for first preamble byte of reveiced message
read_buff_array = []
buff = self.s.read(1)
preamble_count = 0
for i in buff:
read_buff_array.append(ord(i))
if read_buff_array[0] == 0x55:
# check for following preamble bytes
while read_buff_array[0] == 0x55 and preamble_count < 10:
read_buff_array = []
buff = self.s.read(1)
for i in buff:
read_buff_array.append(ord(i))
preamble_count = preamble_count + 1
buff = self.s.read(13)
# check preamble length
if preamble_count > 6:
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
elif preamble_count < 2:
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
else:
# preamble ok. evaluate message
preamble_error = 0
# get remaining message
for i in buff:
read_buff_array.append(ord(i))
#check crc
crc = 0x00
for i in range(14):
data = read_buff_array[i]
for k in range(8):
feedback_bit = (crc^data) & 0x80
feedback_bit = (feedback_bit>>7) & 0xFF
if feedback_bit == 1:
crc = (crc<<1) & 0xFF
crc = crc^0x31
else:
crc = (crc<<1) & 0xFF
data = (data<<1) & 0xFF
if crc != 0:
crc_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
else:
crc_error = 0
# no preamble detected
else:
buff = s.read(14)
preamble_error = 1
preamble_bytes = preamble_bytes + 1
retry = retry + 1
if preamble_bytes == 7:
preamble_bytes = 2
# get channel byte
read_channel = int(read_buff_array[0])
# get specifier byte
read_specifier = int(read_buff_array[1])
# get status byte
read_status = int(read_buff_array[2])
# get data bytes
read_data = 256 * int(read_buff_array[3])
read_data = read_data + int(read_buff_array[4])
# get id bytes
read_id = read_buff_array[5]<<8
read_id = (read_id | read_buff_array[6])<<8
read_id = (read_id | read_buff_array[7])<<8
read_id = read_id | read_buff_array[8]
# evaluate recieved message
if read_channel == send_channel:
if read_specifier == send_specifier:
if read_status == 0 or read_status == 8:
if send_specifier == 0:
read_data = read_data / 10.0
else:
read_data = read_data / 1000.0
erro_while_reading = 0
else:
read_data = 0
error_while_reading = 1
else:
read_data = 0
error_while_reading = 1
else:
read_data = 0
error_while_reading = 1
#prepare status object for publishing
# init sensor object
status_object = DiagnosticStatus()
# init local variable for data
key_value = KeyValue()
# set values for temperature parameters
if send_specifier == 0:
if read_data == 85:
level = 1
status_object.message = "sensor damaged"
elif read_data > 50:
level = 2
status_object.message = "temperature critical"
elif read_data >40:
level = 1
status_object.message = "temperature high"
elif read_data > 10:
level = 0
status_object.message = "temperature ok"
elif read_data > -1:
level = 1
status_object.message = "temperature low"
else:
level = 2
status_object.message = "temperature critical"
# mapping for temperature sensors
if read_id == self.head_sensor_param:
status_object.name = "Head Temperature"
status_object.hardware_id = "hcboard_channel " + str(send_channel)
elif read_id == self.eye_sensor_param:
status_object.name = "Eye Camera Temperature"
status_object.hardware_id = "hcboard_channel = " + str(send_channel)
elif read_id == self.torso_module_sensor_param:
status_object.name = "Torso Module Temperature"
status_object.hardware_id = "hcboard_channel =" + str(send_channel)
elif read_id == self.torso_sensor_param:
status_object.name = "Torso Temperature"
status_object.hardware_id = "hcboard_channel =" + str(send_channel)
elif read_id == self.pc_sensor_param:
status_object.name = "PC Temperature"
status_object.hardware_id = "hcboard_channel =" + str(send_channel)
elif read_id == self.engine_sensor_param:
status_object.name = "Engine Temperature"
status_object.hardware_id = "hcboard_channel = " + str(send_channel)
else:
level = 1
status_object.message = "cannot map if from yaml file to temperature sensor"
# set values for voltage parameters
elif send_specifier == 3:
if send_channel == 0:
if read_data > 58:
level = 2
status_object.message = "voltage critical"
elif read_data > 56:
level = 1
status_object.message = "voltage high"
elif read_data > 44:
level = 0
status_object.message = "voltage ok"
elif read_data > 42:
level = 1
status_object.message = "voltage low"
else:
level = 2
status_object.message = "voltage critical"
else:
if read_data > 27:
level = 2
status_object.message = "voltage critical"
elif read_data > 25:
level = 1
status_object.message = "voltage_high"
elif read_data > 23:
level = 0
status_object.message = "voltage ok"
elif read_data > 19:
level = 1
status_object.message = "voltage low"
else:
level = 2
status_object.message = "voltage critical"
if send_channel == 0:
status_object.name = "Akku Voltage"
status_object.hardware_id = "hcboard_channel = 0"
elif send_channel == 1:
status_object.name = "Torso Engine Voltage"
status_object.hardware_id = "hcboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Voltage"
status_object.hardware_id = "hcboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Voltage"
status_object.hardware_id = "hcboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Voltage"
status_object.hardware_id = "hcboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Voltage"
status_object.hardware_id = "hcboard_channel = 7"
# set values for current parameters
else:
if read_data > 15:
level = 2
status_object.message = "current critical"
elif read_data > 10:
level = 1
status_object.message = "current high"
elif read_data < 0:
level = 2
status_object.message = "current critical"
else:
level = 0
status_object.message = "current ok"
if send_channel == 1:
status_object.name = "Torso Engine Current"
status_object.hardware_id = "hcboard_channel = 1"
elif send_channel == 2:
status_object.name = "Torso Logic Current"
status_object.hardware_id = "hcboard_channel = 2"
elif send_channel == 3:
status_object.name = "Tray Logic Current"
status_object.hardware_id = "hcboard_channel = 3"
elif send_channel == 6:
status_object.name = "Arm Engine Current"
status_object.hardware_id = "hcboard_channel = 6"
elif send_channel == 7:
status_object.name = "Tray Engine Current"
status_object.hardware_id = "hcboard_channel = 7"
# evaluate error detection
if error_while_reading == 1:
level = 1
status_object.message = "detected error while receiving answer from hardware control board"
# append status object to publishing message
status_object.level = level
key_value.value = str(read_data)
status_object.values.append(key_value)
pub_message.status.append(status_object)
# publish message
pub.publish(pub_message)
rospy.sleep(1.0)
def apply_effects_to_KMS(self, block_name, other_block_name, action_name):
fname = "{}::{}".format(self.__class__.__name__, self.apply_effects_to_KMS.__name__)
emptyhand_update_type = KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE
not_emptyhand_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE
onblock_update_type = KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE
clear_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE
inhand_update_type = KnowledgeUpdateServiceRequest.ADD_KNOWLEDGE if \
(action_name == "pick_up") else KnowledgeUpdateServiceRequest.REMOVE_KNOWLEDGE
# not_emptyhand predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "not_emptyhand"
update_response = self.update_knowledge_client(not_emptyhand_update_type, updated_knowledge)
rospy.logdebug(
"{}: Updated KMS with {} {}".format(fname, updated_knowledge.attribute_name, not_emptyhand_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, not_emptyhand_update_type, "not_emptyhand"))
# emptyhand predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "emptyhand"
update_response = self.update_knowledge_client(emptyhand_update_type, updated_knowledge)
rospy.logdebug(
"{}: Updated KMS with {} {}".format(fname, updated_knowledge.attribute_name, emptyhand_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, emptyhand_update_type, "emptyhand"))
# (on ?block ?from_block) predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "on"
pair = KeyValue()
pair.key = "block"
pair.value = block_name
updated_knowledge.values.append(pair)
pair = KeyValue()
pair.key = "on_block"
pair.value = other_block_name
updated_knowledge.values.append(pair)
update_response = self.update_knowledge_client(onblock_update_type, updated_knowledge)
rospy.logdebug("{}: Updated KMS with {} ({}, {}) {}".
format(fname,
updated_knowledge.attribute_name,
updated_knowledge.values[0].value,
updated_knowledge.values[1].value,
onblock_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, onblock_update_type, "on"))
# (clear ?from_block) predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "clear"
pair = KeyValue()
pair.key = "block"
pair.value = other_block_name
updated_knowledge.values.append(pair)
update_response = self.update_knowledge_client(clear_update_type, updated_knowledge)
rospy.logdebug("{}: Updated KMS with {} ({}) {}".
format(fname,
updated_knowledge.attribute_name,
updated_knowledge.values[0].value,
clear_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, clear_update_type, "clear"))
# (inhand ?block) predicate
updated_knowledge = KnowledgeItem()
updated_knowledge.knowledge_type = KnowledgeItem.FACT
updated_knowledge.attribute_name = "inhand"
pair = KeyValue()
pair.key = "block"
pair.value = block_name
updated_knowledge.values.append(pair)
update_response = self.update_knowledge_client(inhand_update_type, updated_knowledge)
rospy.logdebug("{}: Updated KMS with {} ({}) {}".
format(fname,
updated_knowledge.attribute_name,
updated_knowledge.values[0].value,
inhand_update_type))
if (update_response.success is not True):
rospy.logerr("{}: Could not update KMS with action effect ({} {})".
format(fname, inhand_update_type, "inhand"))
def sendKeyVal(pub, key, val):
kval = KeyValue()
kval.key = key
kval.value = val
pub.publish(kval)
def initialize_origin():
rospy.init_node('initialize_origin', anonymous=True)
global _origin_pub
global _local_xy_frame
_origin_pub = rospy.Publisher('/local_xy_origin',
GPSFix,
latch=True,
queue_size=2)
diagnostic_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=2)
local_xy_origin = rospy.get_param('~local_xy_origin', 'auto')
_local_xy_frame = rospy.get_param('~local_xy_frame', 'map')
_local_xy_frame_identity = rospy.get_param('~local_xy_frame_identity',
_local_xy_frame + "__identity")
if local_xy_origin == "auto":
global _sub
_sub = rospy.Subscriber("gps", GPSFix, gps_callback)
else:
parse_origin(local_xy_origin)
if len(_local_xy_frame):
tf_broadcaster = tf.TransformBroadcaster()
else:
tf_broadcaster = None
hw_id = rospy.get_param('~hw_id', 'none')
while not rospy.is_shutdown():
if tf_broadcaster:
# Publish transform involving map (to an anonymous unused
# frame) so that TransformManager can support /tf<->/wgs84
# conversions without requiring additional nodes.
tf_broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1),
rospy.Time.now(),
_local_xy_frame_identity,
_local_xy_frame)
if _gps_fix == None:
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
else:
_origin_pub.publish(
_gps_fix) # Publish this at 1Hz for bag convenience
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
if local_xy_origin == 'auto':
status.level = DiagnosticStatus.OK
status.message = "Has Origin (auto)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin is static (non-auto)"
value0 = KeyValue()
value0.key = "Origin"
value0.value = _gps_fix.status.header.frame_id
status.values.append(value0)
value1 = KeyValue()
value1.key = "Latitude"
value1.value = "%f" % _gps_fix.latitude
status.values.append(value1)
value2 = KeyValue()
value2.key = "Longitude"
value2.value = "%f" % _gps_fix.longitude
status.values.append(value2)
value3 = KeyValue()
value3.key = "Altitude"
value3.value = "%f" % _gps_fix.altitude
status.values.append(value3)
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
rospy.sleep(1.0)
from rosplan_dispatch_msgs.msg import ActionDispatch
if __name__ == '__main__':
try:
if (len(sys.argv) < 4):
print "Usage:\n\t{} <pick_up/put_down> <block_1> <block_2>".format(sys.argv[0].split("/")[-1])
rospy.init_node("mock_action_dispatch", log_level=rospy.DEBUG)
publisher = rospy.Publisher("/kcl_rosplan/action_dispatch", ActionDispatch, queue_size=1000, latch=True)
action = ActionDispatch()
action.name = sys.argv[1]
pair = KeyValue()
pair.key = "block"
pair.value = sys.argv[2]
action.parameters.append(pair)
pair = KeyValue()
pair.key = "from_block" if (action.name == "pick_up") else "on_block"
pair.value = sys.argv[3]
action.parameters.append(pair)
publisher.publish(action)
rospy.spin()
except rospy.ROSInterruptException, e:
pass
def initialize_origin():
rospy.init_node('initialize_origin', anonymous=True)
global _origin_pub
global _local_xy_frame
_origin_pub = rospy.Publisher('/local_xy_origin', GPSFix, latch=True)
diagnostic_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
local_xy_origin = rospy.get_param('~local_xy_origin', 'auto')
_local_xy_frame = rospy.get_param('~local_xy_frame', 'map')
if local_xy_origin == "auto":
global _sub
_sub = rospy.Subscriber("gps", GPSFix, gps_callback)
else:
parse_origin(local_xy_origin)
hw_id = rospy.get_param('~hw_id', 'none')
while not rospy.is_shutdown():
if _gps_fix == None:
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
status.level = DiagnosticStatus.ERROR
status.message = "No Origin"
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
else:
_origin_pub.publish(_gps_fix) # Publish this at 1Hz for bag convenience
diagnostic = DiagnosticArray()
diagnostic.header.stamp = rospy.Time.now()
status = DiagnosticStatus()
status.name = "LocalXY Origin"
status.hardware_id = hw_id
if _gps_fix.status.header.frame_id == 'auto':
status.level = DiagnosticStatus.OK
status.message = "Has Origin (auto)"
else:
status.level = DiagnosticStatus.WARN
status.message = "Origin is static (non-auto)"
value0 = KeyValue()
value0.key = "Origin"
value0.value = _gps_fix.status.header.frame_id
status.values.append(value0)
value1 = KeyValue()
value1.key = "Latitude"
value1.value = "%f" % _gps_fix.latitude
status.values.append(value1)
value2 = KeyValue()
value2.key = "Longitude"
value2.value = "%f" % _gps_fix.longitude
status.values.append(value2)
value3 = KeyValue()
value3.key = "Altitude"
value3.value = "%f" % _gps_fix.altitude
status.values.append(value3)
diagnostic.status.append(status)
diagnostic_pub.publish(diagnostic)
rospy.sleep(1.0)
