Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 21 additions & 32 deletions diagnostic_updater/diagnostic_updater/_diagnostic_updater.py
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ def __init__(self, name):
"""Construct a DiagnosticTask setting its name in the process."""
self.name = name

def getName(self):
def get_name(self):
"""Return the name of the DiagnosticTask."""
return self.name

Expand Down Expand Up @@ -133,7 +133,7 @@ def run(self, stat):
stat.summary(combined_summary)
return stat

def addTask(self, t):
def add_task(self, t):
"""
Add a child CompositeDiagnosticTask.

Expand Down Expand Up @@ -171,7 +171,7 @@ def __init__(self):
self.tasks = []
self.lock = threading.Lock()

def addedTaskCallback(self, task):
def added_task_callback(self, task):
"""
Allow an action to be taken when a task is added.

Expand All @@ -195,12 +195,15 @@ def add(self, *args):
elif len(args) == 2:
task = DiagnosticTaskVector.DiagnosticTaskInternal(
args[0], args[1])
else:
raise TypeError(
f'add() takes 1 or 2 arguments ({len(args)} given)')

with self.lock:
self.tasks.append(task)
self.addedTaskCallback(task)
self.added_task_callback(task)

def removeByName(self, name):
def remove_by_name(self, name):
"""
Remove a task based on its name.

Expand All @@ -211,8 +214,8 @@ def removeByName(self, name):
"""
found = False
with self.lock:
for i in range(len(self.tasks)):
if self.tasks[i].name == name:
for i, task in enumerate(self.tasks):
if task.name == name:
self.tasks.pop(i)
found = True
break
Expand Down Expand Up @@ -244,7 +247,7 @@ def __init__(self, node, period=1.0):
self.timer = self.node.create_timer(self.__period, self.update)

self.verbose = False
self.hwid = ''
self.hw_id = ''
self.warn_nohwid_done = False

self.use_fqn_parameter = 'diagnostic_updater.use_fqn'
Expand All @@ -256,7 +259,8 @@ def __init__(self, node, period=1.0):
self.use_fqn_parameter, False).value

if self.__use_fqn:
self.node_name = '/'.join([self.node.get_namespace(), self.node.get_name()])
self.node_name = '/'.join([self.node.get_namespace(),
self.node.get_name()])
else:
self.node_name = self.node.get_name()

Expand All @@ -267,7 +271,7 @@ def update(self):
Causes the diagnostics to update if the inter-update interval has
been exceeded.
"""
warn_nohwid = len(self.hwid) == 0
warn_nohwid = len(self.hw_id) == 0

status_vec = []

Expand All @@ -278,7 +282,7 @@ def update(self):
status.level = DiagnosticStatus.ERROR
status.name = task.name
status.message = 'No message was set'
status.hardware_id = self.hwid
status.hardware_id = self.hw_id

status = task.run(status)

Expand All @@ -289,9 +293,8 @@ def update(self):

if self.verbose and status.level != b'\x00':
self.node.get_logger().warn(
'Non-zero diagnostic status. Name: %s, status\
%s: %s' % (status.name, str(status.level),
status.message))
f'Non-zero diagnostics status. Name: {status.name}, ' +
f'status: {status.level}: {status.message}')

if warn_nohwid and not self.warn_nohwid_done:
self.node.get_logger().warn(
Expand Down Expand Up @@ -338,27 +341,13 @@ def broadcast(self, lvl, msg):

self.publish(status_vec)

def setHardwareID(self, hwid):
def set_hw_id(self, hw_id):
"""Set the hardware ID for all the diagnostics."""
self.hwid = hwid

# TODO(Karsten1987) Re-enable this for eloquent
# def _check_diagnostic_period(self):
# """Recheck the diagnostic_period on the parameter server."""
# # This was getParamCached() call in the cpp code. i.e. it would
# # throttle the actual call to the parameter server using a
# # notification of change mechanism.
# # This is not available in rospy. Hence I throttle the call to the
# # parameter server using a standard timeout mechanism (4Hz)
# now = self.clock.now()
# if now >= self.last_time_period_checked:
# # self.period = self.node.get_parameter(
# self.period_parameter).value
# self.last_time_period_checked = now
self.hw_id = hw_id

def publish(self, msg):
"""Publish a single or a vector of diagnostic statuses."""
if not type(msg) is list:
if not isinstance(msg, list):
msg = [msg]

now = self.node.get_clock().now()
Expand All @@ -375,7 +364,7 @@ def publish(self, msg):
da.status.append(db)
self.publisher.publish(da)

def addedTaskCallback(self, task):
def added_task_callback(self, task):
"""Publish a task (called when added to the updater)."""
stat = DiagnosticStatusWrapper()
stat.name = task.name
Expand Down
4 changes: 2 additions & 2 deletions diagnostic_updater/diagnostic_updater/_publisher.py
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ def __init__(self, name, diag, freq):
CompositeDiagnosticTask.__init__(self, name + ' topic status')
self.diag = diag
self.freq = FrequencyStatus(freq)
self.addTask(self.freq)
self.add_task(self.freq)
self.diag.add(self)

def tick(self):
Expand Down Expand Up @@ -100,7 +100,7 @@ def __init__(self, name, diag, freq, stamp):
"""
HeaderlessTopicDiagnostic.__init__(self, name, diag, freq)
self.stamp = TimeStampStatus(stamp)
self.addTask(self.stamp)
self.add_task(self.stamp)

def tick(self, stamp):
"""
Expand Down
8 changes: 4 additions & 4 deletions diagnostic_updater/diagnostic_updater/example.py
Original file line number Diff line number Diff line change
Expand Up @@ -169,10 +169,10 @@ def main():
# DiagnosticTask that can be used to create a DiagnosticTask from
# a function. This will be useful when combining multiple diagnostic
# tasks using a CompositeDiagnosticTask.
lower = diagnostic_updater.FunctionDiagnosticTask('Lower-bound check',
check_lower_bound)
upper = diagnostic_updater.FunctionDiagnosticTask('Upper-bound check',
check_upper_bound)
lower = diagnostic_updater.FunctionDiagnosticTask(
'Lower-bound check', check_lower_bound)
upper = diagnostic_updater.FunctionDiagnosticTask(
'Upper-bound check', check_upper_bound)

# If you want to merge the outputs of two diagnostic tasks together, you
# can create a CompositeDiagnosticTask, also a derived class from
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ typedef std::function<void (diagnostic_msgs::msg::DiagnosticStatus &)>
*
* Subclasses are provided for generating common diagnostic information.
*
* A DiagnosticTask has a name, and a function that is called to cleate a
* A DiagnosticTask has a name, and a function that is called to create a
* DiagnosticStatusWrapper.
*/
class DiagnosticTask
Expand Down
1 change: 1 addition & 0 deletions diagnostic_updater/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>launch</test_depend>
Expand Down
25 changes: 25 additions & 0 deletions diagnostic_updater/test/test_flake8.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from ament_flake8.main import main_with_errors
import pytest


@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
Loading