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 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 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 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)