|
| 1 | +# Copyright 2023 Clearpath Robotics |
| 2 | +# |
| 3 | +# Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +# you may not use this file except in compliance with the License. |
| 5 | +# You may obtain a copy of the License at |
| 6 | +# |
| 7 | +# http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +# |
| 9 | +# Unless required by applicable law or agreed to in writing, software |
| 10 | +# distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +# See the License for the specific language governing permissions and |
| 13 | +# limitations under the License. |
| 14 | +from datetime import datetime |
| 15 | +from rcl_interfaces.msg import Log |
| 16 | +from ros2cli.node.strategy import NodeStrategy |
| 17 | +from ros2cli.node.strategy import add_arguments |
| 18 | +from ros2rosout.verb import VerbExtension |
| 19 | +import rclpy |
| 20 | +import re |
| 21 | + |
| 22 | + |
| 23 | +class PrintVerb(VerbExtension): |
| 24 | + """Outputs the '/rosout' content in a nicely formatted way""" |
| 25 | + |
| 26 | + BLACK_TEXT = "\033[30;1m" |
| 27 | + BLUE_TEXT = "\033[34;1m" |
| 28 | + BOLD_TEXT = "\033[1m" |
| 29 | + CYAN_TEXT = "\033[36;1m" |
| 30 | + GREEN_TEXT = "\033[32;1m" |
| 31 | + MAGENTA_TEXT = "\033[35;1m" |
| 32 | + RED_TEXT = "\033[31;1m" |
| 33 | + WHITE_TEXT = "\033[37;1m" |
| 34 | + YELLOW_TEXT = "\033[33;1m" |
| 35 | + |
| 36 | + COLOR_RESET = "\033[0m" |
| 37 | + |
| 38 | + def add_arguments(self, parser, cli_name): |
| 39 | + add_arguments(parser) |
| 40 | + parser.add_argument( |
| 41 | + '-l', '--level', default=int.from_bytes(Log.INFO, 'big'), type=int, |
| 42 | + help='Print log statement with priority level greater than this value') |
| 43 | + parser.add_argument( |
| 44 | + '-n', '--node-regex', default=None, |
| 45 | + help='Only print log statements from node(s) matching the regular expression provided') |
| 46 | + parser.add_argument( |
| 47 | + '--no-color', action='store_true', default=False, |
| 48 | + help='Disables the use of ASCII colors for the output of the command') |
| 49 | + parser.add_argument( |
| 50 | + '-f', '--function-detail', action='store_true', default=False, |
| 51 | + help='Output function name, file, and line number') |
| 52 | + |
| 53 | + def level_to_string(self, level): |
| 54 | + if type(level) is not bytes: |
| 55 | + level = level.to_bytes(1, 'big') |
| 56 | + |
| 57 | + match level: |
| 58 | + case Log.DEBUG: |
| 59 | + return "DEBUG" |
| 60 | + case Log.INFO: |
| 61 | + return "INFO " |
| 62 | + case Log.WARN: |
| 63 | + return "WARN " |
| 64 | + case Log.ERROR: |
| 65 | + return "ERROR" |
| 66 | + case Log.FATAL: |
| 67 | + return "FATAL" |
| 68 | + case _: |
| 69 | + return "_____" |
| 70 | + |
| 71 | + def stamp_to_string(self, stamp): |
| 72 | + dt = datetime.fromtimestamp(stamp.sec) |
| 73 | + s = dt.strftime('%Y-%m-%d %H:%M:%S') |
| 74 | + s += '.' + str(int(stamp.nanosec % 1000000000)).zfill(9) |
| 75 | + return f"{s}" |
| 76 | + |
| 77 | + def add_color(self, txt, color=WHITE_TEXT): |
| 78 | + if self.args_.no_color: |
| 79 | + return txt |
| 80 | + |
| 81 | + return f"{color}{txt}{self.COLOR_RESET}" |
| 82 | + |
| 83 | + def get_color(self, level): |
| 84 | + if type(level) is not bytes: |
| 85 | + level = level.to_bytes(1, 'big') |
| 86 | + |
| 87 | + match level: |
| 88 | + case Log.DEBUG: |
| 89 | + return self.GREEN_TEXT |
| 90 | + case Log.INFO: |
| 91 | + return self.WHITE_TEXT |
| 92 | + case Log.WARN: |
| 93 | + return self.YELLOW_TEXT |
| 94 | + case Log.ERROR: |
| 95 | + return self.RED_TEXT |
| 96 | + case Log.FATAL: |
| 97 | + return f"{self.RED_TEXT}{self.BOLD_TEXT}" |
| 98 | + case _: |
| 99 | + return self.BOLD_TEXT |
| 100 | + |
| 101 | + def rosout_cb(self, msg): |
| 102 | + if msg.level < self.args_.level: |
| 103 | + return |
| 104 | + if self.args_.node_regex and not re.search(self.args_.node_regex, msg.name): |
| 105 | + return |
| 106 | + color = self.get_color(msg.level) |
| 107 | + lvl = self.add_color(self.level_to_string(msg.level), color) |
| 108 | + dt = self.add_color(self.stamp_to_string(msg.stamp), color) |
| 109 | + name = self.add_color(msg.name, color) |
| 110 | + mmsg = self.add_color(msg.msg, color) |
| 111 | + text = f"[{dt}] [{lvl}] [{name}]: {mmsg}" |
| 112 | + if self.args_.function_detail: |
| 113 | + file = self.add_color(msg.file, self.BLUE_TEXT) |
| 114 | + line = self.add_color(msg.line, self.BLUE_TEXT) |
| 115 | + function = self.add_color(msg.function, self.CYAN_TEXT) |
| 116 | + text += f" [{file}:{line}({function})]" |
| 117 | + print(f"{text}") |
| 118 | + |
| 119 | + def main(self, *, args): |
| 120 | + self.args_ = args |
| 121 | + |
| 122 | + with NodeStrategy(args) as node: |
| 123 | + self.rosout_ = node.node.create_subscription(Log, '/rosout', self.rosout_cb, 10) |
| 124 | + rclpy.spin(node) |
0 commit comments