From e7902cc8807b66aa40596bdee6ed60f95eb42eec Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 15 Jul 2022 11:37:03 +0900 Subject: [PATCH 01/39] [jsk_topic_tools/HzMeasureNodelet] Modified to measure hz in the updateDiagnostics function. The hz calculation will be correct even if the topic does not come. --- .../jsk_topic_tools/hz_measure_nodelet.h | 1 - jsk_topic_tools/src/hz_measure_nodelet.cpp | 28 +++++++++++++------ 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h index 2fa37d12b..b8cd8ad97 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h @@ -52,7 +52,6 @@ namespace jsk_topic_tools virtual void onInit(); protected: int average_message_num_; - double hz_; double warning_hz_; std::queue buffer_; ros::Publisher hz_pub_; diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 489df6d9a..cb3d77223 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -44,7 +44,6 @@ namespace jsk_topic_tools void HzMeasure::onInit() { pnh_ = getPrivateNodeHandle(); - hz_ = std::numeric_limits::max(); if (!pnh_.getParam("message_num", average_message_num_)) { average_message_num_ = 10; // defaults to 10 } @@ -82,7 +81,6 @@ namespace jsk_topic_tools double whole_time = (now - oldest).toSec(); double average_time = whole_time / (buffer_.size() - 1); std_msgs::Float32 output; - hz_ = 1.0 / average_time; output.data = 1.0 / average_time; hz_pub_.publish(output); buffer_.pop(); @@ -95,14 +93,28 @@ namespace jsk_topic_tools void HzMeasure::updateDiagnostic( diagnostic_updater::DiagnosticStatusWrapper &stat) { - if (hz_ > warning_hz_) { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, - (boost::format("%s is running at %.2f hz.") - % getName() % hz_).str()); + double hz = -1.0; + if (buffer_.size() > average_message_num_) { + ros::Time now = ros::Time::now(); + ros::Time oldest = buffer_.front(); + double whole_time = (now - oldest).toSec(); + double average_time = whole_time / (buffer_.size() - 1); + hz = 1.0 / average_time; + } + if (hz > 0.0) { + if (hz > warning_hz_) { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, + (boost::format("%s is running at %.2f hz.") + % getName() % hz).str()); + } else { + stat.summary(diagnostic_error_level_, + (boost::format("%s is running at %.2f hz.") + % getName() % hz).str()); + } } else { stat.summary(diagnostic_error_level_, - (boost::format("%s is running at %.2f hz.") - % getName() % hz_).str()); + (boost::format("%s is waiting input topic.") + % getName()).str()); } } } From 7e353574e0303b344e7c3ca5e4db805e0c8273b4 Mon Sep 17 00:00:00 2001 From: iory Date: Tue, 23 Aug 2022 20:25:20 +0900 Subject: [PATCH 02/39] [jsk_topic_tools] Eval at every timer callback --- doc/jsk_topic_tools/scripts/boolean_node.rst | 7 +++++++ jsk_topic_tools/scripts/boolean_node.py | 12 +++++++----- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/doc/jsk_topic_tools/scripts/boolean_node.rst b/doc/jsk_topic_tools/scripts/boolean_node.rst index 262583f7c..b320be1e5 100644 --- a/doc/jsk_topic_tools/scripts/boolean_node.rst +++ b/doc/jsk_topic_tools/scripts/boolean_node.rst @@ -66,6 +66,13 @@ Parameters input1_condition: m.header.frame_id in ['base', 'base_link'] + Note that this condition is evaluated each time a topic is published. For example, a condition that checks whether a certain topic has arrived within one second look like this. + + .. code-block:: bash + + input1_condition: "(rospy.Time.now() - t).to_sec() < 1.0" + + * ``~rate`` (Int, Default: ``100``) Publishing rate [Hz]. diff --git a/jsk_topic_tools/scripts/boolean_node.py b/jsk_topic_tools/scripts/boolean_node.py index e93b67334..ab8d8e371 100755 --- a/jsk_topic_tools/scripts/boolean_node.py +++ b/jsk_topic_tools/scripts/boolean_node.py @@ -78,18 +78,20 @@ def callback(self, topic_name, msg): lambda msg, tn=topic_name: self.callback(tn, msg)) self.subs[topic_name] = deserialized_sub return - filter_fn = self.filter_fns[topic_name] - result = filter_fn(topic_name, msg, rospy.Time.now()) - self.data[topic_name] = result + self.data[topic_name] = topic_name, msg, rospy.Time.now() def timer_cb(self, timer): if len(self.data) != self.n_input: return + eval_values = [] + for topic_name, msg, received_time in self.data.values(): + filter_fn = self.filter_fns[topic_name] + eval_values.append(filter_fn(topic_name, msg, received_time)) for op_str, pub in self.pubs.items(): if op_str == 'not': - flag = not list(self.data.values())[0] + flag = not list(eval_values)[0] else: - flag = reduce(OPERATORS[op_str], self.data.values()) + flag = reduce(OPERATORS[op_str], eval_values) pub.publish(std_msgs.msg.Bool(flag)) From 7435a088726cab9c37e6b6fa3ee057b387094b70 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 14:28:20 +0900 Subject: [PATCH 03/39] [jsk_topic_tools/boolean_node] Add escape sequence descriptions --- doc/jsk_topic_tools/scripts/boolean_node.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/jsk_topic_tools/scripts/boolean_node.rst b/doc/jsk_topic_tools/scripts/boolean_node.rst index b320be1e5..56ce87766 100644 --- a/doc/jsk_topic_tools/scripts/boolean_node.rst +++ b/doc/jsk_topic_tools/scripts/boolean_node.rst @@ -72,6 +72,8 @@ Parameters input1_condition: "(rospy.Time.now() - t).to_sec() < 1.0" + Use escape sequence when using the following symbols <(``<``), >(``>``), &(``&``), '(``'``) and "(``"``) in launch file. + * ``~rate`` (Int, Default: ``100``) From 77c14cbc3fa118cd82be51f1aa0a83d5cfdd9a1a Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 14:42:13 +0900 Subject: [PATCH 04/39] [jsk_topic_tools] Add eval_utils module to eval Python expression --- .../src/jsk_topic_tools/eval_utils.py | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 jsk_topic_tools/src/jsk_topic_tools/eval_utils.py diff --git a/jsk_topic_tools/src/jsk_topic_tools/eval_utils.py b/jsk_topic_tools/src/jsk_topic_tools/eval_utils.py new file mode 100644 index 000000000..eac24ac5a --- /dev/null +++ b/jsk_topic_tools/src/jsk_topic_tools/eval_utils.py @@ -0,0 +1,22 @@ +from importlib import import_module + +import rospy + + +def expr_eval(expr, modules=None): + modules = modules or {} + def eval_fn(topic, m, t): + return eval(expr, modules, {'m': m, 'topic': topic, 't': t}) + return eval_fn + + +def import_modules(import_list): + modules = {} + for module in import_list: + try: + mod = import_module(module) + except ImportError: + rospy.logerr('Failed to import module: %s' % module) + else: + modules[module] = mod + return modules From 449197bd7fe6e883d08882424bf8ada9c1cdf308 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 14:46:15 +0900 Subject: [PATCH 05/39] [jsk_tools/test_topic_published] Use eval_utils module and enabled import moduels for input_condition --- .../test_nodes/test_topic_published.rst | 9 +++++++++ jsk_tools/src/test_topic_published.py | 18 +++++++++++------- jsk_tools/test/test_topic_published.launch | 18 ++++++++++++++++++ 3 files changed, 38 insertions(+), 7 deletions(-) diff --git a/doc/jsk_tools/test_nodes/test_topic_published.rst b/doc/jsk_tools/test_nodes/test_topic_published.rst index e5590083a..260e17bec 100644 --- a/doc/jsk_tools/test_nodes/test_topic_published.rst +++ b/doc/jsk_tools/test_nodes/test_topic_published.rst @@ -22,6 +22,11 @@ Parameters If it's ``True``, it tests if **not** published. +- ``~import`` (``List[str]``, Default: ``[]``, Optional) + + List of Python modules to import and use in ``~condition_%d`` expression. + For commodity rospy and numpy modules are imported by default. + - ``~condition_%d`` (``str``, Default: ``None``, Optional) Check bool value condition using the given Python expression. @@ -43,6 +48,10 @@ Parameters .. code-block:: bash condition_0: m.data < 'spbm' + The modules given to ``~import`` can also be used. + + .. code-block:: bash + condition_0: 'numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) < 10.0' Example ------- diff --git a/jsk_tools/src/test_topic_published.py b/jsk_tools/src/test_topic_published.py index 41b12bd21..55d293bbc 100755 --- a/jsk_tools/src/test_topic_published.py +++ b/jsk_tools/src/test_topic_published.py @@ -15,20 +15,19 @@ NAME = 'test_topic_published' -def expr_eval(expr): - def eval_fn(topic, m, t): - return eval(expr) - return eval_fn +from jsk_topic_tools.eval_utils import expr_eval +from jsk_topic_tools.eval_utils import import_modules class PublishChecker(object): - def __init__(self, topic_name, timeout, condition=None): + def __init__(self, topic_name, timeout, condition=None, modules=None): + modules = modules or {} self.topic_name = topic_name self.deadline = rospy.Time.now() + rospy.Duration(timeout) msg_class, _, _ = rostopic.get_topic_class( rospy.resolve_name(topic_name), blocking=True) if condition is not None: - self.condition_expr = expr_eval(condition) + self.condition_expr = expr_eval(condition, modules) self.condition = condition self.msg = None self.invalid_conditioned_msg = None @@ -60,6 +59,11 @@ def __init__(self, *args): self.timeouts = [] self.negatives = [] self.conditions = [] + + import_list = rospy.get_param('~import', []) + import_list += ['rospy', 'numpy'] + self.modules = import_modules(import_list) + params = rospy.get_param(rospy.get_name(), []) for name, value in params.items(): if not re.match(r'^topic_\d$', name): @@ -112,7 +116,7 @@ def _check_topic_pubilshed(self): checkers = [] for topic_name, timeout, negative, condition in zip( self.topics, self.timeouts, self.negatives, self.conditions): - checker = PublishChecker(topic_name, timeout, condition) + checker = PublishChecker(topic_name, timeout, condition, self.modules) checkers.append(checker) topics_finished = [] diff --git a/jsk_tools/test/test_topic_published.launch b/jsk_tools/test/test_topic_published.launch index 905af4c00..458bbc657 100644 --- a/jsk_tools/test/test_topic_published.launch +++ b/jsk_tools/test/test_topic_published.launch @@ -92,4 +92,22 @@ + + + + + + + topic_0: int + timeout_0: 10 + condition_0: "numpy.sum(numpy.arange(m.data)) == 45" + import: [numpy,] + + + From b3c1ae95700e1d482e49ccc7ae516c7ef6d782bf Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 16:42:06 +0900 Subject: [PATCH 06/39] [jsk_topic_tools/boolean_node] Use eval_utils's expr_eval and enable import options --- doc/jsk_topic_tools/scripts/boolean_node.rst | 10 +++++++ .../sample/sample_boolean_node.launch | 26 +++++++++++++++++++ jsk_topic_tools/scripts/boolean_node.py | 15 ++++++----- jsk_topic_tools/test/test_boolean_node.test | 6 +++++ 4 files changed, 50 insertions(+), 7 deletions(-) diff --git a/doc/jsk_topic_tools/scripts/boolean_node.rst b/doc/jsk_topic_tools/scripts/boolean_node.rst index 262583f7c..d7a1d1c3d 100644 --- a/doc/jsk_topic_tools/scripts/boolean_node.rst +++ b/doc/jsk_topic_tools/scripts/boolean_node.rst @@ -45,6 +45,11 @@ Publishing Topics Parameters ---------- +- ``~import`` (``List[str]``, Default: ``[]``, Optional) + + List of Python modules to import and use in ``~input{%d}_condition`` expression. + For commodity rospy and numpy modules are imported by default. + * ``~input{%d}_condition`` (String, Default: ``m.data``) @@ -65,6 +70,11 @@ Parameters input1_condition: m.header.frame_id in ['base', 'base_link'] + The modules given to ``~import`` can also be used. + + .. code-block:: bash + condition_0: 'numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) < 10.0' + * ``~rate`` (Int, Default: ``100``) diff --git a/jsk_topic_tools/sample/sample_boolean_node.launch b/jsk_topic_tools/sample/sample_boolean_node.launch index 630e6714d..8c8eca0f5 100644 --- a/jsk_topic_tools/sample/sample_boolean_node.launch +++ b/jsk_topic_tools/sample/sample_boolean_node.launch @@ -48,4 +48,30 @@ + + + + + + number_of_input: 2 + input1_condition: "'base' in m.header.frame_id" + input2_condition: "topic == '/chatter'" + + + + + + + + + number_of_input: 2 + input1_condition: "'base' in m.header.frame_id" + input2_condition: "(rospy.Time.now() - t).to_sec() < 10.0" + + + diff --git a/jsk_topic_tools/scripts/boolean_node.py b/jsk_topic_tools/scripts/boolean_node.py index e93b67334..0a01249a8 100755 --- a/jsk_topic_tools/scripts/boolean_node.py +++ b/jsk_topic_tools/scripts/boolean_node.py @@ -9,6 +9,9 @@ import rospy import std_msgs.msg +from jsk_topic_tools.eval_utils import expr_eval +from jsk_topic_tools.eval_utils import import_modules + OPERATORS = { 'or': lambda a, b: a or b, @@ -18,12 +21,6 @@ } -def expr_eval(expr): - def eval_fn(topic, m, t): - return eval(expr) - return eval_fn - - class BooleanNode(object): def __init__(self): @@ -33,6 +30,10 @@ def __init__(self): rospy.logerr('~number_of_input should be greater than 0.') sys.exit(1) + import_list = rospy.get_param('~import', []) + import_list += ['rospy', 'numpy'] + self.modules = import_modules(import_list) + self.pubs = {} if self.n_input == 1: self.pubs['not'] = rospy.Publisher( @@ -52,7 +53,7 @@ def __init__(self): condition = rospy.get_param( '{}_condition'.format(topic_name), 'm.data == True') topic_name = rospy.resolve_name(topic_name) - self.filter_fns[topic_name] = expr_eval(condition) + self.filter_fns[topic_name] = expr_eval(condition, self.modules) sub = rospy.Subscriber( topic_name, rospy.AnyMsg, diff --git a/jsk_topic_tools/test/test_boolean_node.test b/jsk_topic_tools/test/test_boolean_node.test index aa3fe495b..92bc2021f 100644 --- a/jsk_topic_tools/test/test_boolean_node.test +++ b/jsk_topic_tools/test/test_boolean_node.test @@ -15,6 +15,12 @@ timeout_2: 10 topic_3: /robot_is_not_speaking timeout_3: 10 + topic_4: /boolean_node_to_check_topic_expression/output/and + timeout_4: 10 + condition_4: "m.data is True" + topic_5: /boolean_node_to_check_t_expression/output/and + timeout_5: 10 + condition_5: "m.data is True" From af8a413fdc21ca5e81bb771d5056e66369c340c2 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 18:18:46 +0900 Subject: [PATCH 07/39] [jsk_tools/test_topic_published] Fixed document's code block by adding newline --- doc/jsk_tools/test_nodes/test_topic_published.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/jsk_tools/test_nodes/test_topic_published.rst b/doc/jsk_tools/test_nodes/test_topic_published.rst index 260e17bec..05a07239a 100644 --- a/doc/jsk_tools/test_nodes/test_topic_published.rst +++ b/doc/jsk_tools/test_nodes/test_topic_published.rst @@ -36,21 +36,25 @@ Parameters For example, topic is ``std_msgs/String`` and if you want to check whether a sentence is a ``hello``, you can do the following. .. code-block:: bash + condition_0: m.data == 'hello' If you want to check the frame id of the header, you can do the following. .. code-block:: bash + condition_0: m.header.frame_id in ['base', 'base_link'] Note that, use escape sequence when using the following symbols ``<(<)``, ``>(>)``, ``&(&)``, ``'(')`` and ``"(")``. .. code-block:: bash + condition_0: m.data < 'spbm' The modules given to ``~import`` can also be used. .. code-block:: bash + condition_0: 'numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) < 10.0' Example From 0adc74a6c0ea7ff40b29c73adcb8fed279f85dfe Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 18:19:37 +0900 Subject: [PATCH 08/39] [jsk_topic_tools/boolean_node.py] Fixed document's code block by adding newline --- doc/jsk_topic_tools/scripts/boolean_node.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/jsk_topic_tools/scripts/boolean_node.rst b/doc/jsk_topic_tools/scripts/boolean_node.rst index d7a1d1c3d..d8ba0b382 100644 --- a/doc/jsk_topic_tools/scripts/boolean_node.rst +++ b/doc/jsk_topic_tools/scripts/boolean_node.rst @@ -73,6 +73,7 @@ Parameters The modules given to ``~import`` can also be used. .. code-block:: bash + condition_0: 'numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) < 10.0' From 45ef39de1f0d1a08c807e7607b73ea5497aac7d5 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 20:35:21 +0900 Subject: [PATCH 09/39] [jsk_tools/test_topic_published] Fixed E402 (module level import not at top of file) error --- jsk_tools/src/test_topic_published.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/jsk_tools/src/test_topic_published.py b/jsk_tools/src/test_topic_published.py index 55d293bbc..1e3e82a34 100755 --- a/jsk_tools/src/test_topic_published.py +++ b/jsk_tools/src/test_topic_published.py @@ -10,15 +10,14 @@ import rospy import rostopic +from jsk_topic_tools.eval_utils import expr_eval +from jsk_topic_tools.eval_utils import import_modules + PKG = 'jsk_tools' NAME = 'test_topic_published' -from jsk_topic_tools.eval_utils import expr_eval -from jsk_topic_tools.eval_utils import import_modules - - class PublishChecker(object): def __init__(self, topic_name, timeout, condition=None, modules=None): modules = modules or {} From 1f5491d2110f7d76893becaa43bd0da0c413f9de Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 20:57:26 +0900 Subject: [PATCH 10/39] [jsk_topic_tools/HzMeasureNodelet] Fixed calculating hz condition --- jsk_topic_tools/src/hz_measure_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index cb3d77223..c68c04cc6 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -94,7 +94,7 @@ namespace jsk_topic_tools diagnostic_updater::DiagnosticStatusWrapper &stat) { double hz = -1.0; - if (buffer_.size() > average_message_num_) { + if (buffer_.size() >= average_message_num_) { ros::Time now = ros::Time::now(); ros::Time oldest = buffer_.front(); double whole_time = (now - oldest).toSec(); From 40815e5d116707058014439eeb5a41e15274fada Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 20:58:07 +0900 Subject: [PATCH 11/39] [jsk_topic_tools/HzMeasureNodelet] Fixed calculation of average_time (buffer_.size() - 1) to buffer_.size() --- jsk_topic_tools/src/hz_measure_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index c68c04cc6..5aceb6432 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -98,7 +98,7 @@ namespace jsk_topic_tools ros::Time now = ros::Time::now(); ros::Time oldest = buffer_.front(); double whole_time = (now - oldest).toSec(); - double average_time = whole_time / (buffer_.size() - 1); + double average_time = whole_time / buffer_.size(); hz = 1.0 / average_time; } if (hz > 0.0) { From b2f156adc9c63caec612961f8f3c355dd5eae2ce Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 22:43:20 +0900 Subject: [PATCH 12/39] [jsk_topic_tools/HzMeasureNodelet] Enable ~measure_time param to calculate `hz` from the number of topics received in time in `~measure_time` --- .../class/hz_measure_nodelet.md | 8 +- .../jsk_topic_tools/hz_measure_nodelet.h | 5 ++ jsk_topic_tools/src/hz_measure_nodelet.cpp | 75 ++++++++++++++----- 3 files changed, 68 insertions(+), 20 deletions(-) diff --git a/doc/jsk_topic_tools/class/hz_measure_nodelet.md b/doc/jsk_topic_tools/class/hz_measure_nodelet.md index d95c3508b..002271c77 100644 --- a/doc/jsk_topic_tools/class/hz_measure_nodelet.md +++ b/doc/jsk_topic_tools/class/hz_measure_nodelet.md @@ -26,9 +26,13 @@ If target `hz` is smaller than `~warning_hz`, this node outputs `diagnostics` at Diagnostic messages. ## Parameter -- `~message_num` (`Int`, default: `10`): +- `~measure_time` (`Double`, default: `1.0`): - Calculate `hz` from the arrival times of `~message_num` topics. + Calculate `hz` from the number of topics received in time in `~measure_time`. + +- `~message_num` (`Int`, default: `-1`): + + Calculate `hz` from the arrival times of `~message_num` topics. Note that if this value is less than 0 or not set, `~measure_time` will be used. - `~warning_hz` (`Double`, default: `-1`): diff --git a/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h index b8cd8ad97..efbc7ee5c 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h @@ -38,6 +38,7 @@ #include #include +#include #include "jsk_topic_tools/timered_diagnostic_updater.h" @@ -52,11 +53,15 @@ namespace jsk_topic_tools virtual void onInit(); protected: int average_message_num_; + double measure_time_; double warning_hz_; std::queue buffer_; ros::Publisher hz_pub_; ros::Subscriber sub_; ros::NodeHandle pnh_; + boost::mutex mutex_; + virtual void popBufferQueue(); + virtual double calculateHz(); virtual void inputCallback(const boost::shared_ptr& msg); /** @brief diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 5aceb6432..3591fa274 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -44,8 +44,24 @@ namespace jsk_topic_tools void HzMeasure::onInit() { pnh_ = getPrivateNodeHandle(); - if (!pnh_.getParam("message_num", average_message_num_)) { - average_message_num_ = 10; // defaults to 10 + if (pnh_.getParam("message_num", average_message_num_) && + pnh_.getParam("measure_time", measure_time_)) { + NODELET_WARN("Both ~measure_time and ~message_num are given. Prioritize ~measure_time."); + average_message_num_ = -1.0; + } + else if (pnh_.getParam("measure_time", measure_time_)) { + average_message_num_ = -1.0; // disable average_message_num_ + if (measure_time_ < 0.0) { + NODELET_ERROR("~measure_time should be greater than 0.0. Set 1.0 as default."); + measure_time_ = 1.0; + } + } + else if (pnh_.getParam("message_num", average_message_num_)) { + measure_time_ = -1.0; // diable measure_time_ + if (average_message_num_ < 0) { + NODELET_ERROR("~message_num should be greater than 0. Set 10 as default."); + average_message_num_ = 10; + } } if (!pnh_.getParam("warning_hz", warning_hz_)) { warning_hz_ = -1; @@ -72,20 +88,48 @@ namespace jsk_topic_tools &HzMeasure::inputCallback, this); } + void HzMeasure::popBufferQueue() { + ros::Time now = ros::Time::now(); + while (!buffer_.empty() + && ((measure_time_ > 0 && measure_time_ < (now - buffer_.front()).toSec()) + || (average_message_num_ > 0 && average_message_num_ < buffer_.size()))) { + buffer_.pop(); + } + } + + double HzMeasure::calculateHz() { + double hz = -1.0; + if (average_message_num_ > 0) { + if (buffer_.size() == average_message_num_) { + ros::Time now = ros::Time::now(); + ros::Time oldest = buffer_.front(); + double whole_time = (now - oldest).toSec(); + double average_time = whole_time / buffer_.size(); + hz = 1.0 / average_time; + } + } else { + double time_width = measure_time_; + if (buffer_.size() > 0) { + ros::Time now = ros::Time::now(); + time_width = std::min(measure_time_, std::max((now - buffer_.front()).toSec(), 0.0001)); + } + hz = (buffer_.size() - 1) / time_width; + } + return hz; + } + void HzMeasure::inputCallback(const boost::shared_ptr& msg) { + boost::mutex::scoped_lock lock(mutex_); + popBufferQueue(); ros::Time now = ros::Time::now(); buffer_.push(now); - if (buffer_.size() > average_message_num_) { - ros::Time oldest = buffer_.front(); - double whole_time = (now - oldest).toSec(); - double average_time = whole_time / (buffer_.size() - 1); + double hz = calculateHz(); + if (hz > 0.0) { std_msgs::Float32 output; - output.data = 1.0 / average_time; + output.data = hz; hz_pub_.publish(output); - buffer_.pop(); - } - else { + } else { NODELET_DEBUG("there is no enough messages yet"); } } @@ -93,14 +137,9 @@ namespace jsk_topic_tools void HzMeasure::updateDiagnostic( diagnostic_updater::DiagnosticStatusWrapper &stat) { - double hz = -1.0; - if (buffer_.size() >= average_message_num_) { - ros::Time now = ros::Time::now(); - ros::Time oldest = buffer_.front(); - double whole_time = (now - oldest).toSec(); - double average_time = whole_time / buffer_.size(); - hz = 1.0 / average_time; - } + boost::mutex::scoped_lock lock(mutex_); + popBufferQueue(); + double hz = calculateHz(); if (hz > 0.0) { if (hz > warning_hz_) { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, From f43468bea4eec41ef75bd497c753e77c5cb8fa8e Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 25 Aug 2022 22:44:11 +0900 Subject: [PATCH 13/39] [jsk_topic_tools/HzMeasureNodelet] Add test for ~measure_time and ~message_num --- jsk_topic_tools/test/test_hz_measure.test | 52 ++++++++++++++++++----- 1 file changed, 41 insertions(+), 11 deletions(-) diff --git a/jsk_topic_tools/test/test_hz_measure.test b/jsk_topic_tools/test/test_hz_measure.test index 6ff6208f2..c1de2b1d7 100644 --- a/jsk_topic_tools/test/test_hz_measure.test +++ b/jsk_topic_tools/test/test_hz_measure.test @@ -3,20 +3,50 @@ type="nodelet" name="nodelet_manager" args="manager" /> - - - + + + + + + message_num: 300 + + + + + + + + + + + + + measure_time: 1.0 + + + + + + + + - From a44168d5faabe204096bdf962b3b073f07bc7f1a Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 26 Aug 2022 00:02:56 +0900 Subject: [PATCH 14/39] [jsk_topic_tools/HzMeasureNodelet] Fixed timing of scoped_lock to publish /diagnostics. Also, added test for diagnostics --- jsk_topic_tools/src/hz_measure_nodelet.cpp | 4 ++-- jsk_topic_tools/test/test_hz_measure.test | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 3591fa274..9e01ea6c1 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -89,6 +89,7 @@ namespace jsk_topic_tools } void HzMeasure::popBufferQueue() { + boost::mutex::scoped_lock lock(mutex_); ros::Time now = ros::Time::now(); while (!buffer_.empty() && ((measure_time_ > 0 && measure_time_ < (now - buffer_.front()).toSec()) @@ -98,6 +99,7 @@ namespace jsk_topic_tools } double HzMeasure::calculateHz() { + boost::mutex::scoped_lock lock(mutex_); double hz = -1.0; if (average_message_num_ > 0) { if (buffer_.size() == average_message_num_) { @@ -120,7 +122,6 @@ namespace jsk_topic_tools void HzMeasure::inputCallback(const boost::shared_ptr& msg) { - boost::mutex::scoped_lock lock(mutex_); popBufferQueue(); ros::Time now = ros::Time::now(); buffer_.push(now); @@ -137,7 +138,6 @@ namespace jsk_topic_tools void HzMeasure::updateDiagnostic( diagnostic_updater::DiagnosticStatusWrapper &stat) { - boost::mutex::scoped_lock lock(mutex_); popBufferQueue(); double hz = calculateHz(); if (hz > 0.0) { diff --git a/jsk_topic_tools/test/test_hz_measure.test b/jsk_topic_tools/test/test_hz_measure.test index c1de2b1d7..3cf423a6e 100644 --- a/jsk_topic_tools/test/test_hz_measure.test +++ b/jsk_topic_tools/test/test_hz_measure.test @@ -49,4 +49,13 @@ name="rostopic_30Hz" args="pub -r 30 /hz/input std_msgs/String foo" /> + + + topic_0: /diagnostics + timeout_0: 10 + + + From 872802a42742e4bddb0b3c522cbd0557e1b7e799 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 26 Aug 2022 00:16:34 +0900 Subject: [PATCH 15/39] [jsk_topic_tools/HzMeasureNodelet] Call popBufferQueue in calculateHz --- jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h | 2 -- jsk_topic_tools/src/hz_measure_nodelet.cpp | 5 +---- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h index efbc7ee5c..a5a298255 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/hz_measure_nodelet.h @@ -38,7 +38,6 @@ #include #include -#include #include "jsk_topic_tools/timered_diagnostic_updater.h" @@ -59,7 +58,6 @@ namespace jsk_topic_tools ros::Publisher hz_pub_; ros::Subscriber sub_; ros::NodeHandle pnh_; - boost::mutex mutex_; virtual void popBufferQueue(); virtual double calculateHz(); virtual void inputCallback(const boost::shared_ptr& msg); diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 9e01ea6c1..3a3b24889 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -89,7 +89,6 @@ namespace jsk_topic_tools } void HzMeasure::popBufferQueue() { - boost::mutex::scoped_lock lock(mutex_); ros::Time now = ros::Time::now(); while (!buffer_.empty() && ((measure_time_ > 0 && measure_time_ < (now - buffer_.front()).toSec()) @@ -99,8 +98,8 @@ namespace jsk_topic_tools } double HzMeasure::calculateHz() { - boost::mutex::scoped_lock lock(mutex_); double hz = -1.0; + popBufferQueue(); if (average_message_num_ > 0) { if (buffer_.size() == average_message_num_) { ros::Time now = ros::Time::now(); @@ -122,7 +121,6 @@ namespace jsk_topic_tools void HzMeasure::inputCallback(const boost::shared_ptr& msg) { - popBufferQueue(); ros::Time now = ros::Time::now(); buffer_.push(now); double hz = calculateHz(); @@ -138,7 +136,6 @@ namespace jsk_topic_tools void HzMeasure::updateDiagnostic( diagnostic_updater::DiagnosticStatusWrapper &stat) { - popBufferQueue(); double hz = calculateHz(); if (hz > 0.0) { if (hz > warning_hz_) { From da553f66176c4ba88ee4e931c15d3dcadd57a195 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 26 Aug 2022 00:29:29 +0900 Subject: [PATCH 16/39] [jsk_topic_tools/HzMeasureNodelet] Fixed calculation for hz. --- jsk_topic_tools/src/hz_measure_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 3a3b24889..46cd78b26 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -114,7 +114,7 @@ namespace jsk_topic_tools ros::Time now = ros::Time::now(); time_width = std::min(measure_time_, std::max((now - buffer_.front()).toSec(), 0.0001)); } - hz = (buffer_.size() - 1) / time_width; + hz = std::max(int(buffer_.size() - 1), 0) / time_width; } return hz; } From 7a620078d2d0255f6df70987fd4f8bcd3b7cb2b6 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 26 Aug 2022 18:41:29 +0900 Subject: [PATCH 17/39] [jsk_topic_tools/DiagnosticNodelet] Use node name for diagnostic message --- jsk_topic_tools/src/diagnostic_nodelet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_topic_tools/src/diagnostic_nodelet.cpp b/jsk_topic_tools/src/diagnostic_nodelet.cpp index 04ceb47d5..170384b18 100644 --- a/jsk_topic_tools/src/diagnostic_nodelet.cpp +++ b/jsk_topic_tools/src/diagnostic_nodelet.cpp @@ -55,7 +55,7 @@ namespace jsk_topic_tools new TimeredDiagnosticUpdater(*pnh_, ros::Duration(1.0))); diagnostic_updater_->setHardwareID(getName()); diagnostic_updater_->add( - getName() + "::" + name_, + getName(), boost::bind( &DiagnosticNodelet::updateDiagnostic, this, @@ -94,7 +94,7 @@ namespace jsk_topic_tools } else { jsk_topic_tools::addDiagnosticErrorSummary( - name_, vital_checker_, stat, diagnostic_error_level_); + getName(), vital_checker_, stat, diagnostic_error_level_); } } else { From 6006867b87f25c247af06df0b6068ea80e6e8b8b Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 26 Aug 2022 18:42:48 +0900 Subject: [PATCH 18/39] [jsk_topic_tools/DiagnosticNodelet] Use %.1f for readability --- jsk_topic_tools/src/diagnostic_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_topic_tools/src/diagnostic_utils.cpp b/jsk_topic_tools/src/diagnostic_utils.cpp index 14801b603..b44d31503 100644 --- a/jsk_topic_tools/src/diagnostic_utils.cpp +++ b/jsk_topic_tools/src/diagnostic_utils.cpp @@ -60,7 +60,7 @@ namespace jsk_topic_tools { stat.summary( error_level, - (boost::format("%s not running for %f sec") + (boost::format("%s not running for %.1f sec") % string_prefix % vital_checker->deadSec()).str()); } From 9ab50717487fdb63c7f23240ffaef81dac273b65 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Tue, 13 Sep 2022 14:47:51 +0900 Subject: [PATCH 19/39] fix(battery_capacity_summary): avoid usin keys[i] --- jsk_tools/bin/battery_capacity_summary.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_tools/bin/battery_capacity_summary.py b/jsk_tools/bin/battery_capacity_summary.py index d3a9d64f3..02d7e2c19 100755 --- a/jsk_tools/bin/battery_capacity_summary.py +++ b/jsk_tools/bin/battery_capacity_summary.py @@ -56,8 +56,8 @@ def output(): sorted_keys = ["HardwareID"] + sorted(data_column.keys()) sorted_names = sorted(results) fmt = "{:>31}" - for i in range(len(data_column.keys())): - fmt += "| {:>" + "{w}".format(w=len(data_column.keys()[i])) + "}" + for key in data_column.keys(): + fmt += "| {:>" + "{w}".format(w=len(key)) + "}" print(fmt.format("Battery Name", *data_column.keys())) for name in sorted_names: color = getColor(results[name]) From fce4888c0237bc941f43840da3235c121a18d3e5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 5 Jul 2022 10:29:24 +0900 Subject: [PATCH 20/39] [jsk_topic_tools] add ConstantRateThrottleNodelet and sample/docs for it. [jsk_topic_tools] add constant_rate_throttle_node files [jsk_topic_tools] update for constant_rate_throttle_nodelet [jsk_topic_tools] add compiling of constant_rate_throttle_nodelet [jsk_topic_tools] update constatn_rate_throttle_nodelet [jsk_topic_tools] fix class name for ConstantRateThrottle [jsk_topic_tools] fix plugin build [jsk_topic_tools] update sample for constant_rate_nodelet [jsk_topic_tools] fix bugs [jsk_topic_tools] add constant_rate_throttle_nodelet docs --- .../lib/constant_rate_throttle_nodelet.md | 26 ++++ jsk_topic_tools/CMakeLists.txt | 3 + jsk_topic_tools/cfg/ConstantRateThrottle.cfg | 12 ++ .../constant_rate_throttle_nodelet.h | 48 +++++++ jsk_topic_tools/jsk_topic_tools_nodelet.xml | 8 ++ .../sample_constant_rate_nodelet.launch | 31 +++++ .../src/constant_rate_throttle_nodelet.cpp | 120 ++++++++++++++++++ 7 files changed, 248 insertions(+) create mode 100644 doc/jsk_topic_tools/lib/constant_rate_throttle_nodelet.md create mode 100755 jsk_topic_tools/cfg/ConstantRateThrottle.cfg create mode 100644 jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h create mode 100644 jsk_topic_tools/sample/sample_constant_rate_nodelet.launch create mode 100644 jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp diff --git a/doc/jsk_topic_tools/lib/constant_rate_throttle_nodelet.md b/doc/jsk_topic_tools/lib/constant_rate_throttle_nodelet.md new file mode 100644 index 000000000..c3f997a68 --- /dev/null +++ b/doc/jsk_topic_tools/lib/constant_rate_throttle_nodelet.md @@ -0,0 +1,26 @@ +# constant_rate_throttle_nodelet + +![images/lightweight_throttle_nodelet_diagram.svg](images/lightweight_throttle_nodelet_diagram.svg) + +## Description + +This nodelet provides function like `lightweight_throttle_nodelet`, but support accurate publish rate. + +The rate of throttled message is configurable by passing `~update rate` parameter on launching this nodelet. + +## Subscribing Topic +- `~input` (`AnyMsg`): + + Input topic. This topic is throttled to low publish rate. + +## Publishing Topic +- `~output` (`AnyMsg`): + + Throttled topic. + Publish rate of throttled topic is configurable by setting `~update_rate` parameter. + If `~update_rate` is higher than input topic rate, published message is used from buffer. + +## Parameter +- `~update_rate` (Double, default: `1.0`): + + Publish rate of throttled message [Hz]. This parameter is updated only on launching this nodelet. diff --git a/jsk_topic_tools/CMakeLists.txt b/jsk_topic_tools/CMakeLists.txt index 9234c0732..4f645fa44 100644 --- a/jsk_topic_tools/CMakeLists.txt +++ b/jsk_topic_tools/CMakeLists.txt @@ -52,6 +52,7 @@ add_service_files( FILES List.srv Update.srv ChangeTopic.srv PassthroughDuration.srv ) generate_dynamic_reconfigure_options( + cfg/ConstantRateThrottle.cfg cfg/LightweightThrottle.cfg cfg/StealthRelay.cfg cfg/SynchronizedThrottle.cfg @@ -114,6 +115,8 @@ macro(jsk_topic_tools_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_n endmacro(jsk_topic_tools_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name) # nodelet shared object +jsk_topic_tools_nodelet(src/constant_rate_throttle_nodelet.cpp + "jsk_topic_tools/ConstantRateThrottle" "constant_rate_throttle") jsk_topic_tools_nodelet(src/lightweight_throttle_nodelet.cpp "jsk_topic_tools/LightweightThrottle" "lightweight_throttle") jsk_topic_tools_nodelet(src/mux_nodelet.cpp diff --git a/jsk_topic_tools/cfg/ConstantRateThrottle.cfg b/jsk_topic_tools/cfg/ConstantRateThrottle.cfg new file mode 100755 index 000000000..178c2f3dd --- /dev/null +++ b/jsk_topic_tools/cfg/ConstantRateThrottle.cfg @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * + +PKG = "jsk_topic_tools" + +gen = ParameterGenerator() + +gen.add("update_rate", double_t, 0, "Desired publish rate of throttled messages", 1.0, 0.0, 1000.0) +gen.add("duration_message_valid", double_t, 0, "Timeout duration of subscribed message", 1.0, 0.0, 1000.0) + +exit(gen.generate(PKG, PKG, "ConstantRateThrottle")) diff --git a/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h new file mode 100644 index 000000000..42a00c9c5 --- /dev/null +++ b/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h @@ -0,0 +1,48 @@ +#ifndef CONSTANT_RATE_THROTTLE_NODELET_H_ +#define CONSTANT_RATE_THROTTLE_NODELET_H_ + +#include +#include + +#include +#include +#include +#include + +namespace jsk_topic_tools +{ + class ConstantRateThrottle : public nodelet::Nodelet + { + public: + typedef ConstantRateThrottleConfig Config; + virtual void onInit(); + virtual void configCallback(Config& config, uint32_t level); + virtual void publishMessage(const ros::TimerEvent&); + virtual bool isLoopAlive(); + virtual void startPublishLoop(double loop_rate); + virtual void stopPublishLoop(); + virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub); + virtual void inCallback(const boost::shared_ptr& msg); + protected: + typedef boost::shared_ptr SubscriberPtr; + boost::shared_ptr > srv_; + SubscriberPtr sub_; + ros::Publisher pub_; + ros::TransportHints th_; + ros::NodeHandle pnh_; + ros::Timer timer_publish_; + + boost::mutex mutex_; + + bool subscribing_; + bool advertised_; + + double update_rate_; + ros::Duration duration_message_valid_; + + ros::Time time_cached_; + boost::shared_ptr msg_cached_; + }; +} + +#endif diff --git a/jsk_topic_tools/jsk_topic_tools_nodelet.xml b/jsk_topic_tools/jsk_topic_tools_nodelet.xml index 97bac98ff..0990b2f1a 100644 --- a/jsk_topic_tools/jsk_topic_tools_nodelet.xml +++ b/jsk_topic_tools/jsk_topic_tools_nodelet.xml @@ -18,6 +18,14 @@ as std_msgs/Float32. + + + constant rate throttle nodelet. + Publish ~output from ~input with the fixed rate specified by + ~update_rate parameter. + + diff --git a/jsk_topic_tools/sample/sample_constant_rate_nodelet.launch b/jsk_topic_tools/sample/sample_constant_rate_nodelet.launch new file mode 100644 index 000000000..a5bf4753a --- /dev/null +++ b/jsk_topic_tools/sample/sample_constant_rate_nodelet.launch @@ -0,0 +1,31 @@ + + + + + + + + update_rate: 7.0 + + + + + + + update_rate: 7.0 + + + + + diff --git a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp new file mode 100644 index 000000000..e5a928b1d --- /dev/null +++ b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp @@ -0,0 +1,120 @@ +#include + +namespace jsk_topic_tools +{ + void ConstantRateThrottle::onInit() + { + pnh_ = this->getPrivateNodeHandle(); + subscribing_ = false; + advertised_ = false; + msg_cached_ = boost::shared_ptr(new topic_tools::ShapeShifter()); + + srv_ = boost::make_shared >(pnh_); + dynamic_reconfigure::Server::CallbackType f = boost::bind(&ConstantRateThrottle::configCallback, this, _1, _2); + srv_->setCallback(f); + + sub_.reset(new ros::Subscriber( + pnh_.subscribe( + "input", 1, + &ConstantRateThrottle::inCallback, + this, + th_))); + } + + void ConstantRateThrottle::configCallback(Config& config, uint32_t level) + { + boost::mutex::scoped_lock lock(mutex_); + update_rate_ = config.update_rate; + duration_message_valid_ = ros::Duration(config.duration_message_valid); + if ( this->isLoopAlive() ) { + this->stopPublishLoop(); + this->startPublishLoop(update_rate_); + } + } + + void ConstantRateThrottle::connectionCallback(const ros::SingleSubscriberPublisher& pub) + { + boost::mutex::scoped_lock lock(mutex_); + if (pub_.getNumSubscribers() > 0) { + if (!subscribing_) { + sub_.reset(new ros::Subscriber( + pnh_.subscribe( + "input", 1, + &ConstantRateThrottle::inCallback, + this, + th_))); + subscribing_ = true; + this->startPublishLoop(update_rate_); + } + } + else { // No subscribers, nodelet can unsubscribe input topic + if (subscribing_) { + sub_->shutdown(); + subscribing_ = false; + this->stopPublishLoop(); + } + } + } + + void ConstantRateThrottle::inCallback(const boost::shared_ptr& msg) + { + boost::mutex::scoped_lock lock(mutex_); + if (!advertised_) { + sub_->shutdown(); + ros::SubscriberStatusCallback connect_cb = boost::bind(&ConstantRateThrottle::connectionCallback, this, _1); + ros::AdvertiseOptions opts("output", 1, + msg->getMD5Sum(), + msg->getDataType(), + msg->getMessageDefinition(), + connect_cb, + connect_cb); + advertised_ = true; + pub_ = pnh_.advertise(opts); + } + + *msg_cached_ = *msg; + time_cached_ = ros::Time::now(); + } + + void ConstantRateThrottle::publishMessage(const ros::TimerEvent&) + { + ros::Time current_time = ros::Time::now(); + if ( not msg_cached_ ) { + ROS_WARN("No message is Cached ."); + } else if ( current_time - time_cached_ < duration_message_valid_ ) { + pub_.publish(msg_cached_); + } else { + ROS_WARN("Cached message is too old."); + } + } + + bool ConstantRateThrottle::isLoopAlive() + { + return timer_publish_.isValid() and timer_publish_.hasStarted(); + } + + void ConstantRateThrottle::startPublishLoop(double loop_rate) + { + if ( not timer_publish_.isValid() ) { + timer_publish_ = pnh_.createTimer( + ros::Duration(1.0/update_rate_), + &ConstantRateThrottle::publishMessage, + this + ); + } else { + timer_publish_.setPeriod(ros::Duration(1.0/update_rate_)); + timer_publish_.start(); + } + } + + void ConstantRateThrottle::stopPublishLoop() + { + timer_publish_.stop(); + } + +} + + +#include +typedef jsk_topic_tools::ConstantRateThrottle ConstantRateThrottle; +PLUGINLIB_EXPORT_CLASS(ConstantRateThrottle, nodelet::Nodelet) From b52fbb67f772f7d551c072fda3651cc21581829a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 18 Sep 2022 11:28:24 +0900 Subject: [PATCH 21/39] [jsk_topic_tools] enalbe c++11 support --- jsk_topic_tools/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_topic_tools/CMakeLists.txt b/jsk_topic_tools/CMakeLists.txt index 4f645fa44..56e472d0b 100644 --- a/jsk_topic_tools/CMakeLists.txt +++ b/jsk_topic_tools/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_topic_tools) +add_compile_options(-std=c++11) + # Use ccache if installed to make it fast to generate object files if (CMAKE_VERSION VERSION_LESS 3.4) find_program(CCACHE_FOUND ccache) From 2cce88f3ef28bb004fafe360fdc9bc515eeb964a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 18 Sep 2022 11:31:09 +0900 Subject: [PATCH 22/39] [jsk_topic_tools] add a test for ConstantRateThrottle --- jsk_topic_tools/CMakeLists.txt | 1 + .../test/test_constant_rate_throttle.test | 28 +++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 jsk_topic_tools/test/test_constant_rate_throttle.test diff --git a/jsk_topic_tools/CMakeLists.txt b/jsk_topic_tools/CMakeLists.txt index 56e472d0b..4699dea1f 100644 --- a/jsk_topic_tools/CMakeLists.txt +++ b/jsk_topic_tools/CMakeLists.txt @@ -178,6 +178,7 @@ if (CATKIN_ENABLE_TESTING) # jsk_topic_tools_add_rostest(test/test_topic_buffer_fixed_rate.launch) # jsk_topic_tools_add_rostest(test/test_topic_buffer_fixed_rate_and_update_rate.launch) # jsk_topic_tools_add_rostest(test/test_topic_buffer_update_rate.launch) + jsk_topic_tools_add_rostest(test/test_constant_rate_throttle.test) jsk_topic_tools_add_rostest(test/test_lightweight_throttle.test) jsk_topic_tools_add_rostest(test/test_topic_compare.test) jsk_topic_tools_add_rostest(test/test_hz_measure.test) diff --git a/jsk_topic_tools/test/test_constant_rate_throttle.test b/jsk_topic_tools/test/test_constant_rate_throttle.test new file mode 100644 index 000000000..7a099de62 --- /dev/null +++ b/jsk_topic_tools/test/test_constant_rate_throttle.test @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + From 1b39e05bb71b0e08fc68e472631414646a075b9e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 18 Sep 2022 17:42:45 +0900 Subject: [PATCH 23/39] [jsk_topic_tools] fix timer start flag of ConstantRateThrottleNodelet --- .../include/jsk_topic_tools/constant_rate_throttle_nodelet.h | 1 + jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h index 42a00c9c5..d1ab8e775 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h @@ -31,6 +31,7 @@ namespace jsk_topic_tools ros::TransportHints th_; ros::NodeHandle pnh_; ros::Timer timer_publish_; + bool timer_started_; boost::mutex mutex_; diff --git a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp index e5a928b1d..b0de6e9d1 100644 --- a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp @@ -7,6 +7,7 @@ namespace jsk_topic_tools pnh_ = this->getPrivateNodeHandle(); subscribing_ = false; advertised_ = false; + timer_started_ = false; msg_cached_ = boost::shared_ptr(new topic_tools::ShapeShifter()); srv_ = boost::make_shared >(pnh_); @@ -90,7 +91,7 @@ namespace jsk_topic_tools bool ConstantRateThrottle::isLoopAlive() { - return timer_publish_.isValid() and timer_publish_.hasStarted(); + return timer_publish_.isValid() and timer_started_; } void ConstantRateThrottle::startPublishLoop(double loop_rate) @@ -104,12 +105,14 @@ namespace jsk_topic_tools } else { timer_publish_.setPeriod(ros::Duration(1.0/update_rate_)); timer_publish_.start(); + timer_started_ = false; } } void ConstantRateThrottle::stopPublishLoop() { timer_publish_.stop(); + timer_started_ = false; } } From 2ddefb1fb6a1c371c98e284d53f4eadecfa3acd6 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 30 Sep 2022 23:14:26 +0900 Subject: [PATCH 24/39] catch StopIteration when all images do not have stamp --- .../python/jsk_rosbag_tools/bag_to_video.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py index 0dcc3851a..c8157cc77 100644 --- a/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py +++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py @@ -110,10 +110,15 @@ def bag_to_video(input_bagfile, # remove 0 time stamp stamp = 0.0 - while stamp == 0.0: - stamp, _, img, _ = next(images) - if show_progress_bar: - progress.update(1) + try: + while stamp == 0.0: + stamp, _, img, _ = next(images) + if show_progress_bar: + progress.update(1) + except StopIteration: + print('[bag_to_video] No timestamp found in all images') + print('[bag_to_video] Skipping {}'.format(image_topic)) + break start_stamp = stamp width, height = img.shape[1], img.shape[0] From ab63a1ba0f647c93bec1d5389886b666bdf4c2ca Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 1 Oct 2022 13:26:22 +0900 Subject: [PATCH 25/39] [.github/workflows/config.yml] skip X server on 14.04 --- .github/workflows/config.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/config.yml b/.github/workflows/config.yml index c7317b6b2..9999cb953 100644 --- a/.github/workflows/config.yml +++ b/.github/workflows/config.yml @@ -60,6 +60,7 @@ jobs: - name: Start X server run: | + if [[ "${{ matrix.CONTAINER }}" =~ "jskrobotics/ros-ubuntu:14.04" ]]; then exit 0; fi echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections || echo "failing on ros-ubuntu is OK" # set non interactive tzdata https://stackoverflow.com/questions/8671308/non-interactive-method-for-dpkg-reconfigure-tzdata sudo apt-get -y -qq install mesa-utils x11-xserver-utils xserver-xorg-video-dummy wget export DISPLAY=:0 @@ -68,6 +69,7 @@ jobs: sleep 3 # wait x server up export QT_X11_NO_MITSHM=1 # http://wiki.ros.org/docker/Tutorials/GUI xhost +local:root + shell: bash - name: Run jsk_travis uses: jsk-ros-pkg/jsk_travis@master From 1fd5e9d0e8bd5c959150d67d641ffb919fbdefeb Mon Sep 17 00:00:00 2001 From: Aoi Nakane Date: Mon, 24 Oct 2022 11:56:54 +0900 Subject: [PATCH 26/39] Add timeout argument to download() for wget --- jsk_data/src/jsk_data/download_data.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/jsk_data/src/jsk_data/download_data.py b/jsk_data/src/jsk_data/download_data.py index d398a9866..632a3c695 100644 --- a/jsk_data/src/jsk_data/download_data.py +++ b/jsk_data/src/jsk_data/download_data.py @@ -96,20 +96,26 @@ def decompress_rosbag(path, quiet=False, chmod=True): print('[%s] Finished decompressing the rosbag' % path) -def download(client, url, output, quiet=False, chmod=True): +def download(client, url, output, quiet=False, chmod=True, timeout=30): print('[%s] Downloading from %s' % (output, url)) - cmd = '{client} {url} -O {output}'.format(client=client, url=url, - output=output) + if client == 'wget': + cmd = '{client} {url} -O {output} -T {timeout} --tries 1'.format(client=client, url=url, + output=output, timeout=timeout) + else: + cmd = '{client} {url} -O {output}'.format(client=client, url=url, + output=output) if quiet: cmd += ' --quiet' try: - subprocess.call(shlex.split(cmd)) + status = subprocess.call(shlex.split(cmd)) finally: if chmod: if not is_file_writable(output): os.chmod(output, 0o766) - print('[%s] Finished downloading' % output) - + if status == 0: + print('[%s] Finished downloading' % output) + else: + print('[%s] Failed downloading. exit_status: %d' % (output, status)) def check_md5sum(path, md5): # validate md5 string length if it is specified From d47c2102b8993f9f5f25e8f5f0c8a4b2695ceeeb Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 16 Nov 2022 22:19:31 +0900 Subject: [PATCH 27/39] [.github] specify full version of actions/Checkout action for github workflow --- .github/workflows/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/config.yml b/.github/workflows/config.yml index 9999cb953..782b7943b 100644 --- a/.github/workflows/config.yml +++ b/.github/workflows/config.yml @@ -56,7 +56,7 @@ jobs: fi - name: Chcekout - uses: actions/checkout@v2 + uses: actions/checkout@v3.0.2 - name: Start X server run: | From 8fb047d61a3aa1601ae23663d93989e08a26b494 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 8 Dec 2022 10:10:52 +0900 Subject: [PATCH 28/39] add 2 blank lines before function definition ros roslint ``` src/jsk_data/download_data.py:120:1: E302 expected 2 blank lines, found 1 ``` --- jsk_data/src/jsk_data/download_data.py | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_data/src/jsk_data/download_data.py b/jsk_data/src/jsk_data/download_data.py index 632a3c695..2bd7bca6c 100644 --- a/jsk_data/src/jsk_data/download_data.py +++ b/jsk_data/src/jsk_data/download_data.py @@ -117,6 +117,7 @@ def download(client, url, output, quiet=False, chmod=True, timeout=30): else: print('[%s] Failed downloading. exit_status: %d' % (output, status)) + def check_md5sum(path, md5): # validate md5 string length if it is specified if md5 and len(md5) != 32: From 03e30e3156447badec50a9198945e93d3fac61c0 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 8 Dec 2022 17:39:25 +0900 Subject: [PATCH 29/39] add test to compile on 22.04, see #1770 --- .github/workflows/config.yml | 74 ++++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) diff --git a/.github/workflows/config.yml b/.github/workflows/config.yml index 782b7943b..510c54d4d 100644 --- a/.github/workflows/config.yml +++ b/.github/workflows/config.yml @@ -84,3 +84,77 @@ jobs: TEST_PKGS : ${{ matrix.TEST_PKGS }} BEFORE_SCRIPT : ${{ matrix.BEFORE_SCRIPT }} EXTRA_DEB : ${{ matrix.EXTRA_DEB }} + + + ubuntu: + runs-on: ubuntu-latest + + strategy: + fail-fast: false + matrix: + include: + - DISTRO: ubuntu:22.04 + + container: ${{ matrix.DISTRO }} + + steps: + - name: Chcekout Source + uses: actions/checkout@v3.0.2 + + - name: Install Buildtools + run: | + set -x + echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections + apt update -q + apt install -y -q -qq catkin git curl build-essential libboost-all-dev python3-pip python3-venv python3-rosdep2 + apt install -y -q -qq ros-desktop-dev rosbash + apt install -y -q -qq libself-test-dev libpcl-ros-dev + apt install -y -q -qq ros-core-dev ros-robot-dev + pip install vcstool + rosdep update + + - name: Setup rosinstall_generator + run: | + set -x + # install rosinstall_geneartor with --depend-type buildtool build, see https://github.com/ros-infrastructure/rosinstall_generator/pull/81 + git clone https://github.com/k-okada/rosinstall_generator -b add_depend_type + cd rosinstall_generator + python3 ./setup.py install + + - name: Setup Dependencies Workspace + run: | + set -x + # install dependencies + mkdir -p ~/ws_depend/src + cd ~/ws_depend/src + ROS_PACKAGE_PATH=/usr/share rosinstall_generator --rosdistro noetic --from-path $GITHUB_WORKSPACE --deps --exclude RPP --depend-type buildtool build | tee repos + vcs import --shallow < repos + # override to use latest development for 22.04 + rosinstall_generator laser_filters laser_assembler map_server --rosdistro noetic --upstream-development | vcs import --force + rosinstall_generator catkin_virtualenv --rosdistro noetic | vcs import --force + rm -fr jsk_common + curl -s -L -O https://patch-diff.githubusercontent.com/raw/locusrobotics/catkin_virtualenv/pull/89.diff + patch -p1 < 89.diff + rosdep install -qq -r -y --from-path . --ignore-src || echo "OK" + cd .. + catkin_make_isolated --cmake-args -DCATKIN_ENABLE_TESTING=OFF + + - name: Setup Workspace + run: | + set -x + # hack! + apt install -y -q -qq python-is-python3 + sed -i s/noetic/Debian/ $GITHUB_WORKSPACE/jsk_rosbag_tools/CMakeLists.txt + # setup workspace + mkdir -p ~/ws_current/src + cd ~/ws_current/src + ln -sf $GITHUB_WORKSPACE . + rosdep install -qq -r -y --from-path . --ignore-src || echo "OK" + + - name: Compile Packages + run: | + set -x + cd ~/ws_current/ + source ~/ws_depend/devel_isolated/setup.bash + catkin_make_isolated --cmake-args -DCATKIN_ENABLE_TESTING=OFF + shell: bash From 18e200ca51ac035af3e8ab5955cde2b76fc9c980 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 4 Oct 2022 14:08:09 +0200 Subject: [PATCH 30/39] do not specify c++ standard it break with current log4cxx which requires c++17. No, going forward it's no option to specify the standard anymore. And it's not necessary either. --- jsk_ros_patch/image_view2/CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/jsk_ros_patch/image_view2/CMakeLists.txt b/jsk_ros_patch/image_view2/CMakeLists.txt index c9509c3c9..447840333 100644 --- a/jsk_ros_patch/image_view2/CMakeLists.txt +++ b/jsk_ros_patch/image_view2/CMakeLists.txt @@ -11,12 +11,6 @@ if (CMAKE_VERSION VERSION_LESS 3.4) endif(CCACHE_FOUND) endif() -if(NOT $ENV{ROS_DISTRO} STRLESS noetic) - add_compile_options(-std=c++14) -elseif(NOT $ENV{ROS_DISTRO} STRLESS kinetic) - add_compile_options(-std=c++11) -endif() - find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters message_generation std_srvs pcl_ros) generate_dynamic_reconfigure_options( From bbf92efeb74a7e886db9bf0262d4937008f81f7b Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 29 Oct 2022 17:06:11 +0200 Subject: [PATCH 31/39] update pluginlib includes the non-hpp headers are deprecated since kinetic --- jsk_topic_tools/src/block_nodelet.cpp | 2 +- jsk_topic_tools/src/deprecated_relay_nodelet.cpp | 2 +- jsk_topic_tools/src/hz_measure_nodelet.cpp | 2 +- jsk_topic_tools/src/lightweight_throttle_nodelet.cpp | 2 +- jsk_topic_tools/src/mux_nodelet.cpp | 2 +- jsk_topic_tools/src/passthrough_nodelet.cpp | 2 +- jsk_topic_tools/src/relay_nodelet.cpp | 2 +- jsk_topic_tools/src/snapshot_nodelet.cpp | 2 +- jsk_topic_tools/src/stealth_relay_nodelet.cpp | 2 +- jsk_topic_tools/src/string_relay_nodelet.cpp | 2 +- jsk_topic_tools/src/synchronized_throttle_nodelet.cpp | 2 +- jsk_topic_tools/src/tests/test_log_utils_nodelet.cpp | 2 +- jsk_topic_tools/src/vital_checker_nodelet.cpp | 2 +- 13 files changed, 13 insertions(+), 13 deletions(-) diff --git a/jsk_topic_tools/src/block_nodelet.cpp b/jsk_topic_tools/src/block_nodelet.cpp index cf5534c39..1f9e2a60b 100644 --- a/jsk_topic_tools/src/block_nodelet.cpp +++ b/jsk_topic_tools/src/block_nodelet.cpp @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include "jsk_topic_tools/block_nodelet.h" #include diff --git a/jsk_topic_tools/src/deprecated_relay_nodelet.cpp b/jsk_topic_tools/src/deprecated_relay_nodelet.cpp index ff66e106a..adfda15f2 100644 --- a/jsk_topic_tools/src/deprecated_relay_nodelet.cpp +++ b/jsk_topic_tools/src/deprecated_relay_nodelet.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include "jsk_topic_tools/deprecated_relay.h" #include "jsk_topic_tools/log_utils.h" namespace jsk_topic_tools diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 2a378ce6d..bdc22f111 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include "jsk_topic_tools/hz_measure_nodelet.h" #include "std_msgs/Float32.h" diff --git a/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp b/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp index 1caf36c97..f68e0f263 100644 --- a/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp @@ -120,6 +120,6 @@ namespace jsk_topic_tools } } -#include +#include typedef jsk_topic_tools::LightweightThrottle LightweightThrottle; PLUGINLIB_EXPORT_CLASS(LightweightThrottle, nodelet::Nodelet) diff --git a/jsk_topic_tools/src/mux_nodelet.cpp b/jsk_topic_tools/src/mux_nodelet.cpp index 3d2575c9a..c9a34fc51 100644 --- a/jsk_topic_tools/src/mux_nodelet.cpp +++ b/jsk_topic_tools/src/mux_nodelet.cpp @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include "jsk_topic_tools/mux_nodelet.h" #include #include "jsk_topic_tools/rosparam_utils.h" diff --git a/jsk_topic_tools/src/passthrough_nodelet.cpp b/jsk_topic_tools/src/passthrough_nodelet.cpp index ad7a4276f..0c8b5adee 100644 --- a/jsk_topic_tools/src/passthrough_nodelet.cpp +++ b/jsk_topic_tools/src/passthrough_nodelet.cpp @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include "jsk_topic_tools/passthrough_nodelet.h" namespace jsk_topic_tools diff --git a/jsk_topic_tools/src/relay_nodelet.cpp b/jsk_topic_tools/src/relay_nodelet.cpp index 4ff3fb8f7..c96f04a25 100644 --- a/jsk_topic_tools/src/relay_nodelet.cpp +++ b/jsk_topic_tools/src/relay_nodelet.cpp @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include "jsk_topic_tools/relay_nodelet.h" namespace jsk_topic_tools diff --git a/jsk_topic_tools/src/snapshot_nodelet.cpp b/jsk_topic_tools/src/snapshot_nodelet.cpp index 84770b786..e5ac57308 100644 --- a/jsk_topic_tools/src/snapshot_nodelet.cpp +++ b/jsk_topic_tools/src/snapshot_nodelet.cpp @@ -101,5 +101,5 @@ namespace jsk_topic_tools } -#include +#include PLUGINLIB_EXPORT_CLASS (jsk_topic_tools::Snapshot, nodelet::Nodelet); diff --git a/jsk_topic_tools/src/stealth_relay_nodelet.cpp b/jsk_topic_tools/src/stealth_relay_nodelet.cpp index 4a01d0915..9fc7c9866 100644 --- a/jsk_topic_tools/src/stealth_relay_nodelet.cpp +++ b/jsk_topic_tools/src/stealth_relay_nodelet.cpp @@ -197,6 +197,6 @@ namespace jsk_topic_tools } } -#include +#include typedef jsk_topic_tools::StealthRelay StealthRelay; PLUGINLIB_EXPORT_CLASS(StealthRelay, nodelet::Nodelet) diff --git a/jsk_topic_tools/src/string_relay_nodelet.cpp b/jsk_topic_tools/src/string_relay_nodelet.cpp index 1cf270797..fc3ac622d 100644 --- a/jsk_topic_tools/src/string_relay_nodelet.cpp +++ b/jsk_topic_tools/src/string_relay_nodelet.cpp @@ -60,6 +60,6 @@ namespace jsk_topic_tools } } -#include +#include typedef jsk_topic_tools::StringRelay StringRelay; PLUGINLIB_EXPORT_CLASS(StringRelay, nodelet::Nodelet) diff --git a/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp b/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp index c58f93c54..282a98273 100644 --- a/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp @@ -427,6 +427,6 @@ void SynchronizedThrottle::inputCallback( } // jsk_topic_tools -#include +#include typedef jsk_topic_tools::SynchronizedThrottle SynchronizedThrottle; PLUGINLIB_EXPORT_CLASS(SynchronizedThrottle, nodelet::Nodelet) diff --git a/jsk_topic_tools/src/tests/test_log_utils_nodelet.cpp b/jsk_topic_tools/src/tests/test_log_utils_nodelet.cpp index d789cb392..62a1bd909 100644 --- a/jsk_topic_tools/src/tests/test_log_utils_nodelet.cpp +++ b/jsk_topic_tools/src/tests/test_log_utils_nodelet.cpp @@ -45,5 +45,5 @@ class LogUtils : public nodelet::Nodelet } // namespace jsk_topic_tools -#include +#include PLUGINLIB_EXPORT_CLASS(jsk_topic_tools::test::LogUtils, nodelet::Nodelet) diff --git a/jsk_topic_tools/src/vital_checker_nodelet.cpp b/jsk_topic_tools/src/vital_checker_nodelet.cpp index d39a0c7cd..395a31e6b 100644 --- a/jsk_topic_tools/src/vital_checker_nodelet.cpp +++ b/jsk_topic_tools/src/vital_checker_nodelet.cpp @@ -86,6 +86,6 @@ namespace jsk_topic_tools } } -#include +#include typedef jsk_topic_tools::VitalCheckerNodelet VitalCheckerNodelet; PLUGINLIB_EXPORT_CLASS(VitalCheckerNodelet, nodelet::Nodelet) From 07997f522099ade7906d35cd8342be85b5c2af08 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 29 Oct 2022 17:12:04 +0200 Subject: [PATCH 32/39] avoid boost::bind (esp. global _1) it's deprecated to use global _1 from boost for a while now and the Debian ROS packages do not provide it anymore. --- jsk_ros_patch/image_view2/image_view2.cpp | 2 +- .../points_rectangle_extractor.cpp | 4 +-- jsk_tilt_laser/src/spin_laser_snapshotter.cpp | 4 +-- .../connection_based_nodelet.h | 22 +++++--------- jsk_topic_tools/src/diagnostic_nodelet.cpp | 5 +--- jsk_topic_tools/src/hz_measure_nodelet.cpp | 4 +-- .../src/lightweight_throttle_nodelet.cpp | 4 +-- jsk_topic_tools/src/mux_nodelet.cpp | 2 +- jsk_topic_tools/src/relay_nodelet.cpp | 3 +- jsk_topic_tools/src/stealth_relay_nodelet.cpp | 2 +- .../src/synchronized_throttle_nodelet.cpp | 29 ++++++++++++++----- .../src/timered_diagnostic_updater.cpp | 5 +--- jsk_topic_tools/src/topic_buffer_client.cpp | 3 +- jsk_topic_tools/src/topic_buffer_server.cpp | 2 +- 14 files changed, 46 insertions(+), 45 deletions(-) diff --git a/jsk_ros_patch/image_view2/image_view2.cpp b/jsk_ros_patch/image_view2/image_view2.cpp index 754672116..b6a76efb7 100644 --- a/jsk_ros_patch/image_view2/image_view2.cpp +++ b/jsk_ros_patch/image_view2/image_view2.cpp @@ -116,7 +116,7 @@ namespace image_view2{ srv_ = boost::make_shared >(local_nh); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&ImageView2::config_callback, this, _1, _2); + [this](auto& config, auto level){ config_callback(config, level); }; srv_->setCallback(f); } diff --git a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp b/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp index 89d4f13cc..8fb321236 100644 --- a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp +++ b/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp @@ -43,11 +43,11 @@ class PointsRectExtractor sync_a_polygon_ = boost::make_shared < message_filters::Synchronizer< PolygonApproxSyncPolicy > > (queue_size); sync_a_polygon_->connectInput (points_sub_, rect_sub_); - sync_a_polygon_->registerCallback (boost::bind (&PointsRectExtractor::callback_polygon, this, _1, _2)); + sync_a_polygon_->registerCallback (boost::bind (&PointsRectExtractor::callback_polygon, this, boost::placeholders::_1, boost::placeholders::_2)); sync_a_point_ = boost::make_shared < message_filters::Synchronizer< PointApproxSyncPolicy > > (queue_size); sync_a_point_->connectInput (points_sub_, point_sub_); - sync_a_point_->registerCallback (boost::bind (&PointsRectExtractor::callback_point, this, _1, _2)); + sync_a_point_->registerCallback (boost::bind (&PointsRectExtractor::callback_point, this, boost::placeholders::_1, boost::placeholders::_2)); pub_ = pnode_.advertise< sensor_msgs::PointCloud2 > ("output", 1); } diff --git a/jsk_tilt_laser/src/spin_laser_snapshotter.cpp b/jsk_tilt_laser/src/spin_laser_snapshotter.cpp index d92bdb2c3..32b441d1a 100644 --- a/jsk_tilt_laser/src/spin_laser_snapshotter.cpp +++ b/jsk_tilt_laser/src/spin_laser_snapshotter.cpp @@ -103,8 +103,8 @@ class SpinLaserSnapshotter private_ns_.param("spindle_frame", spindle_frame_, std::string("multisense/spindle")); private_ns_.param("motor_frame", motor_frame_, std::string("multisense/motor")); timer_ = private_ns_.createTimer( - ros::Duration(1.0 / rate_), boost::bind( - &SpinLaserSnapshotter::timerCallback, this, _1)); + ros::Duration(1.0 / rate_), + [this](auto& event){ timerCallback(event); }); } } diff --git a/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h index 0657d29d6..d48a8ad74 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h @@ -192,9 +192,9 @@ namespace jsk_topic_tools { boost::mutex::scoped_lock lock(connection_mutex_); ros::SubscriberStatusCallback connect_cb - = boost::bind(&ConnectionBasedNodelet::connectionCallback, this, _1); + = [this](auto& pub){ connectionCallback(pub); }; ros::SubscriberStatusCallback disconnect_cb - = boost::bind(&ConnectionBasedNodelet::connectionCallback, this, _1); + = [this](auto& pub){ connectionCallback(pub); }; ros::Publisher ret = nh.advertise(topic, queue_size, connect_cb, disconnect_cb, @@ -254,11 +254,9 @@ namespace jsk_topic_tools { boost::mutex::scoped_lock lock(connection_mutex_); image_transport::SubscriberStatusCallback connect_cb - = boost::bind(&ConnectionBasedNodelet::imageConnectionCallback, - this, _1); + = [this](auto& pub){ imageConnectionCallback(pub); }; image_transport::SubscriberStatusCallback disconnect_cb - = boost::bind(&ConnectionBasedNodelet::imageConnectionCallback, - this, _1); + = [this](auto& pub){ imageConnectionCallback(pub); }; image_transport::Publisher pub = image_transport::ImageTransport(nh).advertise( topic, 1, connect_cb, @@ -318,17 +316,13 @@ namespace jsk_topic_tools { boost::mutex::scoped_lock lock(connection_mutex_); image_transport::SubscriberStatusCallback connect_cb - = boost::bind(&ConnectionBasedNodelet::cameraConnectionCallback, - this, _1); + = [this](auto& pub){ cameraConnectionCallback(pub); }; image_transport::SubscriberStatusCallback disconnect_cb - = boost::bind(&ConnectionBasedNodelet::cameraConnectionCallback, - this, _1); + = [this](auto& pub){ cameraConnectionCallback(pub); }; ros::SubscriberStatusCallback info_connect_cb - = boost::bind(&ConnectionBasedNodelet::cameraInfoConnectionCallback, - this, _1); + = [this](auto& pub){ cameraInfoConnectionCallback(pub); }; ros::SubscriberStatusCallback info_disconnect_cb - = boost::bind(&ConnectionBasedNodelet::cameraInfoConnectionCallback, - this, _1); + = [this](auto& pub){ cameraInfoConnectionCallback(pub); }; image_transport::CameraPublisher pub = image_transport::ImageTransport(nh).advertiseCamera( topic, 1, diff --git a/jsk_topic_tools/src/diagnostic_nodelet.cpp b/jsk_topic_tools/src/diagnostic_nodelet.cpp index 14e5bd0ca..e95f83de5 100644 --- a/jsk_topic_tools/src/diagnostic_nodelet.cpp +++ b/jsk_topic_tools/src/diagnostic_nodelet.cpp @@ -56,10 +56,7 @@ namespace jsk_topic_tools diagnostic_updater_->setHardwareID(getName()); diagnostic_updater_->add( getName(), - boost::bind( - &DiagnosticNodelet::updateDiagnostic, - this, - _1)); + [this](auto& stat){ updateDiagnostic(stat); }); bool use_warn; nh_->param("/diagnostic_nodelet/use_warn", use_warn, false); diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index bdc22f111..7b0638666 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -78,9 +78,7 @@ namespace jsk_topic_tools diagnostic_updater_.reset(new TimeredDiagnosticUpdater(pnh_, ros::Duration(1.0))); diagnostic_updater_->setHardwareID(getName()); - diagnostic_updater_->add(getName(), - boost::bind(&HzMeasure::updateDiagnostic, - this, _1)); + diagnostic_updater_->add(getName(), [this](auto& stat){ updateDiagnostic(stat); }); diagnostic_updater_->start(); hz_pub_ = pnh_.advertise("output", 1); diff --git a/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp b/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp index f68e0f263..4c29a69bb 100644 --- a/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp @@ -45,7 +45,7 @@ namespace jsk_topic_tools srv_ = boost::make_shared >(pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&LightweightThrottle::configCallback, this, _1, _2); + [this](auto& config, auto level){ configCallback(config, level); }; srv_->setCallback(f); // Subscribe input topic at first in order to decide @@ -95,7 +95,7 @@ namespace jsk_topic_tools // This section should be called once sub_->shutdown(); // Shutdown before advertising topic ros::SubscriberStatusCallback connect_cb - = boost::bind(&LightweightThrottle::connectionCallback, this, _1); + = [this](auto& pub){ connectionCallback(pub); }; ros::AdvertiseOptions opts("output", 1, msg->getMD5Sum(), msg->getDataType(), diff --git a/jsk_topic_tools/src/mux_nodelet.cpp b/jsk_topic_tools/src/mux_nodelet.cpp index c9a34fc51..b9da17acc 100644 --- a/jsk_topic_tools/src/mux_nodelet.cpp +++ b/jsk_topic_tools/src/mux_nodelet.cpp @@ -194,7 +194,7 @@ namespace jsk_topic_tools { if (!advertised_) { // first time ros::SubscriberStatusCallback connect_cb - = boost::bind(&MUX::connectCb, this, _1); + = [this](auto& pub){ connectCb(pub); }; ros::AdvertiseOptions opts("output", 1, msg->getMD5Sum(), msg->getDataType(), diff --git a/jsk_topic_tools/src/relay_nodelet.cpp b/jsk_topic_tools/src/relay_nodelet.cpp index c96f04a25..62a11f433 100644 --- a/jsk_topic_tools/src/relay_nodelet.cpp +++ b/jsk_topic_tools/src/relay_nodelet.cpp @@ -50,8 +50,7 @@ namespace jsk_topic_tools diagnostic_updater_->setHardwareID(getName()); diagnostic_updater_->add( getName() + "::Relay", - boost::bind( - &Relay::updateDiagnostic, this, _1)); + [this](auto& stat){ updateDiagnostic(stat); }); double vital_rate; pnh_.param("vital_rate", vital_rate, 1.0); vital_checker_.reset( diff --git a/jsk_topic_tools/src/stealth_relay_nodelet.cpp b/jsk_topic_tools/src/stealth_relay_nodelet.cpp index 9fc7c9866..c62ded51a 100644 --- a/jsk_topic_tools/src/stealth_relay_nodelet.cpp +++ b/jsk_topic_tools/src/stealth_relay_nodelet.cpp @@ -69,7 +69,7 @@ namespace jsk_topic_tools srv_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&StealthRelay::configCallback, this, _1, _2); + [this](auto& config, auto level){ configCallback(config, level); }; srv_->setCallback(f); /* To advertise output topic as the same type of input topic, diff --git a/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp b/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp index 282a98273..83b228717 100644 --- a/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp @@ -66,7 +66,7 @@ void SynchronizedThrottle::onInit() srv_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&SynchronizedThrottle::configCallback, this, _1, _2); + [this](auto& config, auto level){ configCallback(config, level); }; srv_->setCallback(f); // message_filter supports 2~8 input topics @@ -89,7 +89,7 @@ void SynchronizedThrottle::onInit() { check_sub_[i] = pnh_->subscribe( input_topics_[i], 1, - boost::bind(&SynchronizedThrottle::checkCallback, this, _1, i)); + [this,i](auto& msg){ checkCallback(msg, i); }); sub_[i].reset(new message_filters::Subscriber()); } @@ -178,7 +178,7 @@ void SynchronizedThrottle::subscribe() if (n_topics < MAX_SYNC_NUM) { sub_[0]->registerCallback( - boost::bind(&SynchronizedThrottle::fillNullMessage, this, _1)); + [this](auto& msg){ fillNullMessage(msg); }); } if (approximate_sync_) @@ -220,8 +220,16 @@ void SynchronizedThrottle::subscribe() return; } async_->registerCallback( - boost::bind(&SynchronizedThrottle::inputCallback, this, - _1, _2, _3, _4, _5, _6, _7, _8)); + boost::bind(&SynchronizedThrottle::inputCallback, this, + boost::placeholders::_1, + boost::placeholders::_2, + boost::placeholders::_3, + boost::placeholders::_4, + boost::placeholders::_5, + boost::placeholders::_6, + boost::placeholders::_7, + boost::placeholders::_8)); + } else { sync_ = boost::make_shared >(queue_size_); @@ -260,8 +268,15 @@ void SynchronizedThrottle::subscribe() return; } sync_->registerCallback( - boost::bind(&SynchronizedThrottle::inputCallback, this, - _1, _2, _3, _4, _5, _6, _7, _8)); + boost::bind(&SynchronizedThrottle::inputCallback, this, + boost::placeholders::_1, + boost::placeholders::_2, + boost::placeholders::_3, + boost::placeholders::_4, + boost::placeholders::_5, + boost::placeholders::_6, + boost::placeholders::_7, + boost::placeholders::_8)); } } diff --git a/jsk_topic_tools/src/timered_diagnostic_updater.cpp b/jsk_topic_tools/src/timered_diagnostic_updater.cpp index 93c07f9e7..bfa42ecd9 100644 --- a/jsk_topic_tools/src/timered_diagnostic_updater.cpp +++ b/jsk_topic_tools/src/timered_diagnostic_updater.cpp @@ -43,10 +43,7 @@ namespace jsk_topic_tools diagnostic_updater_(new diagnostic_updater::Updater) { timer_ = nh.createTimer( - timer_duration, boost::bind( - &TimeredDiagnosticUpdater::timerCallback, - this, - _1)); + timer_duration, [this](auto& event){ timerCallback(event); }); timer_.stop(); } diff --git a/jsk_topic_tools/src/topic_buffer_client.cpp b/jsk_topic_tools/src/topic_buffer_client.cpp index f4aaafacf..d5724c750 100644 --- a/jsk_topic_tools/src/topic_buffer_client.cpp +++ b/jsk_topic_tools/src/topic_buffer_client.cpp @@ -200,7 +200,8 @@ int main(int argc, char **argv) pub_info->advertised = false; pub_info->topic_with_header = false; ROS_INFO_STREAM("subscribe " << pub_info->topic_name+string("_update")); - pub_info->sub = new ros::Subscriber(n.subscribe(pub_info->topic_name+string("_update"), 10, boost::bind(in_cb, _1, pub_info))); + pub_info->sub = new ros::Subscriber(n.subscribe(pub_info->topic_name+string("_update"), 10, + [pub_info](auto& msg){ in_cb(msg, pub_info); })); g_pubs.push_back(pub_info); } diff --git a/jsk_topic_tools/src/topic_buffer_server.cpp b/jsk_topic_tools/src/topic_buffer_server.cpp index 4358e4ab2..f78b7f77b 100644 --- a/jsk_topic_tools/src/topic_buffer_server.cpp +++ b/jsk_topic_tools/src/topic_buffer_server.cpp @@ -219,7 +219,7 @@ int main(int argc, char **argv) sub_info->advertised = false; sub_info->periodic = false; ROS_INFO_STREAM("subscribe " << sub_info->topic_name); - sub_info->sub = new ros::Subscriber(n.subscribe(sub_info->topic_name, 10, boost::bind(in_cb, _1, sub_info))); + sub_info->sub = new ros::Subscriber(n.subscribe(sub_info->topic_name, 10, [sub_info](auto& msg){ in_cb(msg, sub_info); })); // waiting for all topics are publisherd while (sub_info->sub->getNumPublishers() == 0 ) { From beee45cf53c130d7d6761dadf3aefa7e6a8e9fd5 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 29 Oct 2022 17:14:51 +0200 Subject: [PATCH 33/39] fixup documentation this confused me when I looked through the code. --- .../include/jsk_topic_tools/connection_based_nodelet.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h index d48a8ad74..bba9e4385 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h @@ -86,7 +86,7 @@ namespace jsk_topic_tools virtual void onInitPostProcess(); /** @brief - * callback function which is called when new subscriber come + * callback function which is called when new subscriber connects or disconnects */ virtual void connectionCallback(const ros::SingleSubscriberPublisher& pub); From f834c0a86a2d19013675e60ffbb2c26ba5b87e6c Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 9 Nov 2022 13:47:07 +0100 Subject: [PATCH 34/39] use setuptools setup from distutils is deprecated and will be removed eventually. --- jsk_network_tools/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_network_tools/setup.py b/jsk_network_tools/setup.py index 7ffd6d804..844bbc07c 100755 --- a/jsk_network_tools/setup.py +++ b/jsk_network_tools/setup.py @@ -1,6 +1,6 @@ ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD -from distutils.core import setup +from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup # fetch values from package.xml From 7fb8ffaba9cd958b49b0b5a820eff070fe425074 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 12 Dec 2022 20:14:29 +0900 Subject: [PATCH 35/39] do not specify c++ standard it break with current log4cxx which requires c++17. No, going forward it's no option to specify the standard anymore. And it's not necessary either. same fix as https://github.com/jsk-ros-pkg/jsk_common/pull/1773/commits/18e200ca51ac035af3e8ab5955cde2b76fc9c980 --- jsk_topic_tools/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/jsk_topic_tools/CMakeLists.txt b/jsk_topic_tools/CMakeLists.txt index 4699dea1f..7d4895d06 100644 --- a/jsk_topic_tools/CMakeLists.txt +++ b/jsk_topic_tools/CMakeLists.txt @@ -2,8 +2,6 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_topic_tools) -add_compile_options(-std=c++11) - # Use ccache if installed to make it fast to generate object files if (CMAKE_VERSION VERSION_LESS 3.4) find_program(CCACHE_FOUND ccache) From 4973030c552e937566ccfaf182676478b508e439 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 12 Dec 2022 20:24:17 +0900 Subject: [PATCH 36/39] avoid boost::bind (esp. global _1) it's deprecated to use global _1 from boost for a while now and the Debian ROS packages do not provide it anymore. --- jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp index b0de6e9d1..6da3502d3 100644 --- a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp @@ -11,7 +11,7 @@ namespace jsk_topic_tools msg_cached_ = boost::shared_ptr(new topic_tools::ShapeShifter()); srv_ = boost::make_shared >(pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind(&ConstantRateThrottle::configCallback, this, _1, _2); + dynamic_reconfigure::Server::CallbackType f = [this](auto& config, auto level) {configCallback(config, level); }; srv_->setCallback(f); sub_.reset(new ros::Subscriber( @@ -62,7 +62,7 @@ namespace jsk_topic_tools boost::mutex::scoped_lock lock(mutex_); if (!advertised_) { sub_->shutdown(); - ros::SubscriberStatusCallback connect_cb = boost::bind(&ConstantRateThrottle::connectionCallback, this, _1); + ros::SubscriberStatusCallback connect_cb = [this](auto& pub){ connectionCallback(pub); }; ros::AdvertiseOptions opts("output", 1, msg->getMD5Sum(), msg->getDataType(), From 6fd594b5f5f382487dab8218d7c6433a0be2fb1a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 12 Dec 2022 13:45:10 +0000 Subject: [PATCH 37/39] requries -std=c++11 compile options, but it is not used in source tree --- .../include/jsk_topic_tools/constant_rate_throttle_nodelet.h | 1 - 1 file changed, 1 deletion(-) diff --git a/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h index d1ab8e775..4da299844 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/constant_rate_throttle_nodelet.h @@ -7,7 +7,6 @@ #include #include #include -#include namespace jsk_topic_tools { From a4e0d40d358348ee08ba7fb52f23b436f1531e4f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 12 Dec 2022 13:46:47 +0000 Subject: [PATCH 38/39] revert code before C++11 --- jsk_ros_patch/image_view2/image_view2.cpp | 4 ++ .../points_rectangle_extractor.cpp | 21 ++++++++++ jsk_tilt_laser/src/spin_laser_snapshotter.cpp | 4 ++ .../connection_based_nodelet.h | 38 ++++++++++++++++++ .../src/constant_rate_throttle_nodelet.cpp | 14 ++++++- jsk_topic_tools/src/diagnostic_nodelet.cpp | 10 ++++- jsk_topic_tools/src/hz_measure_nodelet.cpp | 6 +++ .../src/lightweight_throttle_nodelet.cpp | 8 ++++ jsk_topic_tools/src/mux_nodelet.cpp | 4 ++ jsk_topic_tools/src/relay_nodelet.cpp | 5 +++ jsk_topic_tools/src/stealth_relay_nodelet.cpp | 4 ++ .../src/synchronized_throttle_nodelet.cpp | 39 ++++++++++++++++++- .../src/timered_diagnostic_updater.cpp | 8 +++- jsk_topic_tools/src/topic_buffer_client.cpp | 8 +++- jsk_topic_tools/src/topic_buffer_server.cpp | 9 ++++- 15 files changed, 172 insertions(+), 10 deletions(-) diff --git a/jsk_ros_patch/image_view2/image_view2.cpp b/jsk_ros_patch/image_view2/image_view2.cpp index b6a76efb7..f50b74f82 100644 --- a/jsk_ros_patch/image_view2/image_view2.cpp +++ b/jsk_ros_patch/image_view2/image_view2.cpp @@ -116,7 +116,11 @@ namespace image_view2{ srv_ = boost::make_shared >(local_nh); dynamic_reconfigure::Server::CallbackType f = +#if __cplusplus < 201100L + boost::bind(&ImageView2::config_callback, this, _1, _2); +#else [this](auto& config, auto level){ config_callback(config, level); }; +#endif srv_->setCallback(f); } diff --git a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp b/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp index 8fb321236..7eac64e96 100644 --- a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp +++ b/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp @@ -13,6 +13,27 @@ #include #include +#if BOOST_VERSION < 106000 // since 1.60.0, boost uses placeholders namesapce for _1,_2... +#ifndef BOOST_PLAEHOLDERS +#define BOOST_PLAEHOLDERS +namespace boost +{ + namespace placeholders + { + extern boost::arg<1> _1; + extern boost::arg<2> _2; + extern boost::arg<3> _3; + extern boost::arg<4> _4; + extern boost::arg<5> _5; + extern boost::arg<6> _6; + extern boost::arg<7> _7; + extern boost::arg<8> _8; + extern boost::arg<9> _9; + } // namespace placeholders +} // namespace boost +#endif // BOOST_PLAEHOLDERS +#endif // BOOST_VERSION < 106000 + class PointsRectExtractor { typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, diff --git a/jsk_tilt_laser/src/spin_laser_snapshotter.cpp b/jsk_tilt_laser/src/spin_laser_snapshotter.cpp index 32b441d1a..e60164717 100644 --- a/jsk_tilt_laser/src/spin_laser_snapshotter.cpp +++ b/jsk_tilt_laser/src/spin_laser_snapshotter.cpp @@ -104,7 +104,11 @@ class SpinLaserSnapshotter private_ns_.param("motor_frame", motor_frame_, std::string("multisense/motor")); timer_ = private_ns_.createTimer( ros::Duration(1.0 / rate_), +#if __cplusplus < 201100L + boost::bind(&SpinLaserSnapshotter::timerCallback, this, _1)); +#else [this](auto& event){ timerCallback(event); }); +#endif } } diff --git a/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h b/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h index bba9e4385..075ac3897 100644 --- a/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h +++ b/jsk_topic_tools/include/jsk_topic_tools/connection_based_nodelet.h @@ -192,9 +192,17 @@ namespace jsk_topic_tools { boost::mutex::scoped_lock lock(connection_mutex_); ros::SubscriberStatusCallback connect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::connectionCallback, this, _1); +#else = [this](auto& pub){ connectionCallback(pub); }; +#endif ros::SubscriberStatusCallback disconnect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::connectionCallback, this, _1); +#else = [this](auto& pub){ connectionCallback(pub); }; +#endif ros::Publisher ret = nh.advertise(topic, queue_size, connect_cb, disconnect_cb, @@ -254,9 +262,19 @@ namespace jsk_topic_tools { boost::mutex::scoped_lock lock(connection_mutex_); image_transport::SubscriberStatusCallback connect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::imageConnectionCallback, + this, _1); +#else = [this](auto& pub){ imageConnectionCallback(pub); }; +#endif image_transport::SubscriberStatusCallback disconnect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::imageConnectionCallback, + this, _1); +#else = [this](auto& pub){ imageConnectionCallback(pub); }; +#endif image_transport::Publisher pub = image_transport::ImageTransport(nh).advertise( topic, 1, connect_cb, @@ -316,13 +334,33 @@ namespace jsk_topic_tools { boost::mutex::scoped_lock lock(connection_mutex_); image_transport::SubscriberStatusCallback connect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::cameraConnectionCallback, + this, _1); +#else = [this](auto& pub){ cameraConnectionCallback(pub); }; +#endif image_transport::SubscriberStatusCallback disconnect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::cameraConnectionCallback, + this, _1); +#else = [this](auto& pub){ cameraConnectionCallback(pub); }; +#endif ros::SubscriberStatusCallback info_connect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::cameraInfoConnectionCallback, + this, _1); +#else = [this](auto& pub){ cameraInfoConnectionCallback(pub); }; +#endif ros::SubscriberStatusCallback info_disconnect_cb +#if __cplusplus < 201100L + = boost::bind(&ConnectionBasedNodelet::cameraInfoConnectionCallback, + this, _1); +#else = [this](auto& pub){ cameraInfoConnectionCallback(pub); }; +#endif image_transport::CameraPublisher pub = image_transport::ImageTransport(nh).advertiseCamera( topic, 1, diff --git a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp index 6da3502d3..f43a4ac54 100644 --- a/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/constant_rate_throttle_nodelet.cpp @@ -11,7 +11,12 @@ namespace jsk_topic_tools msg_cached_ = boost::shared_ptr(new topic_tools::ShapeShifter()); srv_ = boost::make_shared >(pnh_); - dynamic_reconfigure::Server::CallbackType f = [this](auto& config, auto level) {configCallback(config, level); }; + dynamic_reconfigure::Server::CallbackType f +#if __cplusplus < 201100L + = boost::bind(&ConstantRateThrottle::configCallback, this, _1, _2); +#else + = [this](auto& config, auto level) {configCallback(config, level); }; +#endif srv_->setCallback(f); sub_.reset(new ros::Subscriber( @@ -62,7 +67,12 @@ namespace jsk_topic_tools boost::mutex::scoped_lock lock(mutex_); if (!advertised_) { sub_->shutdown(); - ros::SubscriberStatusCallback connect_cb = [this](auto& pub){ connectionCallback(pub); }; + ros::SubscriberStatusCallback connect_cb +#if __cplusplus < 201100L + = boost::bind(&ConstantRateThrottle::connectionCallback, this, _1); +#else + = [this](auto& pub){ connectionCallback(pub); }; +#endif ros::AdvertiseOptions opts("output", 1, msg->getMD5Sum(), msg->getDataType(), diff --git a/jsk_topic_tools/src/diagnostic_nodelet.cpp b/jsk_topic_tools/src/diagnostic_nodelet.cpp index e95f83de5..365968ec0 100644 --- a/jsk_topic_tools/src/diagnostic_nodelet.cpp +++ b/jsk_topic_tools/src/diagnostic_nodelet.cpp @@ -56,7 +56,15 @@ namespace jsk_topic_tools diagnostic_updater_->setHardwareID(getName()); diagnostic_updater_->add( getName(), - [this](auto& stat){ updateDiagnostic(stat); }); +#if __cplusplus < 201100L + boost::bind( + &DiagnosticNodelet::updateDiagnostic, + this, + _1) +#else + [this](auto& stat){ updateDiagnostic(stat); } +#endif + ); bool use_warn; nh_->param("/diagnostic_nodelet/use_warn", use_warn, false); diff --git a/jsk_topic_tools/src/hz_measure_nodelet.cpp b/jsk_topic_tools/src/hz_measure_nodelet.cpp index 7b0638666..f859fe9b9 100644 --- a/jsk_topic_tools/src/hz_measure_nodelet.cpp +++ b/jsk_topic_tools/src/hz_measure_nodelet.cpp @@ -78,7 +78,13 @@ namespace jsk_topic_tools diagnostic_updater_.reset(new TimeredDiagnosticUpdater(pnh_, ros::Duration(1.0))); diagnostic_updater_->setHardwareID(getName()); +#if __cplusplus < 201100L + diagnostic_updater_->add(getName(), + boost::bind(&HzMeasure::updateDiagnostic, + this, _1)); +#else diagnostic_updater_->add(getName(), [this](auto& stat){ updateDiagnostic(stat); }); +#endif diagnostic_updater_->start(); hz_pub_ = pnh_.advertise("output", 1); diff --git a/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp b/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp index 4c29a69bb..dd59d9551 100644 --- a/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/lightweight_throttle_nodelet.cpp @@ -45,7 +45,11 @@ namespace jsk_topic_tools srv_ = boost::make_shared >(pnh_); dynamic_reconfigure::Server::CallbackType f = +#if __cplusplus < 201100L + boost::bind(&LightweightThrottle::configCallback, this, _1, _2); +#else [this](auto& config, auto level){ configCallback(config, level); }; +#endif srv_->setCallback(f); // Subscribe input topic at first in order to decide @@ -95,7 +99,11 @@ namespace jsk_topic_tools // This section should be called once sub_->shutdown(); // Shutdown before advertising topic ros::SubscriberStatusCallback connect_cb +#if __cplusplus < 201100L + = boost::bind(&LightweightThrottle::connectionCallback, this, _1); +#else = [this](auto& pub){ connectionCallback(pub); }; +#endif ros::AdvertiseOptions opts("output", 1, msg->getMD5Sum(), msg->getDataType(), diff --git a/jsk_topic_tools/src/mux_nodelet.cpp b/jsk_topic_tools/src/mux_nodelet.cpp index b9da17acc..fe27fcff2 100644 --- a/jsk_topic_tools/src/mux_nodelet.cpp +++ b/jsk_topic_tools/src/mux_nodelet.cpp @@ -194,7 +194,11 @@ namespace jsk_topic_tools { if (!advertised_) { // first time ros::SubscriberStatusCallback connect_cb +#if __cplusplus < 201100L + = boost::bind(&MUX::connectCb, this, _1); +#else = [this](auto& pub){ connectCb(pub); }; +#endif ros::AdvertiseOptions opts("output", 1, msg->getMD5Sum(), msg->getDataType(), diff --git a/jsk_topic_tools/src/relay_nodelet.cpp b/jsk_topic_tools/src/relay_nodelet.cpp index 62a11f433..d26bd254a 100644 --- a/jsk_topic_tools/src/relay_nodelet.cpp +++ b/jsk_topic_tools/src/relay_nodelet.cpp @@ -50,7 +50,12 @@ namespace jsk_topic_tools diagnostic_updater_->setHardwareID(getName()); diagnostic_updater_->add( getName() + "::Relay", +#if __cplusplus < 201100L + boost::bind( + &Relay::updateDiagnostic, this, _1)); +#else [this](auto& stat){ updateDiagnostic(stat); }); +#endif double vital_rate; pnh_.param("vital_rate", vital_rate, 1.0); vital_checker_.reset( diff --git a/jsk_topic_tools/src/stealth_relay_nodelet.cpp b/jsk_topic_tools/src/stealth_relay_nodelet.cpp index c62ded51a..7b309839e 100644 --- a/jsk_topic_tools/src/stealth_relay_nodelet.cpp +++ b/jsk_topic_tools/src/stealth_relay_nodelet.cpp @@ -69,7 +69,11 @@ namespace jsk_topic_tools srv_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = +#if __cplusplus < 201100L + boost::bind(&StealthRelay::configCallback, this, _1, _2); +#else [this](auto& config, auto level){ configCallback(config, level); }; +#endif srv_->setCallback(f); /* To advertise output topic as the same type of input topic, diff --git a/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp b/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp index 83b228717..995a6db6b 100644 --- a/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp +++ b/jsk_topic_tools/src/synchronized_throttle_nodelet.cpp @@ -39,6 +39,27 @@ #include +#if BOOST_VERSION < 106000 // since 1.60.0, boost uses placeholders namesapce for _1,_2... +#ifndef BOOST_PLAEHOLDERS +#define BOOST_PLAEHOLDERS +namespace boost +{ + namespace placeholders + { + extern boost::arg<1> _1; + extern boost::arg<2> _2; + extern boost::arg<3> _3; + extern boost::arg<4> _4; + extern boost::arg<5> _5; + extern boost::arg<6> _6; + extern boost::arg<7> _7; + extern boost::arg<8> _8; + extern boost::arg<9> _9; + } // namespace placeholders +} // namespace boost +#endif // BOOST_PLAEHOLDERS +#endif // BOOST_VERSION < 106000 + namespace jsk_topic_tools { @@ -66,7 +87,11 @@ void SynchronizedThrottle::onInit() srv_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = +#if __cplusplus < 201100L + boost::bind(&SynchronizedThrottle::configCallback, this, _1, _2); +#else [this](auto& config, auto level){ configCallback(config, level); }; +#endif srv_->setCallback(f); // message_filter supports 2~8 input topics @@ -89,7 +114,12 @@ void SynchronizedThrottle::onInit() { check_sub_[i] = pnh_->subscribe( input_topics_[i], 1, - [this,i](auto& msg){ checkCallback(msg, i); }); +#if __cplusplus < 201100L + boost::bind(&SynchronizedThrottle::checkCallback, this, _1, i) +#else + [this,i](auto& msg){ checkCallback(msg, i); } +#endif + ); sub_[i].reset(new message_filters::Subscriber()); } @@ -178,7 +208,12 @@ void SynchronizedThrottle::subscribe() if (n_topics < MAX_SYNC_NUM) { sub_[0]->registerCallback( - [this](auto& msg){ fillNullMessage(msg); }); +#if __cplusplus < 201100L + boost::bind(&SynchronizedThrottle::fillNullMessage, this, _1) +#else + [this](auto& msg){ fillNullMessage(msg); } +#endif + ); } if (approximate_sync_) diff --git a/jsk_topic_tools/src/timered_diagnostic_updater.cpp b/jsk_topic_tools/src/timered_diagnostic_updater.cpp index bfa42ecd9..c3b384e9d 100644 --- a/jsk_topic_tools/src/timered_diagnostic_updater.cpp +++ b/jsk_topic_tools/src/timered_diagnostic_updater.cpp @@ -43,7 +43,13 @@ namespace jsk_topic_tools diagnostic_updater_(new diagnostic_updater::Updater) { timer_ = nh.createTimer( - timer_duration, [this](auto& event){ timerCallback(event); }); + timer_duration, +#if __cplusplus < 201100L + boost::bind(&TimeredDiagnosticUpdater::timerCallback,this,_1) +#else + [this](auto& event){ timerCallback(event); } +#endif + ); timer_.stop(); } diff --git a/jsk_topic_tools/src/topic_buffer_client.cpp b/jsk_topic_tools/src/topic_buffer_client.cpp index d5724c750..5ab8db2d3 100644 --- a/jsk_topic_tools/src/topic_buffer_client.cpp +++ b/jsk_topic_tools/src/topic_buffer_client.cpp @@ -201,8 +201,12 @@ int main(int argc, char **argv) pub_info->topic_with_header = false; ROS_INFO_STREAM("subscribe " << pub_info->topic_name+string("_update")); pub_info->sub = new ros::Subscriber(n.subscribe(pub_info->topic_name+string("_update"), 10, - [pub_info](auto& msg){ in_cb(msg, pub_info); })); - +#if __cplusplus < 201100L + boost::bind(in_cb, _1, pub_info) +#else + [pub_info](auto& msg){ in_cb(msg, pub_info); } +#endif + )); g_pubs.push_back(pub_info); } diff --git a/jsk_topic_tools/src/topic_buffer_server.cpp b/jsk_topic_tools/src/topic_buffer_server.cpp index f78b7f77b..b79b0f2a5 100644 --- a/jsk_topic_tools/src/topic_buffer_server.cpp +++ b/jsk_topic_tools/src/topic_buffer_server.cpp @@ -219,8 +219,13 @@ int main(int argc, char **argv) sub_info->advertised = false; sub_info->periodic = false; ROS_INFO_STREAM("subscribe " << sub_info->topic_name); - sub_info->sub = new ros::Subscriber(n.subscribe(sub_info->topic_name, 10, [sub_info](auto& msg){ in_cb(msg, sub_info); })); - + sub_info->sub = new ros::Subscriber(n.subscribe(sub_info->topic_name, 10, +#if __cplusplus < 201100L + boost::bind(in_cb, _1, sub_info) +#else + [sub_info](auto& msg){ in_cb(msg, sub_info); } +#endif + )); // waiting for all topics are publisherd while (sub_info->sub->getNumPublishers() == 0 ) { ros::Duration(1.0).sleep(); From 4dd2c68e18403f0e7705851a26ffab6ff5dfdbf9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 13 Dec 2022 00:40:46 +0900 Subject: [PATCH 39/39] inidgo uses setuptools==44.1.1 for https://github.com/jsk-ros-pkg/jsk_common/pull/1773/commits/f834c0a86a2d19013675e60ffbb2c26ba5b87e6c --- .github/workflows/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/config.yml b/.github/workflows/config.yml index 510c54d4d..d42f9826f 100644 --- a/.github/workflows/config.yml +++ b/.github/workflows/config.yml @@ -20,7 +20,7 @@ jobs: ROS_PARALLEL_TEST_JOBS: "-j8" # latest catkin_virtualenv with pip==21.0.1 is incompatible with python 2.x # https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/237 - BEFORE_SCRIPT : "sudo pip install virtualenv==15.1.0" + BEFORE_SCRIPT : "sudo pip install virtualenv==15.1.0 setuptools==44.1.1" - ROS_DISTRO: kinetic CONTAINER: ubuntu:16.04 ROS_PARALLEL_TEST_JOBS: "-j8"