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
