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 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 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 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 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 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 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 nmea_depth_tcp(): # Init node system_name = socket.gethostname() rospy.init_node("lowrance_sonar", anonymous=True) rospy.loginfo("[nmea_depth_tcp] Initializing node...") # Parameters tcp_addr = rospy.get_param('~address') tcp_port = rospy.get_param('~port', 10110) # Lowrance standard port update_rate = rospy.get_param( '~update_rate', 40) # Measurement comm rate for Lowrance (Hz) # Connect TCP client to destination try: tcp_in.connect((tcp_addr, tcp_port)) except IOError as exp: rospy.logerr("Socket error: %s" % exp.strerror) rospy.signal_shutdown(reason="Socket error: %s" % exp.strerror) # NMEA Sentence publisher (to publish NMEA sentence regardless of content) sentence_pub = rospy.Publisher("%s/sonar/nmea_sentence" % system_name, Sentence, queue_size=10) # GPS publishers # GPGGA - Position # GPVTG - Velocity Track Good (ground speed) # GPZDA - Time reference (GPST) # GPGSA - Active Satellites # GPGSV - Satellites in View position_pub = rospy.Publisher("%s/sonar/gps/fix" % system_name, NavSatFix, queue_size=10) vel_pub = rospy.Publisher("%s/sonar/gps/vel" % system_name, TwistStamped, queue_size=10) timeref_pub = rospy.Publisher("%s/sonar/gps/time_reference" % system_name, TimeReference, queue_size=10) active_sat_pub = rospy.Publisher("%s/sonar/gps/active_satellites" % system_name, Gpgsa, queue_size=10) sat_view_pub = rospy.Publisher("%s/sonar/gps/satellites_in_view" % system_name, Gpgsv, queue_size=10) # Sidescanner publishers # SDDBT - Depth Below Transducer # SDDPT - Depth of Water # SDMTW - Mean Temperature of Water # SDVHW - Velocity and heading in Water # SDHDG - Magnetic heading depth_below_trans_pub = rospy.Publisher( "%s/sonar/scanner/water/depth_below_transducer" % system_name, DepthBelowTransducer, queue_size=10) depth_water_pub = rospy.Publisher("%s/sonar/scanner/water/depth" % system_name, DepthOfWater, queue_size=10) temp_water_pub = rospy.Publisher("%s/sonar/scanner/water/temperature" % system_name, Temperature, queue_size=10) water_heading_speed_pub = rospy.Publisher( "%s/sonar/scanner/water/heading_and_speed" % system_name, WaterHeadingSpeed, queue_size=10) mag_heading_pub = rospy.Publisher("%s/sonar/scanner/magnetic_heading" % system_name, MagneticHeading, queue_size=10) # Diagnostics publisher diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10) rate = rospy.Rate(update_rate) # Defines the publish rate rospy.loginfo("[nmea_depth_tcp] Initialization done.") # rospy.loginfo("[nmea_depth_tcp] Published topics:") # rospy.loginfo("[nmea_depth_tcp] Sentence:\t\t\t%s/nmea_sentence" % system_name) # rospy.loginfo("[nmea_depth_tcp] GPS Fix:\t\t\t%s/fix" % system_name) # rospy.loginfo("[nmea_depth_tcp] GPS Velocity:\t\t%s/vel" % system_name) # rospy.loginfo("[nmea_depth_tcp] Time Reference:\t\t%s/time_reference" % system_name) # rospy.loginfo("[nmea_depth_tcp] Depth of Water:\t\t%s/depth/water" % system_name) # rospy.loginfo("[nmea_depth_tcp] Depth below transducer:\t%s/depth/below_transducer" % system_name) # Run node last_update = 0 while not rospy.is_shutdown(): try: nmea_in = tcp_in.makefile().readline() except socket.error: pass nmea_parts = nmea_in.strip().split(',') ros_now = rospy.Time().now() diag_msg = DiagnosticArray() diag_msg.status.append(DiagnosticStatus()) diag_msg.status[0].name = 'sonar' diag_msg.status[0].hardware_id = '%s' % system_name if len(nmea_parts): #### GPS # GPS Fix position if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10: latitude = cast_float( nmea_parts[2][0:2]) + cast_float(nmea_parts[2][2:]) / 60.0 if nmea_parts[3] == 'S': latitude = -latitude longitude = cast_float( nmea_parts[4][0:3]) + cast_float(nmea_parts[4][3:]) / 60.0 if nmea_parts[5] == 'W': longitude = -longitude altitude = cast_float(nmea_parts[9]) nsf = NavSatFix() nsf.header.stamp = ros_now nsf.header.frame_id = system_name nsf.latitude = latitude nsf.longitude = longitude nsf.altitude = altitude position_pub.publish(nsf) # Velocity if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9: vel = TwistStamped() vel.header.frame_id = system_name vel.header.stamp = ros_now vel.twist.linear.x = cast_float( nmea_parts[7]) / 3.6 # Km/h to m/s vel_pub.publish(vel) # Time reference (GPST) if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5: tref = TimeReference() tref.header.frame_id = system_name tref.header.stamp = ros_now hour = cast_int(nmea_parts[1][0:2]) minute = cast_int(nmea_parts[1][2:4]) second = cast_int(nmea_parts[1][4:6]) try: ms = int(float(nmea_parts[1][6:]) * 1000000) except ValueError: ms = 0 day = cast_int(nmea_parts[2]) month = cast_int(nmea_parts[3]) year = cast_int(nmea_parts[4]) try: zda = datetime.datetime(year, month, day, hour, minute, second, ms) tref.time_ref = rospy.Time( calendar.timegm(zda.timetuple()), zda.microsecond * 1000) except ValueError: pass tref.source = system_name timeref_pub.publish(tref) # GPS DOP and active satellites if nmea_parts[0] == '$GPGSA' and len(nmea_parts) >= 18: gsa = Gpgsa() gsa.header.frame_id = system_name gsa.header.stamp = ros_now gsa.auto_manual_mode = nmea_parts[1] gsa.fix_mode = cast_int(nmea_parts[2]) satellite_list = [] for x in nmea_parts[3:14]: try: satellite_list.append(int(x)) except ValueError: break gsa.sv_ids = satellite_list gsa.pdop = cast_float(nmea_parts[15]) gsa.hdop = cast_float(nmea_parts[16]) gsa.vdop = cast_float(nmea_parts[17]) active_sat_pub.publish(gsa) # GPS Satellites in View if nmea_parts[0] == '$GPGSV' and len(nmea_parts) >= 7: gsv = Gpgsv() gsv.header.frame_id = system_name gsv.header.stamp = ros_now gsv.n_msgs = cast_int(nmea_parts[1]) gsv.msg_number = cast_int(nmea_parts[2]) gsv.n_satellites = cast_int(nmea_parts[3]) for i in range(4, len(nmea_parts), 4): gsv_sat = GpgsvSatellite() try: gsv_sat.prn = int(nmea_parts[i]) except ValueError: pass try: gsv_sat.elevation = int(nmea_parts[i + 1]) except ValueError: pass try: gsv_sat.azimuth = int(nmea_parts[i + 2]) except ValueError: pass try: gsv_sat.snr = int(nmea_parts[i + 3].split("*")[0]) except ValueError: pass gsv.satellites.append(gsv_sat) sat_view_pub.publish(gsv) #### Side-scanner # Depth (DBT - Depth Below Transducer) if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7: d = DepthBelowTransducer() d.header.frame_id = system_name d.header.stamp = ros_now try: d.feet = cast_float(nmea_parts[1]) # In feet except ValueError: pass try: d.meters = cast_float(nmea_parts[3]) # In meters except ValueError: pass try: d.fathoms = cast_float(nmea_parts[5]) # In fathoms except ValueError: pass depth_below_trans_pub.publish(d) # Depth (DPT - DePTh of water) if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4: d = DepthOfWater() d.header.frame_id = system_name d.header.stamp = ros_now try: d.depth = cast_float(nmea_parts[1]) # In meters except ValueError: pass try: d.offset = cast_float(nmea_parts[2]) # In meters except ValueError: pass try: d.range = cast_float(nmea_parts[3]) except ValueError: pass depth_water_pub.publish(d) # SDMTW - Mean Temperature of Water if nmea_parts[0] == '$SDMTW' and len(nmea_parts) >= 3: tempC = Temperature() tempC.header.frame_id = system_name tempC.header.stamp = ros_now tempC.temperature = cast_float(nmea_parts[1]) temp_water_pub.publish(tempC) # SDVHW - Water Heading and Speed if nmea_parts[0] == '$SDVHW' and len(nmea_parts) >= 9: whs = WaterHeadingSpeed() whs.header.frame_id = system_name whs.header.stamp = ros_now whs.true_heading = cast_float(nmea_parts[1]) whs.mag_heading = cast_float(nmea_parts[3]) whs.knots = cast_float(nmea_parts[5]) whs.kmph = cast_float(nmea_parts[7]) whs.mps = whs.kmph / 3.6 # Km/h to m/s water_heading_speed_pub.publish(whs) # SDHDG - Magnetic heading if nmea_parts[0] == '$SDHDG' and len(nmea_parts) >= 6: hdg = MagneticHeading() hdg.header.frame_id = system_name hdg.header.stamp = ros_now hdg.heading = cast_float(nmea_parts[1]) hdg.mag_dev = cast_float(nmea_parts[2]) hdg.mag_dev_dir = nmea_parts[3] hdg.mag_var = cast_float(nmea_parts[4]) hdg.mag_var_dir = nmea_parts[5].split('*')[0] quat = quaternion_from_euler(0.0, 0.0, cast_float(hdg.heading)) hdg.quaternion.x = quat[0] hdg.quaternion.y = quat[1] hdg.quaternion.z = quat[2] hdg.quaternion.w = quat[3] mag_heading_pub.publish(hdg) # NMEA Sentence (published regardless of content) sentence_msg = Sentence() sentence_msg.header.frame_id = system_name sentence_msg.header.stamp = ros_now sentence_msg.sentence = nmea_in sentence_pub.publish(sentence_msg) diag_msg.status[0].level = DiagnosticStatus.OK diag_msg.status[0].message = 'OK' diag_msg.status[0].values = [ KeyValue(key="Sentence", value=sentence_msg.sentence) ] last_update = ros_now # Check for stale status elapsed = ros_now.to_sec() - last_update.to_sec() if elapsed > 35: diag_msg.status[0].level = DiagnosticStatus.STALE diag_msg.status[0].message = 'Stale' diag_msg.status[0].values = [ KeyValue(key="Update Status", value='Stale'), KeyValue(key="Time Since Update", value=str(elapsed)) ] # Publish diagnostics message diag_pub.publish(diag_msg) # Node sleeps for some time rate.sleep()
def upgrade_load_action(): params = [KeyValue('Type', 'load')] return Action(action_type='upgrade', params=params, action_name='upgrade_load')
def bid_action(job_name, bid): params = [KeyValue('Job', job_name), KeyValue('Bid', str(bid))] return Action(action_type='bid_for_job', params=params, action_name='bid_for_' + job_name)
def assist_assemble_action(to_agent): params = [KeyValue('Agent', to_agent)] return Action(action_type='assist_assemble', params=params, action_name='assist_assemble_' + to_agent)
def retrieve_delivered_action(item_name, amount=1): params = [KeyValue('Item', item_name), KeyValue('Amount', str(amount))] return Action(action_type='retrieve_delivered', params=params, action_name='retrieve_delivered_' + item_name)
def check_memory(self): values = [] level = DiagnosticStatus.OK msg = '' mem_dict = {0: 'OK', 1: 'Low Memory', 2: 'Very Low Memory'} try: p = subprocess.Popen('free -tm', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) stdout, stderr = p.communicate() retcode = p.returncode if retcode != 0: values.append( KeyValue(key="\"free -tm\" Call Error", value=str(retcode))) return DiagnosticStatus.ERROR, values rows = stdout.split('\n') data = rows[1].split() total_mem_physical = data[1] used_mem_physical = data[2] free_mem_physical = data[3] data = rows[2].split() total_mem_swap = data[1] used_mem_swap = data[2] free_mem_swap = data[3] data = rows[3].split() total_mem = data[1] used_mem = data[2] free_mem = data[3] level = DiagnosticStatus.OK mem_usage = float(used_mem_physical) / float(total_mem_physical) if (mem_usage < self._mem_level_warn): level = DiagnosticStatus.OK elif (mem_usage < self._mem_level_error): level = DiagnosticStatus.WARN else: level = DiagnosticStatus.ERROR values.append(KeyValue(key='Memory Status', value=mem_dict[level])) values.append( KeyValue(key='Total Memory (Physical)', value=total_mem_physical + "M")) values.append( KeyValue(key='Used Memory (Physical)', value=used_mem_physical + "M")) values.append( KeyValue(key='Free Memory (Physical)', value=free_mem_physical + "M")) values.append( KeyValue(key='Total Memory (Swap)', value=total_mem_swap + "M")) values.append( KeyValue(key='Used Memory (Swap)', value=used_mem_swap + "M")) values.append( KeyValue(key='Free Memory (Swap)', value=free_mem_swap + "M")) values.append(KeyValue(key='Total Memory', value=total_mem + "M")) values.append(KeyValue(key='Used Memory', value=used_mem + "M")) values.append(KeyValue(key='Free Memory', value=free_mem + "M")) msg = mem_dict[level] except Exception, e: rospy.logerr(traceback.format_exc()) msg = 'Memory Usage Check Error' values.append(KeyValue(key=msg, value=str(e))) level = DiagnosticStatus.ERROR
def check_sensor(self): net_stats = psutil.net_if_stats() net = psutil.net_io_counters(pernic=True) diag_level = 0 diag_vals = [] diag_msg = 'warn at >%.2f%% at %.0fMBit' % (self._net_load_warn * 100.0, self._net_speed) now = time.time() warn_level = self._net_load_warn if diag_level == DiagnosticStatus.WARN: warn_level = warn_level * 0.9 interfaces = [] for net_if, net_if_stats in net_stats.items(): interfaces.append(net_if) do_parse = net_if in net if not self._interface: do_parse = do_parse and net_if_stats.isup and net_if_stats.speed > 0 else: do_parse = do_parse and net_if == self._interface if do_parse: if self.settings is not None and not self._interface: # TODO: support more than one interface self._interface = net_if self.settings.set_param('sysmon/Network/interface', net_if) net_values = net[net_if] # in psutil versions below 5.3.0 there is no 'nowrap' argument. We need to calculate current rate itself. if net_if not in self._net_stat_last: self._net_stat_last[net_if] = (0, 0) duration = now - self._ts_last bytes_sent_1s = (net_values.bytes_sent - self._net_stat_last[net_if][0]) / duration bytes_recv_1s = (net_values.bytes_recv - self._net_stat_last[net_if][1]) / duration # store current overall stats for next rate calculation self._net_stat_last[net_if] = (net_values.bytes_sent, net_values.bytes_recv) # diag_vals.append(KeyValue(key='%s: bytes sent (total)' % net_if, value=net_values.bytes_sent)) # diag_vals.append(KeyValue(key='%s: bytes recv (total)' % net_if, value=net_values.bytes_recv)) # diag_vals.append(KeyValue(key='%s: packets sent (total)' % net_if, value=net_values.packets_sent)) # diag_vals.append(KeyValue(key='%s: packets recv (total)' % net_if, value=net_values.packets_recv)) diag_vals.append( KeyValue(key='%s: sent [1s]' % net_if, value='%.2f' % bytes_sent_1s)) diag_vals.append( KeyValue(key='%s: recv [1s]' % net_if, value='%.2f' % bytes_recv_1s)) percent_sent = bytes_sent_1s / (self._net_speed * 1024 * 1024 / 8) # diag_vals.append(KeyValue(key='%s: sent [%%]' % net_if, value='%.2f' % (percent_sent * 100))) percent_recv = bytes_recv_1s / (self._net_speed * 1024 * 1024 / 8) # diag_vals.append(KeyValue(key='%s: recv [%%]' % net_if, value='%.2f' % (percent_recv * 100))) if percent_sent >= warn_level or percent_recv >= warn_level: diag_level = DiagnosticStatus.WARN if percent_sent >= warn_level: diag_msg = 'Net load for sent is %.0f%% (warn >%.0f%% [%dMBit])' % ( percent_sent * 100, self._net_load_warn * 100, self._net_speed) if percent_recv >= warn_level: if diag_msg: diag_msg = 'Net load for sent is %.0f%% and recv %.0f%% (warn >%.0f%% [%dMBit])' % ( percent_sent * 100, percent_recv * 100, self._net_load_warn * 100, self._net_speed) else: diag_msg = 'Net load for recv is %.0f%% (warn >%.0f%% [%dMBit])' % ( percent_recv * 100, self._net_load_warn * 100, self._net_speed) # print '%s: percent %.2f, %.2f' % (net_if, percent_sent, percent_recv), ", level:", diag_level if self.settings is not None: self.settings.set_param('sysmon/Network/interface', interfaces, tag=':alt') # Update status with self.mutex: self._ts_last = now self._stat_msg.level = diag_level self._stat_msg.values = diag_vals self._stat_msg.message = diag_msg
def __init__(self, coz): """ :type coz: cozmo.Robot :param coz: The cozmo SDK robot handle (object). """ # vars self._cozmo = coz self._lin_vel = .0 self._ang_vel = .0 self._cmd_lin_vel = .0 self._cmd_ang_vel = .0 self._last_pose = self._cozmo.pose self._wheel_vel = (0, 0) self._optical_frame_orientation = quaternion_from_euler( -np.pi / 2., .0, -np.pi / 2.) self._camera_info_manager = CameraInfoManager( 'cozmo_camera', namespace='/cozmo_camera') # tf self._tfb = TransformBroadcaster() # params self._odom_frame = rospy.get_param('~odom_frame', 'odom') self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint') self._base_frame = rospy.get_param('~base_frame', 'base_link') self._head_frame = rospy.get_param('~head_frame', 'head_link') self._camera_frame = rospy.get_param('~camera_frame', 'camera_link') self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera') camera_info_url = rospy.get_param('~camera_info_url', '') # pubs self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1) self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1) # Note: camera is published under global topic (preceding "/") self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10) self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10) # subs self._backpack_led_sub = rospy.Subscriber('backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1) self._twist_sub = rospy.Subscriber('turtle1/cmd_vel', Twist, self._twist_callback, queue_size=1) self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1) self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1) self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1) # diagnostics self._diag_array = DiagnosticArray() self._diag_array.header.frame_id = self._base_frame diag_status = DiagnosticStatus() diag_status.hardware_id = 'Cozmo Robot' diag_status.name = 'Cozmo Status' diag_status.values.append(KeyValue(key='Battery Voltage', value='')) diag_status.values.append(KeyValue(key='Head Angle', value='')) diag_status.values.append(KeyValue(key='Lift Height', value='')) self._diag_array.status.append(diag_status) self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1) # camera info manager self._camera_info_manager.setURL(camera_info_url) self._camera_info_manager.loadCameraInfo()
def waypoints_callback(self, data): """ Callback appelé à chaque passage de waypoint : - Détection de début et fin de ligne + publication sur /navigation/line_status - Enregistrement des waypoints de fin de lignes - Enregistrement dans un dictionnaire à chaque nouvelle ligne de la type de ligne et du numéro du premier waypoint - Publication de diagnostics Entrée : data : mavros_msgs Waypoint - data.waypoints : liste des waypoints de la mission - data.current_seq : waypoint courant """ #----- Callback appelé à chaque changement de waypoints #rospy.logwarn("Changement de waypoint") rospy.loginfo("Nouveau waypoint :" + str(data.current_seq)) rospy.loginfo("Liste des waypoints enregistrés" + str(self.last_waypoint_list)) self.wp_number = len(data.waypoints) self.waypoints_list = data.waypoints #------------ Fin de ligne ---------------------- if data.waypoints[data.current_seq].param1 == 1: if data.waypoints[self.current_wp].param1 == 0: #------------- Enregistrement du dernier waypoint de la ligne self.last_waypoint = self.current_wp rospy.loginfo("Fin de ligne") rospy.loginfo("Waypoint enregistré de fin de ligne " + str(self.last_waypoint)) self.last_waypoint_list.append(self.last_waypoint) nav_msg = KeyValue() nav_msg.key = "End" if data.waypoints[data.current_seq].param2 == 0: nav_msg.value = "Reg" else: nav_msg.value = "Trav" self.nav_pub.publish(nav_msg) #----------Debut de ligne ---------------------- elif data.waypoints[data.current_seq].param1 == 0: if data.waypoints[self.current_wp].param1 == 1: rospy.loginfo("Début de ligne") nav_msg = KeyValue() nav_msg.key = "Start" if data.waypoints[data.current_seq].param2 == 0: nav_msg.value = "Reg" self.nbr_reg += 1 ligne = "Reg" + str(self.nbr_reg) else: nav_msg.value = "Trav" self.nbr_trav += 1 ligne = "Trav" + str(self.nbr_trav) self.last_waypoint_dictionnary[ligne] = data.current_seq self.nav_pub.publish(nav_msg) self.current_wp = data.current_seq #rospy.loginfo("dictionnaire : " + str(self.last_waypoint_dictionnary)) #---------Pubication des diagnostiques----------- diagnostics = DiagnosticArray() diagnostics.status.append( DiagnosticStatus(level=0, name="controller/waypoints/Number", message=str(self.wp_number))) diagnostics.status.append( DiagnosticStatus(level=0, name="controller/waypoints/Current", message=str(self.current_wp))) diagnostics.header.stamp = rospy.Time.now() self.diag_pub.publish(diagnostics)
def check_network(self): values = [] net_dict = { 0: 'OK', 1: 'High Network Usage', 2: 'Network Down', 3: 'Call Error' } try: p = subprocess.Popen('ifstat -q -S 1 1', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) stdout, _ = p.communicate() retcode = p.returncode if retcode != 0: values.append( KeyValue(key="\"ifstat -q -S 1 1\" Call Error", value=str(retcode))) return DiagnosticStatus.ERROR, net_dict[3], values rows = stdout.split('\n') data = rows[0].split() ifaces = [] for i in range(0, len(data)): ifaces.append(data[i]) data = rows[2].split() kb_in = [] kb_out = [] for i in range(0, len(data), 2): kb_in.append(data[i]) kb_out.append(data[i + 1]) level = DiagnosticStatus.OK for i in range(0, len(ifaces)): values.append(KeyValue(key='Interface Name', value=ifaces[i])) (retcode, cmd_out) = get_sys_net(ifaces[i], 'operstate') if retcode == 0: values.append(KeyValue(key='State', value=cmd_out)) ifacematch = re.match('eth[0-9]+', ifaces[i]) if ifacematch and (cmd_out == 'down' or cmd_out == 'dormant'): level = DiagnosticStatus.ERROR try: in_traffic = float(kb_in[i]) / 1024 out_traffic = float(kb_out[i]) / 1024 net_usage_in = float(kb_in[i]) / 1024 / self._net_capacity net_usage_out = float( kb_out[i]) / 1024 / self._net_capacity except ValueError: in_traffic = 0 out_traffic = 0 net_usage_in = 0 net_usage_out = 0 values.append( KeyValue(key='Input Traffic', value=str(in_traffic) + " (MB/s)")) values.append( KeyValue(key='Output Traffic', value=str(out_traffic) + " (MB/s)")) if net_usage_in > self._net_level_warn or\ net_usage_out > self._net_level_warn: level = DiagnosticStatus.WARN (retcode, cmd_out) = get_sys_net(ifaces[i], 'mtu') if retcode == 0: values.append(KeyValue(key='MTU', value=cmd_out)) (retcode, cmd_out) = get_sys_net_stat(ifaces[i], 'rx_bytes') if retcode == 0: values.append( KeyValue(key='Total received MB', value=str(float(cmd_out) / 1024 / 1024))) (retcode, cmd_out) = get_sys_net_stat(ifaces[i], 'tx_bytes') if retcode == 0: values.append( KeyValue(key='Total transmitted MB', value=str(float(cmd_out) / 1024 / 1024))) (retcode, cmd_out) = get_sys_net_stat(ifaces[i], 'collisions') if retcode == 0: values.append(KeyValue(key='Collisions', value=cmd_out)) (retcode, cmd_out) = get_sys_net_stat(ifaces[i], 'rx_errors') if retcode == 0: values.append(KeyValue(key='Rx Errors', value=cmd_out)) (retcode, cmd_out) = get_sys_net_stat(ifaces[i], 'tx_errors') if retcode == 0: values.append(KeyValue(key='Tx Errors', value=cmd_out)) except Exception, e: rospy.logerr(traceback.format_exc()) msg = 'Network Usage Check Error' values.append(KeyValue(key=msg, value=str(e))) level = DiagnosticStatus.ERROR
def synch(self): ############################################################# """ Escribir algo useful """ ##################### Status ##################### """ Create blank diagnostics """ self.generic_diagnostic_array = DiagnosticArray() self.generic_diagnostic_status = DiagnosticStatus() self.generic_key_value = KeyValue() # Generate header self.generic_diagnostic_array.header = Header() ##################### Drive System Controllers ##################### """ Detect change of joysticks """ if self.joyL_old != self.joyL: self.joyL_change = 1 self.joyL_old = self.joyL if self.joyR_old != self.joyR: self.joyR_change = 1 self.joyR_old = self.joyR """ Send current speed """ if self.joyL_change == 1 or self.joyR_change == 1: left_speed = 0 right_speed = 0 rospy.loginfo("INFOR: Drive system = Left: %s Right: %s", self.joyL, self.joyR) data = bytearray([0x00, 0x00,0x00,0x00]) """ Set left side speed """ if self.joyL > 0: left_speed = self.joyL else: left_speed = -self.joyL left_speed = left_speed | (1 << 7) """ Set right side speed """ if self.joyR > 0: right_speed = self.joyR else: right_speed = -self.joyR right_speed = right_speed | (1 << 7) data = bytearray([0x00, left_speed, right_speed, 0x00]) self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(25) #Read response from arduino self.generic_diagnostic_status.level = 0 self.generic_diagnostic_status.name = 'Motor Controllers avg V/Temp' self.generic_diagnostic_status.message = 'Average voltage and temperature of drivers' self.generic_diagnostic_status.hardware_id = '1' # Add values self.sh_yaw_temp_key_value = KeyValue() self.sh_yaw_temp_key_value.key = 'AVG Voltage:' self.sh_yaw_temp_key_value.value = str(float(data[0:3])/10.0) + ' C' self.generic_diagnostic_status.values.append(self.sh_yaw_temp_key_value) self.sh_pitch_temp_key_value = KeyValue() self.sh_pitch_temp_key_value.key = 'AVG Temp:' self.sh_pitch_temp_key_value.value = str(float(data[3:8])/1000.0) + ' V' self.generic_diagnostic_status.values.append(self.sh_pitch_temp_key_value) #Append other diagnostics self.generic_diagnostic_array.status.append(self.generic_diagnostic_status) except: pass self.joyL_change = 0 self.joyR_change = 0 ##################### ARM Controllers ##################### """ Shoulder YAW """ if self.PLaR_old != self.PLaR: self.PLaR_change = 1 self.PLaR_old = self.PLaR if self.PLaR_change == 1: if self.PLaR == -1: self.PLaR_speed = 100 rospy.loginfo("INFO: Shoulder moving: LEFT ") if self.PLaR == 1: self.PLaR_speed = 100 | (1 << 7) rospy.loginfo("INFO: Shoulder moving: RIGHT ") if self.PLaR == 0: self.PLaR_speed = 0 """ Shoulder PITCH """ if self.L1_old != self.L1: self.L1_change = 1 self.L1_old = self.L1 if self.L2_old != self.L2: self.L2_change = 1 self.L2_old = self.L2 if self.L1_change == 1: if self.L1 == 1: self.L1_L2_speed = 80 rospy.loginfo("INFO: Shoulder moving: DOWN ") else: self.L1_L2_speed = 0 if self.L2_change == 1 : if self.L2 == 1 : self.L1_L2_speed = 80 | (1 << 7) rospy.loginfo("INFO: Shoulder moving: UP") else: self.L1_L2_speed = 0 """ Elbow PITCH """ if self.R1_old != self.R1: self.R1_change = 1 self.R1_old = self.R1 if self.R2_old != self.R2: self.R2_change = 1 self.R2_old = self.R2 if self.R1_change == 1: if self.R1 == 1: self.R1_R2_speed = 80 | (1 << 7) rospy.loginfo("INFO: Shoulder moving: DOWN ") else: self.R1_R2_speed = 0 if self.R2_change == 1 : if self.R2 == 1 : self.R1_R2_speed = 80 rospy.loginfo("INFO: Shoulder moving: UP") else: self.R1_R2_speed = 0 if (self.PLaR_change == 1) or (self.L1_change == 1) or (self.L2_change == 1) or (self.R1_change == 1) or (self.R2_change == 1): # Print speed rospy.loginfo("INFO: Shoulder yaw: %s Shoulder pitch: %s Elbow pitch: %s", self.PLaR_speed, self.L1_L2_speed, self.R1_R2_speed) # Concatenate values data = bytearray([self.R1_R2_speed, self.L1_L2_speed, self.PLaR_speed, 0x07]) self.socket.sendto(data, self.address) #send command to arduino # Reset changes self.PLaR_change = 0 self.L1_change = 0 self.L2_change = 0 self.R1_change = 0 self.R2_change = 0 ##################### Gripper Controllers ##################### """ Wrist PITCH """ if self.PUaD == -1: self.PUaD_speed = 0x01 self.PUaD_change = 1 self.PUaD_trigger = 0 rospy.loginfo("INFO: Wrist moving: DOWN ") if self.PUaD == 1: self.PUaD_speed = 0x02 self.PUaD_change = 1 self.PUaD_trigger = 0 rospy.loginfo("INFO: Wrist moving: UP ") if self.PUaD == 0: self.PUaD_speed = 0x00 if(self.PUaD_trigger == 0): self.PUaD_change = 1 self.PUaD_trigger = 1 else: self.PUaD_change = 0 """ Wrist ROLL (Drill) """ if self.SQ_old != self.SQ: self.SQ_change = 1 self.SQ_old = self.SQ if self.CI_old != self.CI: self.CI_change = 1 self.CI_old = self.CI if self.SQ_change == 1: if self.SQ == 1: self.WRIST_ROLL_speed = 40 | (1 << 7) rospy.loginfo("INFO: Wrist moving: ROLL LEFT") else: self.WRIST_ROLL_speed = 0 if self.CI_change == 1 : if self.CI == 1 : self.WRIST_ROLL_speed = 40 rospy.loginfo("INFO: Wrist moving: ROLL RIGHT") else: self.WRIST_ROLL_speed = 0 """ Gripper ROLL """ if self.TR_old != self.TR: self.TR_change = 1 self.TR_old = self.TR if self.CR_old != self.CR: self.CR_change = 1 self.CR_old = self.CR if self.TR_change == 1: if self.TR == 1: self.GRIPPER_ROLL_speed = 100 | (1 << 7) rospy.loginfo("INFO: Gripper moving: OPENING") else: self.GRIPPER_ROLL_speed = 0 if self.CR_change == 1 : if self.CR == 1 : self.GRIPPER_ROLL_speed = 100 rospy.loginfo("INFO: Gripper moving: CLOSING") else: self.GRIPPER_ROLL_speed = 0 if (self.PUaD_change == 1) or (self.SQ_change == 1) or (self.CI_change == 1) or (self.TR_change == 1) or (self.CR_change == 1): # Print speed rospy.loginfo("INFO: Wrist yaw: %s Wrist roll: %s Gripper Roll: %s", self.PUaD_speed, self.WRIST_ROLL_speed, self.GRIPPER_ROLL_speed) # Concatenate values data = bytearray([self.GRIPPER_ROLL_speed, self.WRIST_ROLL_speed, self.PUaD_speed, 0x08]) self.socket.sendto(data, self.address) #send command to arduino # Reset changes self.PUaD_change = 0 self.SQ_change = 0 self.CI_change = 0 self.TR_change = 0 self.CR_change = 0 ##################### Cooling System ##################### if self.OP == 1: self.cool_left ^= 1 data = bytearray([0x00, 0x00, (self.cool_left << 1) | self.cool_right, 0x0D]) self.socket.sendto(data, self.address) #send command to arduino if self.SH == 1: self.cool_right ^= 1 data = bytearray([0x00, 0x00, (self.cool_left << 1) | self.cool_right, 0x0D]) self.socket.sendto(data, self.address) #send command to arduino ##################### IMU ##################### #if (rospy.Time.now().secs - self.time_nsec) >= 1: """ data = bytearray([0x00, 0x00, 0x00, 0x0E]) # Suspension Right Back self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.jointRB = data except: pass data = bytearray([0x00, 0x00, 0x00, 0x4E]) # Suspension Right Front self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.jointRF = data except: pass data = bytearray([0x00, 0x00, 0x00, 0x8E]) # Suspension Left Front self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.jointLF = data except: pass data = bytearray([0x00, 0x00, 0x00, 0xCE]) # Suspension Left Back self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.jointLB = data except: pass data = bytearray([0x00, 0x00, 0x00, 0x0F]) # Suspension Left Back self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.chassisR = data except: pass data = bytearray([0x00, 0x00, 0x00, 0x4F]) # Suspension Left Back self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.chassisP = data except: pass data = bytearray([0x00, 0x00, 0x00, 0x8F]) # Suspension Left Back self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino self.chassisY = data except: pass #print "Acel: " + self.jointRB + " " + self.jointRF +" "+ self.jointLF + " " + self.jointLB +" "+ self.chassisP + " " + self.chassisR + " " + self.chassisY self.joints.header = Header() self.joints.header.stamp = rospy.Time.now() #self.joints.position = [float(self.chassisP), float(self.chassisR), float(self.chassisY), float(self.jointRB), float(self.jointLB), float(self.jointRF), float(self.jointLF)] self.joints.position = [0, 0, 0, float(self.jointRB), float(self.jointLB), float(self.jointRF), float(self.jointLF)] self.joints.velocity = [] self.joints.effort = [] self.imu.publish(self.joints) self.time_nsec = rospy.Time.now().secs """ ##################### Location ##################### if (rospy.Time.now().secs - self.time_sec) >= 2: rospy.loginfo("INFO: Location: Query") # Get Latitude data = bytearray([0x00,0x00,0x00,0x11]) self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino if data == "-1": self.loc_flag = 1 else: self.navsat.latitude = float(data) #print data except: pass # Get Longitude data = bytearray([0x00,0x00,0x00,0x51]) self.socket.sendto(data, self.address) #send command to arduino try: data, addr = self.socket.recvfrom(15) #Read response from arduino if data == "-1" or self.loc_flag == 1: rospy.loginfo("WARNING: No FIX, Dropping frame") self.loc_flag = 0 else: self.navsat.longitude = float(data) rospy.loginfo("INFO: Location: Recieved") #print data except: pass self.location.publish(self.navsat) self.time_sec = rospy.Time.now().secs # Publish self.current_status.publish(self.generic_diagnostic_array) """ # dx = (l + r) / 2
def check_nfs_stat(self): if rospy.is_shutdown(): with self._mutex: self.cancel_timers() return nfs_level = 0 msg = 'OK' vals = [ KeyValue(key='Update Status', value='OK'), KeyValue(key='Time Since Last Update', value=str(0)) ] try: p = subprocess.Popen('iostat -n', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) stdout, stderr = p.communicate() retcode = p.returncode if retcode != 0: nfs_level = DiagnosticStatus.ERROR msg = 'iostat Error' vals.append( KeyValue(key='\"iostat -n\" Call Error', value=str(e))) stdout = '' for index, row in enumerate(stdout.split('\n')): if index < 3: continue lst = row.split() if len(lst) < 7: continue file_sys = lst[0] read_blk = lst[1] write_blk = lst[2] read_blk_dir = lst[3] write_blk_dir = lst[4] r_blk_srv = lst[5] w_blk_srv = lst[6] vals.append( KeyValue(key='%s Read Blks/s' % file_sys, value=read_blk)) vals.append( KeyValue(key='%s Write Blks/s' % file_sys, value=write_blk)) vals.append( KeyValue(key='%s Read Blk dir/s' % file_sys, value=read_blk_dir)) vals.append( KeyValue(key='%s Write Blks dir/s' % file_sys, value=write_blk_dir)) vals.append( KeyValue(key='%s Read Blks srv/s' % file_sys, value=r_blk_srv)) vals.append( KeyValue(key='%s Write Blks srv/s' % file_sys, value=w_blk_srv)) except Exception, e: rospy.logerr(traceback.format_exc()) nfs_level = DiagnosticStatus.ERROR msg = 'Exception' vals.append(KeyValue(key='Exception', value=str(e)))
def check_ipmi(): diag_vals = [] diag_msgs = [] diag_level = DiagnosticStatus.OK try: p = subprocess.Popen('sudo ipmitool sdr', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) stdout, stderr = p.communicate() retcode = p.returncode if retcode != 0: diag_level = DiagnosticStatus.ERROR diag_msgs = ['ipmitool Error'] diag_vals = [KeyValue(key='IPMI Error', value=stderr)] return diag_vals, diag_msgs, diag_level lines = stdout.split('\n') if len(lines) < 2: diag_vals = [KeyValue(key='ipmitool status', value='No output')] diag_msgs = ['No ipmitool response'] diag_level = DiagnosticStatus.ERROR return diag_vals, diag_msgs, diag_level for ln in lines: if len(ln) < 3: continue words = ln.split('|') if len(words) < 3: continue name = words[0].strip() ipmi_val = words[1].strip() stat_byte = words[2].strip() # CPU temps if words[0].startswith('CPU') and words[0].strip().endswith( 'Temp'): if words[1].strip().endswith('degrees C'): tmp = ipmi_val.rstrip(' degrees C').lstrip() if unicode(tmp).isnumeric(): temperature = float(tmp) diag_vals.append(KeyValue(key=name + ' (C)', value=tmp)) cpu_name = name.split()[0] if temperature >= 80 and temperature < 89: diag_level = max(diag_level, DiagnosticStatus.WARN) if diag_msgs.count('CPU Hot') == 0: diag_msgs.append('CPU Warm') if temperature >= 89: # CPU should shut down here diag_level = max(diag_level, DiagnosticStatus.ERROR) diag_msgs.append('CPU Hot') # Don't keep CPU Warm in list if CPU is hot if diag_msgs.count('CPU Warm') > 0: idx = diag_msgs.index('CPU Warm') diag_msgs.pop(idx) else: diag_vals.append(KeyValue(key=name, value=words[1])) # MP, BP, FP temps if name == 'MB Temp' or name == 'BP Temp' or name == 'FP Temp': if ipmi_val.endswith('degrees C'): tmp = ipmi_val.rstrip(' degrees C').lstrip() diag_vals.append(KeyValue(key=name + ' (C)', value=tmp)) # Give temp warning dev_name = name.split()[0] if unicode(tmp).isnumeric(): temperature = float(tmp) if temperature >= 60 and temperature < 75: diag_level = max(diag_level, DiagnosticStatus.WARN) diag_msgs.append('%s Warm' % dev_name) if temperature >= 75: diag_level = max(diag_level, DiagnosticStatus.ERROR) diag_msgs.append('%s Hot' % dev_name) else: diag_level = max(diag_level, DiagnosticStatus.ERROR) diag_msgs.append('%s Error' % dev_name) else: diag_vals.append(KeyValue(key=name, value=ipmi_val)) # CPU fan speeds if (name.startswith('CPU') and name.endswith('Fan')) or name == 'MB Fan': if ipmi_val.endswith('RPM'): rpm = ipmi_val.rstrip(' RPM').lstrip() if unicode(rpm).isnumeric(): if int(rpm) == 0: diag_level = max(diag_level, DiagnosticStatus.ERROR) diag_msgs.append('CPU Fan Off') diag_vals.append(KeyValue(key=name + ' RPM', value=rpm)) else: diag_vals.append(KeyValue(key=name, value=ipmi_val)) # If CPU is hot we get an alarm from ipmitool, report that too # CPU should shut down if we get a hot alarm, so report as error if name.startswith('CPU') and name.endswith('hot'): if ipmi_val == '0x01': diag_vals.append(KeyValue(key=name, value='OK')) else: diag_vals.append(KeyValue(key=name, value='Hot')) diag_level = max(diag_level, DiagnosticStatus.ERROR) diag_msgs.append('CPU Hot Alarm') except Exception, e: diag_vals.append( KeyValue(key='Exception', value=traceback.format_exc())) diag_level = DiagnosticStatus.ERROR diag_msgs.append('Exception')
def __init__(self, hostname, diag_hostname): self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray) self._mutex = threading.Lock() self._check_ipmi = rospy.get_param('~check_ipmi_tool', True) self._enforce_speed = rospy.get_param('~enforce_clock_speed', True) self._check_core_temps = rospy.get_param('~check_core_temps', False) if self._check_core_temps: rospy.logwarn( 'Checking CPU core temperatures is deprecated. This will be removed in D-turtle' ) self._check_nfs = rospy.get_param('~check_nfs', False) if self._check_nfs: rospy.logwarn( 'NFS checking is deprecated for CPU monitor. This will be removed in D-turtle' ) self._load1_threshold = rospy.get_param('~load1_threshold', 5.0) self._load5_threshold = rospy.get_param('~load5_threshold', 3.0) self._num_cores = rospy.get_param('~num_cores', 8.0) self._temps_timer = None self._usage_timer = None self._nfs_timer = None # Get temp_input files self._temp_vals = get_core_temp_names() # CPU stats self._temp_stat = DiagnosticStatus() self._temp_stat.name = '%s CPU Temperature' % diag_hostname self._temp_stat.level = 1 self._temp_stat.hardware_id = hostname self._temp_stat.message = 'No Data' self._temp_stat.values = [ KeyValue(key='Update Status', value='No Data'), KeyValue(key='Time Since Last Update', value='N/A') ] self._usage_stat = DiagnosticStatus() self._usage_stat.name = '%s CPU Usage' % diag_hostname self._usage_stat.level = 1 self._usage_stat.hardware_id = hostname self._usage_stat.message = 'No Data' self._usage_stat.values = [ KeyValue(key='Update Status', value='No Data'), KeyValue(key='Time Since Last Update', value='N/A') ] self._nfs_stat = DiagnosticStatus() self._nfs_stat.name = '%s NFS IO' % diag_hostname self._nfs_stat.level = 1 self._nfs_stat.hardware_id = hostname self._nfs_stat.message = 'No Data' self._nfs_stat.values = [ KeyValue(key='Update Status', value='No Data'), KeyValue(key='Time Since Last Update', value='N/A') ] self._last_temp_time = 0 self._last_usage_time = 0 self._last_nfs_time = 0 self._last_publish_time = 0 # Start checking everything self.check_temps() if self._check_nfs: self.check_nfs_stat() self.check_usage()
def check_network(self): level = DiagnosticStatus.OK net_msg = 'OK' values = [] try: p = subprocess.Popen('ifstat -q -S 1 1', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) stdout, stderr = p.communicate() ret_code = p.returncode if ret_code != 0: values.append(KeyValue(key="\"ifstat -q -S 1 1\" Call Error", value=str(ret_code))) return DiagnosticStatus.ERROR, 'Call Error', values rows = stdout.split('\n') data = rows[0].split() ifaces = [] for i in range(0, len(data)): ifaces.append(data[i]) data = rows[2].split() kb_in = [] kb_out = [] for i in range(0, len(data), 2): kb_in.append(data[i]) kb_out.append(data[i + 1]) level = DiagnosticStatus.OK for i in range(0, len(ifaces)): values.append(KeyValue(key=str(i), value="=======================")) values.append(KeyValue(key='Interface Name', value=ifaces[i])) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'operstate') if ret_code == 0: values.append(KeyValue(key='State', value=cmd_out)) ifacematch = re.match('eth[0-9]+', ifaces[i]) or re.match('eno[0-9]+', ifaces[i]) if ifacematch and (cmd_out == 'down' or cmd_out == 'dormant'): level = DiagnosticStatus.ERROR net_msg = 'Network Down' values.append(KeyValue(key='Input Traffic', value=str(float(kb_in[i]) / 1024) + " (MB/s)")) if kb_in[i] != 'n/a' else 0 values.append(KeyValue(key='Output Traffic', value=str(float(kb_out[i]) / 1024) + " (MB/s)")) if kb_out[i] != 'n/a' else 0 net_usage_in = float(kb_in[i]) / 1024 / self._net_capacity if kb_in[i] != 'n/a' else 0 net_usage_out = float(kb_out[i]) / 1024 / self._net_capacity if kb_out[i] != 'n/a' else 0 if net_usage_in > self._net_level_warn or \ net_usage_out > self._net_level_warn: level = DiagnosticStatus.WARN net_msg = 'High Network Usage (net_usage_in: {}, net_usage_out: {}, threshold: {})'.format(net_usage_in, net_usage_out, self._net_level_warn) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'mtu') if ret_code == 0: values.append(KeyValue(key='MTU', value=cmd_out)) # additional keys (https://www.kernel.org/doc/Documentation/ABI/testing/sysfs-class-net-statistics) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_bytes') if ret_code == 0: values.append(KeyValue(key='Total received MB', value=str(float(cmd_out) / 1024 / 1024))) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_bytes') if ret_code == 0: values.append(KeyValue(key='Total transmitted MB', value=str(float(cmd_out) / 1024 / 1024))) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'collisions') if ret_code == 0: values.append(KeyValue(key='collisions', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_errors') if ret_code == 0: values.append(KeyValue(key='rx_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_crc_errors') if ret_code == 0: values.append(KeyValue(key='rx_crc_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_dropped') if ret_code == 0: values.append(KeyValue(key='rx_dropped', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_fifo_errors') if ret_code == 0: values.append(KeyValue(key='rx_fifo_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_frame_errors') if ret_code == 0: values.append(KeyValue(key='rx_frame_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_length_errors') if ret_code == 0: values.append(KeyValue(key='rx_length_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_missed_errors') if ret_code == 0: values.append(KeyValue(key='rx_missed_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_over_errors') if ret_code == 0: values.append(KeyValue(key='rx_over_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'rx_packets') if ret_code == 0: values.append(KeyValue(key='rx_packets', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_errors') if ret_code == 0: values.append(KeyValue(key='tx_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_aborted_errors') if ret_code == 0: values.append(KeyValue(key='tx_aborted_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_carrier_errors') if ret_code == 0: values.append(KeyValue(key='tx_carrier_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_fifo_errors') if ret_code == 0: values.append(KeyValue(key='tx_fifo_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_heartbeat_errors') if ret_code == 0: values.append(KeyValue(key='tx_heartbeat_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_window_errors') if ret_code == 0: values.append(KeyValue(key='tx_window_errors', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_dropped') if ret_code == 0: values.append(KeyValue(key='tx_dropped', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net_stat(ifaces[i], 'tx_packets') if ret_code == 0: values.append(KeyValue(key='tx_packets', value=cmd_out)) # additional keys (https://www.kernel.org/doc/Documentation/ABI/testing/sysfs-class-net) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'addr_assign_type') if ret_code == 0: try: tmp_dict = {'0': 'permanent address', '1': 'randomly generated', '2': 'stolen from another device', '3': 'set using dev_set_mac_address'} values.append(KeyValue(key='addr_assign_type', value=tmp_dict[cmd_out])) except KeyError: values.append(KeyValue(key='addr_assign_type', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'address') if ret_code == 0: values.append(KeyValue(key='address', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'carrier') if ret_code == 0: try: tmp_dict = {'0': 'physical link is down', '1': 'physical link is up'} values.append(KeyValue(key='carrier', value=tmp_dict[cmd_out])) except KeyError: values.append(KeyValue(key='carrier', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'carrier_changes') if ret_code == 0: values.append(KeyValue(key='carrier_changes', value=cmd_out)) if cmd_out > self._carrier_changes_threshold: level = DiagnosticStatus.WARN net_msg = 'Network unstable (carrier_changes: {}, threshold: {})'.format(cmd_out, self._carrier_changes_threshold) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'carrier_up_count') if ret_code == 0: values.append(KeyValue(key='carrier_up_count', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'carrier_down_count') if ret_code == 0: values.append(KeyValue(key='carrier_down_count', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'speed') if ret_code == 0: values.append(KeyValue(key='speed', value=cmd_out)) (ret_code, cmd_out) = self.get_sys_net(ifaces[i], 'tx_queue_len') if ret_code == 0: values.append(KeyValue(key='tx_queue_len', value=cmd_out)) except Exception, e: rospy.logerr(traceback.format_exc()) net_msg = 'Network Usage Check Error' values.append(KeyValue(key=net_msg, value=str(e))) level = DiagnosticStatus.ERROR
def goto_facility_action(facility_name): params = [KeyValue('Facility', facility_name)] return Action(action_type='goto', params=params, action_name='goto_facility_' + facility_name)
def check_temps(self): if rospy.is_shutdown(): with self._mutex: self.cancel_timers() return diag_strs = [ KeyValue(key='Update Status', value='OK'), KeyValue(key='Time Since Last Update', value='0') ] diag_level = DiagnosticStatus.OK diag_message = 'OK' temp_ok, drives, makes, temps = get_hddtemp_data() for index in range(0, len(drives)): temp = temps[index] if not unicode( temp).isnumeric() and drives[index] not in REMOVABLE: temp_level = DiagnosticStatus.ERROR temp_ok = False elif not unicode(temp).isnumeric() and drives[index] in REMOVABLE: temp_level = DiagnosticStatus.OK temp = "Removed" else: temp_level = DiagnosticStatus.OK if float(temp) >= self._hdd_temp_warn: temp_level = DiagnosticStatus.WARN if float(temp) >= self._hdd_temp_error: temp_level = DiagnosticStatus.ERROR diag_level = max(diag_level, temp_level) diag_strs.append( KeyValue(key='Disk %d Temperature Status' % index, value=temp_dict[temp_level])) diag_strs.append( KeyValue(key='Disk %d Mount Pt.' % index, value=drives[index])) diag_strs.append( KeyValue(key='Disk %d Device ID' % index, value=makes[index])) diag_strs.append( KeyValue(key='Disk %d Temperature' % index, value=str(temp) + "DegC")) if not temp_ok: diag_level = DiagnosticStatus.ERROR with self._mutex: self._last_temp_time = rospy.get_time() self._temp_stat.values = diag_strs self._temp_stat.level = diag_level # Give No Data message if we have no reading self._temp_stat.message = temp_dict[diag_level] if not temp_ok: self._temp_stat.message = 'Error' if self._no_temp_warn and temp_ok: self._temp_stat.level = DiagnosticStatus.OK if not rospy.is_shutdown(): self._temp_timer = threading.Timer(10.0, self.check_temps) self._temp_timer.start() else: self.cancel_timers()
def assemble_action(item_name): params = [KeyValue('Item', item_name)] return Action(action_type='assemble', params=params, action_name='assemble_' + item_name)
def check_disk_usage(self): if rospy.is_shutdown(): with self._mutex: self.cancel_timers() return diag_vals = [ KeyValue(key='Update Status', value='OK'), KeyValue(key='Time Since Last Update', value='0') ] diag_level = DiagnosticStatus.OK diag_message = 'OK' try: p = subprocess.Popen(["df", "-Pht", "ext4"], stdout=subprocess.PIPE, stderr=subprocess.PIPE) stdout, stderr = p.communicate() retcode = p.returncode if (retcode == 0 or retcode == 1): diag_vals.append(KeyValue(key='Disk Space Reading', value='OK')) rows = stdout.split('\n') del rows[0] row_count = 0 for row in rows: if len(row.split()) < 2: continue if unicode(row.split()[0]) == "none": continue row_count += 1 g_available = row.split()[-3] g_use = row.split()[-2] name = row.split()[0] size = row.split()[1] mount_pt = row.split()[-1] hdd_usage = float(g_use.replace("%", "")) * 1e-2 if (hdd_usage < self._hdd_level_warn): level = DiagnosticStatus.OK elif (hdd_usage < self._hdd_level_error): level = DiagnosticStatus.WARN else: level = DiagnosticStatus.ERROR diag_vals.append( KeyValue(key='Disk %d Name' % row_count, value=name)) diag_vals.append( KeyValue(key='Disk %d Size' % row_count, value=size)) diag_vals.append( KeyValue(key='Disk %d Available' % row_count, value=g_available)) diag_vals.append( KeyValue(key='Disk %d Use' % row_count, value=g_use)) diag_vals.append( KeyValue(key='Disk %d Status' % row_count, value=stat_dict[level])) diag_vals.append( KeyValue(key='Disk %d Mount Point' % row_count, value=mount_pt)) diag_level = max(diag_level, level) diag_message = usage_dict[diag_level] else: diag_vals.append( KeyValue(key='Disk Space Reading', value='Failed')) diag_level = DiagnosticStatus.ERROR diag_message = stat_dict[diag_level] except: rospy.logerr(traceback.format_exc()) diag_vals.append( KeyValue(key='Disk Space Reading', value='Exception')) diag_vals.append( KeyValue(key='Disk Space Ex', value=traceback.format_exc())) diag_level = DiagnosticStatus.ERROR diag_message = stat_dict[diag_level] # Update status with self._mutex: self._last_usage_time = rospy.get_time() self._usage_stat.values = diag_vals self._usage_stat.message = diag_message self._usage_stat.level = diag_level if not rospy.is_shutdown(): self._usage_timer = threading.Timer(5.0, self.check_disk_usage) self._usage_timer.start() else: self.cancel_timers()
def deliver_job_action(job_name): params = [KeyValue('Job', job_name)] return Action(action_type='deliver_job', params=params, action_name='deliver_' + job_name)
def run(self): if not self.calib: # Get EC sensor data and publish it self.ec_msg.header.stamp = rospy.Time.now() data = self.bus.get_data(self.ec_address) self.ec_msg.ec = data[0] self.ec_msg.ppm = int(data[1]) self.ec_msg.salinity = float(data[2]) self.ec_msg.specificGrav = float(data[3]) self.ecPub.publish(self.ec_msg) self.ecStatus.status.level = DiagnosticStatus.OK self.ecStatus.status.message = 'OK' self.ecStatus.status.values = [ KeyValue(key='Conductivity', value=str(self.ec_msg.ec)) ] self.ecStatus.last_update = rospy.Time.now() # Get Redox Potential sensor data and publish it self.orp_msg.header.stamp = rospy.Time.now() data = self.bus.get_data(self.orp_address) self.orp_msg.orp = data[0] self.orpPub.publish(self.orp_msg) self.orpStatus.status.level = DiagnosticStatus.OK self.orpStatus.status.message = 'OK' self.orpStatus.status.values = [ KeyValue(key='Oxi-Redox Potential', value=str(self.orp_msg.orp)) ] self.orpStatus.last_update = rospy.Time.now() # Get ph sensor data and publish it self.pH_msg.header.stamp = rospy.Time.now() data = self.bus.get_data(self.ph_address) self.pH_msg.pH = data[0] self.phPub.publish(self.pH_msg) self.pHStatus.status.level = DiagnosticStatus.OK self.pHStatus.status.message = 'OK' self.pHStatus.status.values = [ KeyValue(key='pH', value=str(self.pH_msg.pH)) ] self.pHStatus.last_update = rospy.Time.now() # Get dissolved oxygen sensor data and publish it self.do_msg.header.stamp = rospy.Time.now() data = self.bus.get_data(self.do_address) self.do_msg.do = data[0] self.do_msg.saturation = data[1] self.doPub.publish(self.do_msg) self.doStatus.status.level = DiagnosticStatus.OK self.doStatus.status.message = 'OK' self.doStatus.status.values = [ KeyValue(key='Dissolved Oxygen Saturation', value=str(self.do_msg.saturation)) ] self.doStatus.last_update = rospy.Time.now() # Get RTD sensor data and publish it self.temp_msg.header.stamp = rospy.Time.now() data = self.bus.get_data(self.temp_address) self.temp_msg.celsius = data[0] self.temp_msg.fahrenheit = self.temp_msg.celsius * 1.8 + 32.0 # Conversion to Fahrenheit self.tempPub.publish(self.temp_msg) self.tempStatus.status.level = DiagnosticStatus.OK self.tempStatus.status.message = 'OK' self.tempStatus.status.values = [ KeyValue(key='Temperature (deg C)', value=str(self.temp_msg.celsius)) ] self.tempStatus.last_update = rospy.Time.now() # Check for stale status self.ecStatus.check_stale(rospy.Time.now(), 35) self.orpStatus.check_stale(rospy.Time.now(), 35) self.pHStatus.check_stale(rospy.Time.now(), 35) self.doStatus.check_stale(rospy.Time.now(), 35) self.tempStatus.check_stale(rospy.Time.now(), 35) # Publish diagnostics message diag_msg = DiagnosticArray() diag_msg.status.append(self.ecStatus.status) diag_msg.status.append(self.orpStatus.status) diag_msg.status.append(self.pHStatus.status) diag_msg.status.append(self.doStatus.status) diag_msg.status.append(self.tempStatus.status) self.diag_pub.publish(diag_msg) # Sleep self.rate.sleep()
def dump_action(item_name, amount=1): params = [KeyValue('Item', item_name), KeyValue('Amount', str(amount))] return Action(action_type='dump', params=params, action_name='dump_' + item_name)
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)
def build_new_well_action(well_type): params = [KeyValue('Well', well_type)] return Action(action_type='build', params=params, action_name='build_new_well_' + well_type)
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"))
from actionlib_msgs.msg import GoalStatus from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal from diagnostic_msgs.msg import KeyValue if __name__ == '__main__': rospy.init_node('perceive_cavity_client_tester') client = SimpleActionClient('perceive_cavity_server', GenericExecuteAction) client.wait_for_server() goal = GenericExecuteGoal() if len(sys.argv) > 1: location = str(sys.argv[1]).upper() else: location = "PP01" goal.parameters.append(KeyValue(key='location', value=location)) rospy.loginfo('Sending following goal to perceive cavity server') rospy.loginfo(goal) client.send_goal(goal) timeout = 15.0 finished_within_time = client.wait_for_result(rospy.Duration.from_sec(int(timeout))) if not finished_within_time: client.cancel_goal() state = client.get_state() result = client.get_result() if state == GoalStatus.SUCCEEDED: rospy.loginfo('Action SUCCESS')
def ntp_monitor(offset=500, self_offset=500, diag_hostname=None, error_offset=5000000): pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=100) rospy.init_node(NAME, anonymous=True) hostname = socket.gethostname() if diag_hostname is None: diag_hostname = hostname ntp_hostname = rospy.get_param('~reference_host', 'ntp.ubuntu.com') offset = rospy.get_param('~offset_tolerance', 500) error_offset = rospy.get_param('~error_offset_tolerance', 5000000) stat = DiagnosticStatus() stat.level = 0 stat.name = "NTP offset from " + diag_hostname + " to " + ntp_hostname stat.message = "OK" stat.hardware_id = hostname stat.values = [] # self_stat = DiagnosticStatus() # self_stat.level = DiagnosticStatus.OK # self_stat.name = "NTP self-offset for "+ diag_hostname # self_stat.message = "OK" # self_stat.hardware_id = hostname # self_stat.values = [] while not rospy.is_shutdown(): for st, host, off in [(stat, ntp_hostname, offset)]: try: p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE) res = p.wait() (o, e) = p.communicate() except OSError, (errno, msg): if errno == 4: break #ctrl-c interrupt else: raise if (res == 0): measured_offset = float(re.search("offset (.*),", o).group(1)) * 1000000 st.level = DiagnosticStatus.OK st.message = "OK" st.values = [ KeyValue("Offset (us)", str(measured_offset)), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)) ] if (abs(measured_offset) > off): st.level = DiagnosticStatus.WARN st.message = "NTP Offset Too High" if (abs(measured_offset) > error_offset): st.level = DiagnosticStatus.ERROR st.message = "NTP Offset Too High" else: st.level = DiagnosticStatus.ERROR st.message = "Error Running ntpdate. Returned %d" % res st.values = [ KeyValue("Offset (us)", "N/A"), KeyValue("Offset tolerance (us)", str(off)), KeyValue("Offset tolerance (us) for Error", str(error_offset)), KeyValue("Output", o), KeyValue("Errors", e) ] msg = DiagnosticArray() msg.header.stamp = rospy.get_rostime() msg.status = [stat] pub.publish(msg) time.sleep(1)
def sendKeyVal(pub, key, val): kval = KeyValue() kval.key = key kval.value = val pub.publish(kval)
def make_srv_req_to_KB( knowledge_type_is_instance=True, instance_type="", instance_name="", attribute_name="", values=[], update_type_is_knowledge=True, ): """ make a service call to ``rosplan_knowledge_base`` (mongodb) :param knowledge_type_is_instance: type of knowledge :type knowledge_type_is_instance: bool :param instance_type: type of instance :type instance_type: str :param instance_name: name of instance :type instance_name: str :param attribute_name: name of attribute :type attribute_name: str :param values: values for attribute :type value: list (tuple (str, str)) :param update_type_is_knowledge: type of update to be performed :type update_type_is_knowledge: bool :return: success :rtype: bool example 1: **upload instance**: youbot has one ``dynamixel`` ``gripper``. .. code-block:: bash rosservice call /rosplan_knowledge_base/update "update_type: 0 knowledge: knowledge_type: 0 instance_type: 'gripper' instance_name: 'dynamixel' attribute_name: '' values: {} function_value: 0.0"; To perform the following using this function .. code-block:: python ProblemUploader.make_srv_req_to_KB(knowledge_type_is_instance=True, instance_type='gripper', instance_name='dynamixel') example 2: **upload fact**: object ``o4`` is on location ``S3`` .. code-block:: bash rosservice call /rosplan_knowledge_base/update "update_type: 0 knowledge: knowledge_type: 1 instance_type: '' instance_name: '' attribute_name: 'on' values: - {key: 'o', value: 'o4'} - {key: 'l', value: 'S3'} function_value: 0.0"; To perform the following using this function .. code-block:: python ProblemUploader.make_srv_req_to_KB(knowledge_type_is_instance=False, attribute_name='on', values=[('o', 'o4'), ('l', 'S3')]) """ msg = KnowledgeItem() if knowledge_type_is_instance: msg.knowledge_type = KnowledgeItem.INSTANCE msg.instance_type = instance_type msg.instance_name = instance_name.upper() else: msg.knowledge_type = KnowledgeItem.FACT msg.attribute_name = attribute_name msg.values = [KeyValue(i[0], i[1].upper()) for i in values] update_kb_topic = "/rosplan_knowledge_base/update" rospy.loginfo("Waiting for " + update_kb_topic) rospy.wait_for_service(update_kb_topic) try: update_kb = rospy.ServiceProxy(update_kb_topic, KnowledgeUpdateService) if update_type_is_knowledge: response = update_kb(Req.ADD_KNOWLEDGE, msg) else: response = update_kb(Req.ADD_GOAL, msg) except rospy.ServiceException as e: rospy.logerr("Service call failed: %s" % e) if response.success: rospy.loginfo("KB updated successfully.") return True else: rospy.logerror("KB update failed.") return False
from std_srvs.srv import * from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from time import sleep def callback(msg): print("Got msg") return EmptyResponse() if __name__ == '__main__': rospy.init_node("test_node") rospy.Service('self_test', Empty, callback) pub = rospy.Publisher('/diagnostics', DiagnosticArray) # Publish diagnostics to check runtime monitor, rxconsole msg = DiagnosticArray() stat = DiagnosticStatus() stat.level = 0 stat.name = 'Test Node' stat.message = 'OK' stat.hardware_id = 'HW ID' stat.values = [ KeyValue('Node Status', 'OK') ] msg.status.append(stat) while not rospy.is_shutdown(): pub.publish(msg) rospy.loginfo('Test node is printing things') sleep(1.0)
def check_mpstat(core_count=-1): vals = [] mp_level = DiagnosticStatus.OK load_dict = {0: 'OK', 1: 'High Load', 2: 'Error'} try: p = subprocess.Popen('mpstat -P ALL 1 1', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) stdout, stderr = p.communicate() retcode = p.returncode if retcode != 0: global has_warned_mpstat if not has_warned_mpstat: rospy.logerr( "mpstat failed to run for cpu_monitor. Return code %d.", retcode) has_warned_mpstat = True mp_level = DiagnosticStatus.ERROR vals.append( KeyValue(key='\"mpstat\" Call Error', value=str(retcode))) return mp_level, 'Unable to Check CPU Usage', vals # Check which column '%idle' is, #4539 # mpstat output changed between 8.06 and 8.1 rows = stdout.split('\n') col_names = rows[2].split() idle_col = -1 if (len(col_names) > 2 and col_names[-1] == '%idle') else -2 num_cores = 0 cores_loaded = 0 for index, row in enumerate(stdout.split('\n')): if index < 3: continue # Skip row containing 'all' data if row.find('all') > -1: continue lst = row.split() if len(lst) < 8: continue ## Ignore 'Average: ...' data if lst[0].startswith('Average'): continue cpu_name = '%d' % (num_cores) idle = lst[idle_col].replace(',', '.') user = lst[3].replace(',', '.') nice = lst[4].replace(',', '.') system = lst[5].replace(',', '.') core_level = 0 usage = float(user) + float(nice) if usage > 1000: # wrong reading, use old reading instead rospy.logwarn( 'Read cpu usage of %f percent. Reverting to previous reading of %f percent' % (usage, usage_old)) usage = usage_old usage_old = usage if usage > 90.0: cores_loaded += 1 core_level = DiagnosticStatus.WARN if usage > 110.0: core_level = DiagnosticStatus.ERROR vals.append( KeyValue(key='CPU %s Status' % cpu_name, value=load_dict[core_level])) vals.append(KeyValue(key='CPU %s User' % cpu_name, value=user)) vals.append(KeyValue(key='CPU %s Nice' % cpu_name, value=nice)) vals.append(KeyValue(key='CPU %s System' % cpu_name, value=system)) vals.append(KeyValue(key='CPU %s Idle' % cpu_name, value=idle)) num_cores += 1 # Warn for high load only if we have <= 2 cores that aren't loaded if num_cores - cores_loaded <= 2 and num_cores > 2: mp_level = DiagnosticStatus.WARN # Check the number of cores if core_count > 0, #4850 if core_count > 0 and core_count != num_cores: mp_level = DiagnosticStatus.ERROR global has_error_core_count if not has_error_core_count: rospy.logerr( 'Error checking number of cores. Expected %d, got %d. Computer may have not booted properly.', core_count, num_cores) has_error_core_count = True return DiagnosticStatus.ERROR, 'Incorrect number of CPU cores', vals except Exception, e: mp_level = DiagnosticStatus.ERROR vals.append(KeyValue(key='mpstat Exception', value=str(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)
imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2]*2.0 imuMsg.orientation.w = q[3] imuMsg.header.stamp= rospy.Time.now() imuMsg.header.frame_id = 'base_link' imuMsg.header.seq = seq seq = seq + 1 pub.publish(imuMsg) if (diag_pub_time < rospy.get_time()) : diag_pub_time += 1 diag_arr = DiagnosticArray() diag_arr.header.stamp = rospy.get_rostime() diag_arr.header.frame_id = '1' diag_msg = DiagnosticStatus() diag_msg.name = 'Razor_Imu' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received AHRS measurement' diag_msg.values.append(KeyValue('roll (deg)', str(roll*(180.0/math.pi)))) diag_msg.values.append(KeyValue('pitch (deg)', str(pitch*(180.0/math.pi)))) diag_msg.values.append(KeyValue('yaw (deg)', str(yaw*(180.0/math.pi)))) diag_msg.values.append(KeyValue('sequence number', str(seq))) diag_arr.status.append(diag_msg) diag_pub.publish(diag_arr) ser.close #f.close
def __init__(self): rospy.init_node('system_node', log_level=rospy.DEBUG) self.cpu_publisher = rospy.Publisher('~cpu', msgs.CPUUsage, queue_size=1) self.memory_publisher = rospy.Publisher('~memory', msgs.MemoryUsage, queue_size=1) self.disk_publisher = rospy.Publisher('~disk', msgs.DiskUsage, queue_size=1) self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10) self.drive_path = '/dev/root' self.rate = 1#float(rospy.get_param("~rate", 1)) # hertz rospy.loginfo('Ready') r = rospy.Rate(self.rate) while not rospy.is_shutdown(): # Find CPU. # rospy.loginfo('Finding CPU.') cpu_usage_percent = to_percent(getoutput( "grep 'cpu ' /proc/stat | " "awk '{usage=($2+$4)*100/($2+$4+$5)} END {print usage }'")) cpu_usage_percent_level = OK if cpu_usage_percent >= c.CPU_USAGE_PERCENT_ERROR: cpu_usage_percent_level = ERROR elif cpu_usage_percent >= c.CPU_USAGE_PERCENT_WARN: cpu_usage_percent_level = WARN msg = msgs.CPUUsage() msg.header.stamp = rospy.Time.now() msg.percent_used = cpu_usage_percent # print msg self.cpu_publisher.publish(msg) # Find memory. memory_usage_free_gbytes = to_float(getoutput( "free -m|grep -i 'Mem:'|awk '{print $4}'")) memory_usage_used_gbytes = to_float(getoutput( "free -m|grep -i 'Mem:'|awk '{print $3}'")) memory_usage_total_gbytes = to_float(getoutput( "free -m|grep -i 'Mem:'|awk '{print $2}'")) memory_usage_percent = -1 if memory_usage_used_gbytes != -1 and memory_usage_total_gbytes != -1: memory_usage_percent = memory_usage_used_gbytes/memory_usage_total_gbytes*100 memory_usage_percent_level = OK if memory_usage_percent >= c.MEMORY_USAGE_PERCENT_ERROR: memory_usage_percent_level = ERROR elif memory_usage_percent >= c.MEMORY_USAGE_PERCENT_WARN: memory_usage_percent_level = WARN msg = msgs.MemoryUsage() msg.header.stamp = rospy.Time.now() msg.used_gbytes = memory_usage_used_gbytes msg.free_gbytes = memory_usage_free_gbytes msg.total_gbytes = memory_usage_total_gbytes msg.percent_used = memory_usage_percent # print msg self.memory_publisher.publish(msg) # Find disk. disk_usage_percent = to_percent(getoutput( "df -H | grep -i "+self.drive_path+" | awk '{print $5}'")) disk_usage_free_gbytes = to_gbytes(getoutput( "df -H | grep -i "+self.drive_path+" | awk '{print $4}'")) disk_usage_used_gbytes = to_gbytes(getoutput( "df -H | grep -i "+self.drive_path+" | awk '{print $3}'")) disk_usage_total_gbytes = to_gbytes(getoutput( "df -H | grep -i "+self.drive_path+" | awk '{print $2}'")) disk_usage_level = OK if disk_usage_percent >= c.DISK_USAGE_PERCENT_ERROR: disk_usage_level = ERROR if disk_usage_percent >= c.DISK_USAGE_PERCENT_WARN: disk_usage_level = WARN msg = msgs.DiskUsage() msg.header.stamp = rospy.Time.now() msg.used_gbytes = disk_usage_used_gbytes msg.free_gbytes = disk_usage_free_gbytes msg.total_gbytes = disk_usage_total_gbytes msg.percent_used = disk_usage_percent # print msg self.disk_publisher.publish(msg) # Find CPU clock speed. min_ghz = get_cpu_clock_speed_min()/1000./1000. max_ghz = get_cpu_clock_speed_max()/1000./1000. cpu_clock_speed = get_cpu_clock_speed() # print('cpu_clock_speed:', cpu_clock_speed) cpu_clock_speed_percent = get_cpu_clock_speed_percent() # print('cpu_clock_speed_percent:', cpu_clock_speed_percent) cpu_clock_speed_percent_level = OK if cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_ERROR: cpu_clock_speed_percent_level = ERROR elif cpu_clock_speed_percent <= c.CPU_CLOCK_SPEED_PERCENT_WARN: cpu_clock_speed_percent_level = WARN # Find CPU temperature. cpu_temp = get_cpu_temp() cpu_temp_level = OK if cpu_temp >= c.CPU_TEMP_ERROR: cpu_temp_level = ERROR elif cpu_temp >= c.CPU_TEMP_WARN: cpu_temp_level = WARN # Publish standard diagnostics. array = DiagnosticArray() cpu_temperature_status = DiagnosticStatus( name='CPU Temperature', level=cpu_temp_level) cpu_temperature_status.values = [ KeyValue(key='celcius', value=str(cpu_temp)), ] cpu_usage_status = DiagnosticStatus( name='CPU Usage', level=cpu_usage_percent_level) cpu_usage_status.values = [ KeyValue(key='percent', value=str(cpu_usage_percent)), ] cpu_clock_speed_status = DiagnosticStatus( name='CPU Speed', level=cpu_clock_speed_percent_level) cpu_clock_speed_status.values = [ KeyValue(key='clock speed (GHz)', value=str(cpu_clock_speed)), KeyValue(key='min clock speed (GHz)', value=str(min_ghz)), KeyValue(key='max clock speed (GHz)', value=str(max_ghz)), KeyValue(key='clock speed (percent)', value=str(cpu_clock_speed_percent)), ] disk_usage_status = DiagnosticStatus( name='Disk Usage', level=disk_usage_level) disk_usage_status.values = [ KeyValue(key='percent', value=str(disk_usage_percent)), KeyValue(key='free gb', value=str(disk_usage_free_gbytes)), KeyValue(key='used gb', value=str(disk_usage_used_gbytes)), KeyValue(key='total gb', value=str(disk_usage_total_gbytes)), ] memory_usage_status = DiagnosticStatus( name='Memory Usage', level=memory_usage_percent_level) memory_usage_status.values = [ KeyValue(key='percent', value=str(memory_usage_percent)), KeyValue(key='free gb', value=str(memory_usage_free_gbytes)), KeyValue(key='used gb', value=str(memory_usage_used_gbytes)), KeyValue(key='total gb', value=str(memory_usage_total_gbytes)), ] array.status = [ cpu_temperature_status, cpu_usage_status, cpu_clock_speed_status, disk_usage_status, memory_usage_status, ] self.diagnostics_pub.publish(array) r.sleep()
from diagnostic_msgs.msg import KeyValue 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