diff --git a/.gitignore b/.gitignore index 0ec2884b4149cf669cfbc0834e762dd3cb3f13af..401408a3b2bcbbb24de426fbb994776d7a7af991 100644 --- a/.gitignore +++ b/.gitignore @@ -208,4 +208,17 @@ __pycache__/ #Cred stuff *cred.json -*.egg* \ No newline at end of file +<<<<<<< HEAD +!src/lhw_gui/install/ +!src/lhw_gui/build/ +======= +# +*.egg* + + +# Except lhw_gui stuff +#!src/lhw_gui/install/ +#!src/lhw_gui/build/ +>>>>>>> eb92ecb3fb61903ac8ef8323b58a6e82610d855d +!src/lhw_gui/bin/ +!src/lhw_gui/lib/ diff --git a/src/lhw_gui/bin/rosbridge.js b/src/lhw_gui/bin/rosbridge.js new file mode 100755 index 0000000000000000000000000000000000000000..d71a3c65c1ebbc0a84f375d211486eda893227ac --- /dev/null +++ b/src/lhw_gui/bin/rosbridge.js @@ -0,0 +1,38 @@ +#!/usr/bin/env node + +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const rosbridge = require('../index.js'); +const app = require('commander'); +const pkg = require('../package.json'); + +app + .version(pkg.version) + .option('-p, --port [port_number]', 'Listen port, default to :9090') + .option('-a, --address [address_string]', 'Remote server address (client mode); server mode if unset') + .option('-r, --retry_startup_delay [delay_ms]', 'Retry startup delay in millisecond') + .option('-o, --fragment_timeout [timeout_ms]', 'Fragment timeout in millisecond') + .option('-d, --delay_between_messages [delay_ms]', 'Delay between messages in millisecond') + .option('-m, --max_message_size [byte_size]', 'Max message size') + .option('-t, --topics_glob [glob_list]', 'A list or None') + .option('-s, --services_glob [glob_list]', 'A list or None') + .option('-g, --params_glob [glob_list]', 'A list or None') + .option('-b, --bson_only_mode', 'Unsupported in WebSocket server, will be ignored') + .option('-l, --status_level [level_string]', 'Status level (one of "error", "warning", "info", "none"; default "error")') + .parse(process.argv); + +rosbridge.createServer(app); diff --git a/src/lhw_gui/install/.colcon_install_layout b/src/lhw_gui/install/.colcon_install_layout new file mode 100644 index 0000000000000000000000000000000000000000..3aad5336af1f22b8088508218dceeda3d7bc8cc2 --- /dev/null +++ b/src/lhw_gui/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/src/lhw_gui/install/COLCON_IGNORE b/src/lhw_gui/install/COLCON_IGNORE new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/src/lhw_gui/install/_local_setup_util_ps1.py b/src/lhw_gui/install/_local_setup_util_ps1.py new file mode 100644 index 0000000000000000000000000000000000000000..b3e16ebe67d1d541cc7a1463f355657c372a691c --- /dev/null +++ b/src/lhw_gui/install/_local_setup_util_ps1.py @@ -0,0 +1,376 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' + +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_trailing_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = 'skip extending {env_name} with not existing path: ' \ + '{value}'.format_map(locals()) + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_trailing_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_trailing_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map( + {'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/src/lhw_gui/install/_local_setup_util_sh.py b/src/lhw_gui/install/_local_setup_util_sh.py new file mode 100644 index 0000000000000000000000000000000000000000..07a8cbea68d89fa347e6cc2785d57c076ff75ed9 --- /dev/null +++ b/src/lhw_gui/install/_local_setup_util_sh.py @@ -0,0 +1,376 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' + +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_trailing_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = 'skip extending {env_name} with not existing path: ' \ + '{value}'.format_map(locals()) + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_trailing_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_trailing_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map( + {'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/src/lhw_gui/install/local_setup.bash b/src/lhw_gui/install/local_setup.bash new file mode 100644 index 0000000000000000000000000000000000000000..efd5f8c9e24546b7d9b90d2e7928ea126de164e3 --- /dev/null +++ b/src/lhw_gui/install/local_setup.bash @@ -0,0 +1,107 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo ". \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "Execute generated script:" + echo "<<<" + echo "${_colcon_ordered_commands}" + echo ">>>" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/src/lhw_gui/install/local_setup.ps1 b/src/lhw_gui/install/local_setup.ps1 new file mode 100644 index 0000000000000000000000000000000000000000..229ea75370dfe8d5996101c8204e1821989a2571 --- /dev/null +++ b/src/lhw_gui/install/local_setup.ps1 @@ -0,0 +1,53 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +$_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression diff --git a/src/lhw_gui/install/local_setup.sh b/src/lhw_gui/install/local_setup.sh new file mode 100644 index 0000000000000000000000000000000000000000..f6a5c1d0c0595d7896ec6a5d6d0a72cafc157bca --- /dev/null +++ b/src/lhw_gui/install/local_setup.sh @@ -0,0 +1,114 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/daniel/liu-home-wreckers/src/lhw_gui/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo ". \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "Execute generated script:" + echo "<<<" + echo "${_colcon_ordered_commands}" + echo ">>>" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/src/lhw_gui/install/local_setup.zsh b/src/lhw_gui/install/local_setup.zsh new file mode 100644 index 0000000000000000000000000000000000000000..f7a8d904f2019736b6114cec0f6250da480c415c --- /dev/null +++ b/src/lhw_gui/install/local_setup.zsh @@ -0,0 +1,120 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo ". \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "Execute generated script:" + echo "<<<" + echo "${_colcon_ordered_commands}" + echo ">>>" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/src/lhw_gui/install/setup.bash b/src/lhw_gui/install/setup.bash new file mode 100644 index 0000000000000000000000000000000000000000..ccf107a9432e72b861bd95c0963d39b0de06df78 --- /dev/null +++ b/src/lhw_gui/install/setup.bash @@ -0,0 +1,40 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo ". \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/foxy" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/ros2_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/exjobb/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/liu-home-wreckers/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/src/lhw_gui/install/setup.ps1 b/src/lhw_gui/install/setup.ps1 new file mode 100644 index 0000000000000000000000000000000000000000..b77d1bd7ab45e9787203250c7ec519963e00c24e --- /dev/null +++ b/src/lhw_gui/install/setup.ps1 @@ -0,0 +1,32 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/foxy\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/daniel/ros2_ws/install\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/daniel/exjobb/install\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/daniel/liu-home-wreckers/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/src/lhw_gui/install/setup.sh b/src/lhw_gui/install/setup.sh new file mode 100644 index 0000000000000000000000000000000000000000..9e9899085772fa5fb40a902baca2e8175f392c43 --- /dev/null +++ b/src/lhw_gui/install/setup.sh @@ -0,0 +1,57 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/daniel/liu-home-wreckers/src/lhw_gui/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo ". \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/foxy" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/ros2_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/exjobb/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/liu-home-wreckers/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/src/lhw_gui/install/setup.zsh b/src/lhw_gui/install/setup.zsh new file mode 100644 index 0000000000000000000000000000000000000000..6707a6e09c0cedc11b915ac485630fab67080ca8 --- /dev/null +++ b/src/lhw_gui/install/setup.zsh @@ -0,0 +1,40 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo ". \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/foxy" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/ros2_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/exjobb/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/daniel/liu-home-wreckers/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/src/lhw_gui/lhw_gui/css/goal.css b/src/lhw_gui/lhw_gui/css/goal.css index c9a5a898e4b388702ff5108e6e153814094b7bb3..76a7efaa4b0c83470baba6f9083a7cf390f56a03 100644 --- a/src/lhw_gui/lhw_gui/css/goal.css +++ b/src/lhw_gui/lhw_gui/css/goal.css @@ -20,4 +20,42 @@ h1 { .main{ width: 70%; +} + +.behaviour-container{ + display: flex; + justify-content: center; + flex-direction: column; + width: 40%; + align-items: center; + margin: 1rem 0rem; + border: solid 1px black; + border-radius: 2rem; + padding: 1rem; + background: white; +} + +#stateMachine{ + display: flex; + justify-content: center; + flex-direction: column; + align-items: center; +} +.input_data { + display: flex; + justify-content: space-between; + width: 100%; + border-bottom: dashed 1px gray; + padding: 0.4rem; +} + +.title { + margin: 3rem 0rem; + font-size: 1.5rem; +} + +.buttons { + display: flex; + width: 100%; + justify-content: space-evenly; } \ No newline at end of file diff --git a/src/lhw_gui/lhw_gui/goal.js b/src/lhw_gui/lhw_gui/goal.js index a73ef319c35c3cd9922befb2ffbf5c064b6cf562..67324a4e504fc9a126659a046929f1cb4dac1d61 100644 --- a/src/lhw_gui/lhw_gui/goal.js +++ b/src/lhw_gui/lhw_gui/goal.js @@ -1,13 +1,41 @@ -// Topics +//================ +//-----Topics----- var goal_publisher = new ROSLIB.Topic({ ros : ros, name : '/sm_goal', messageType : 'lhw_interfaces/Behaviour' }); +//================ - +//================ +//Global variables +var sm = new StateMachine() + +var behaviour_definitions = []; +$.ajax({ + async: false, + url: '../goal/Behaviours.json', + success: function(definitions) { + behaviour_definitions = definitions.Behaviours + } +}); + +var editor = new Editor(behaviour_definitions) + +//================ + +function updateUI(){ + document.getElementById("stateMachine").innerHTML = sm.getHTML(); + document.getElementById("editor").innerHTML = editor.getHTML(); +} + +updateUI() + + + +/* function send(){ let behaviour = document.getElementById('behaviours').value let specific_id_value = document.getElementById('specific_id').value == "yes" @@ -67,3 +95,4 @@ document.getElementById('specific_id_container').style.display = "none" }; +*/ \ No newline at end of file diff --git a/src/lhw_gui/lhw_gui/goal/Behaviour.js b/src/lhw_gui/lhw_gui/goal/Behaviour.js new file mode 100644 index 0000000000000000000000000000000000000000..de6beecaf46a5163bc9e7794822fd9a50726b29b --- /dev/null +++ b/src/lhw_gui/lhw_gui/goal/Behaviour.js @@ -0,0 +1,31 @@ +class Behaviour { + constructor(id, parent, label = "Undefined") { + this.id = id + this.parent = parent + this.label = label + this.name = false + this.input_data = [] + } + + getHTML(){ + var input_data = "" + this.input_data.forEach( input => { + if (!input.value){ + input.value = "" + } + input_data += "<div class='input_data'> <div class=input_data-label>" + input.label + ":</div> <div class=input_data-value>" + input.value + "</div></div>" + }) + + var name = "<div class='title'> " + this.label + "</div>"; + var editbutton = "" + if (this.id > 0) { + editbutton ="<button onclick='editor.open(" + this.id + " )'> Edit </button>"; + } + + var nextbutton ="<button onclick='sm.addNext(" + this.id + " )'> Add next </button>"; + var html = "<div class='behaviour-container'>" + input_data + name + "<div class='buttons'>" + editbutton + nextbutton + "</div></div>" + return html + } + + +} diff --git a/src/lhw_gui/lhw_gui/goal/Behaviours.json b/src/lhw_gui/lhw_gui/goal/Behaviours.json new file mode 100644 index 0000000000000000000000000000000000000000..bd498f574c32e07d487c27e5bd0c399f57c919f2 --- /dev/null +++ b/src/lhw_gui/lhw_gui/goal/Behaviours.json @@ -0,0 +1,41 @@ +{ + "Behaviours": [ + { + "label": "Undefined", + "name": false, + "input_data": [] + }, + { + "label": "Find Person", + "name": "find_person:FindPerson", + "input_data": [ + { + "data": "specificid", + "label": "Find specific person?", + "type": "yesno" + }, + { + "data": "id", + "label": "If yes, person ID", + "type": "number" + } + ] + }, + { + "label": "Have A Conversation", + "name": "have_a_conversation:HaveAConversation", + "input_data": [ + { + "data": "type", + "label": "Stop on intention", + "type": "text" + }, + { + "data": "atstart", + "label": "Say at start", + "type": "text" + } + ] + } + ] +} \ No newline at end of file diff --git a/src/lhw_gui/lhw_gui/goal/Editor.js b/src/lhw_gui/lhw_gui/goal/Editor.js new file mode 100644 index 0000000000000000000000000000000000000000..b76c76de6baefdf730257dbf5c7dd5f30caa5d66 --- /dev/null +++ b/src/lhw_gui/lhw_gui/goal/Editor.js @@ -0,0 +1,67 @@ + +class Editor { + constructor(behaviour_definitions) { + this.behaviour_definitions = behaviour_definitions + this.active = false + } + + updateBehaviour(behaviour_idx){ + var new_bahaviour = JSON.parse(JSON.stringify(this.behaviour_definitions[behaviour_idx] )) + this.active.label = new_bahaviour.label + this.active.name = new_bahaviour.name + this.active.input_data = new_bahaviour.input_data + updateUI(); + } + + open(behaviour_id){ + this.active = sm.getBehaviourById(behaviour_id) + updateUI(); + }; + + save(){ + this.active.input_data.forEach((input, idx) => { + this.active.input_data[idx].value = document.getElementById(input.data).value + }) + this.active = false + updateUI() + } + + getHTML(){ + if (this.active){ + + var title = "<h2>Editing Behaviour</h2>"; + + var selectBehaviour = '<select name="behaviours" onchange="editor.updateBehaviour(this.value)">'; + selectBehaviour += "<option value='' selected disabled hidden>" + this.active.label + "</option>" + this.behaviour_definitions.forEach((behaviour, idx) => { + selectBehaviour += "<option value=" + idx + ">" + behaviour.label + "</option>" + }); + selectBehaviour += "</select>"; + + var input_data_fields = "" + if (this.active.name){ + this.active.input_data.forEach((input, idx) => { + if (input.type == "yesno"){ + input_data_fields += "<div>" + input.label + "<select id=" + input.data + "> <option value='yes'>Yes</option> <option value='no'>No</option></select>" + } + else { + if (!input.value){ + input.value = "" + } + input_data_fields += "<div> "+ input.label + ": <input type='text' id=" + input.data + " value='" + input.value + "' > </div>" + } + }); + + } + + var button = "<button onclick='editor.save()'> Save </button>"; + + return "<div>" + title + selectBehaviour + input_data_fields + button + " </div>" + + } + else { + return "" + } + } + + } \ No newline at end of file diff --git a/src/lhw_gui/lhw_gui/goal/StateMachine.js b/src/lhw_gui/lhw_gui/goal/StateMachine.js new file mode 100644 index 0000000000000000000000000000000000000000..6efff804f5dcba2c4ae3568a44cb14cd80a0c50f --- /dev/null +++ b/src/lhw_gui/lhw_gui/goal/StateMachine.js @@ -0,0 +1,60 @@ +class StateMachine{ + constructor(behaviours) { + this.behaviours = [new Behaviour(0, false, "Start")] + this.active_idx = false + } + + getBehaviourById(id){ + return this.behaviours.find(behaviour => behaviour.id == id); + } + + addNext(parent){ + var new_id = sm.behaviours.length + this.behaviours.splice(parent + 1, 0, new Behaviour(new_id, parent)); + editor.open(new_id) + }; + + start(){ + this.active_idx = 1 + this.send_goal() + } + + getDataValue(goal, name){ + var data = goal.input_data.find( input => input.data == name) + if (!data){ + return "" + } + + if (data.type == "yesno"){ + return data.value == "yes" + } + + return data.value + + } + + send_goal() { + var goal = this.behaviours[this.active_idx] + goal_publisher.publish({ + what: goal.name, + specificid: this.getDataValue(goal, "specificid"), + specifictype: this.getDataValue(goal, "specifictype"), + id: this.getDataValue(goal, "id"), + type: this.getDataValue(goal, "type"), + mapname: this.getDataValue(goal, "mapname"), + atstart: this.getDataValue(goal, "atstart"), + behaviourtimeoutinsec: this.getDataValue(goal, "behaviourtimeoutinsec"), + }); + } + + getHTML(){ + var html = '' + html += "<button onclick='sm.start()'> START </button>"; + this.behaviours.forEach(behaviour => { + html += behaviour.getHTML(); + }) + + return html + } + +} \ No newline at end of file diff --git a/src/lhw_gui/lhw_gui/html/goal.html b/src/lhw_gui/lhw_gui/html/goal.html index a1d374e232fb829601e8b1ef3c26d11e89f8f5f2..11b4abd55175baf70164fe2f820ed119f42cf786 100644 --- a/src/lhw_gui/lhw_gui/html/goal.html +++ b/src/lhw_gui/lhw_gui/html/goal.html @@ -5,6 +5,10 @@ <script src="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script> <script src="https://static.robotwebtools.org/roslibjs/current/roslib.js"></script> <script src="https://ajax.googleapis.com/ajax/libs/jquery/2.1.1/jquery.min.js"></script> +<script src="../goal/Behaviour.js"> </script> +<script src="../goal/Editor.js"> </script> +<script src="../goal/StateMachine.js"> </script> + <script src="../connect_to_ros.js"> </script> <link rel="stylesheet" href="../css/goal.css"> </head> @@ -30,45 +34,13 @@ Connection closed. </p> </div> - <div> - Choose behaviour - <select name="behaviours" id="behaviours" onchange="behaviourUpdate(this.value)"> - <option value="none">--Choose Behaviour--</option> - <option value="find_person:FindPerson">Find Person</option> - <option value="categorize:Categorize">Categorize</option> - <option value="explore_room:ExploreRoom">Explore Room</option> - <option value="follow_person:FollowPerson">Follow Person</option> - <option value="go_to_entity:GoToEntity">Go to Entity</option> - <option value="have_a_conversation:HaveAConversation">Have a Conversation</option> - </select> - </div> - <div id="specific_id_container"> - Look after specific id? <select id="specific_id"> - <option value="yes">Yes</option> - <option value="no">No</option> - </select> - </div> - <div id="specific_type_container"> - Look after specific type? <select id="specific_type"> - <option value="yes">Yes</option> - <option value="no">No</option> - </select> - </div> - <div id="id_container">ID: <input type="text" id="id" /></div> - <div id="type_container">Type: <input type="text" id="type" /></div> - <div id="map_name_container">Map file name: <input type="text" id="map_name" /></div> - <div id="at_start_container">At start: <input type="text" id="at_start" /></div> - <div id="behaviour_timeout_in_sec_container">Max time (sec):<input type="text" id="behaviour_timeout_in_sec" /></div> - <button id="send_button"> - Send goal - </button> + <div id="editor"></div> </div> <div class="main"> - hej + <div id="stateMachine"></div> </div> </div> - <script src="../goal.js"></script> </body> </html> diff --git a/src/lhw_gui/lib/bridge.js b/src/lhw_gui/lib/bridge.js new file mode 100644 index 0000000000000000000000000000000000000000..c6f2d6863b98cc5c09dc7e5a60fb28e910004a6e --- /dev/null +++ b/src/lhw_gui/lib/bridge.js @@ -0,0 +1,335 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const ResourceProvider = require('./resource_provider.js'); +const debug = require('debug')('ros2-web-bridge:Bridge'); +const EventEmitter = require('events'); +const uuidv4 = require('uuid/v4'); +const {validator} = require('rclnodejs'); + +const STATUS_LEVELS = ['error', 'warning', 'info', 'none']; + +class MessageParser { + constructor() { + this._buffer = ''; + } + + process(message) { + // The logic below is translated from the current implementation of rosbridge_suit, + // see https://github.com/RobotWebTools/rosbridge_suite/blob/develop/rosbridge_library/src/rosbridge_library/protocol.py + this._buffer += message; + let msg = null; + try { + msg = JSON.parse(this._buffer); + this._buffer = ''; + } + catch (e) { + if (e instanceof SyntaxError) { + let openingBrackets = this._buffer.indexOf('{'); + let closingBrackets = this._buffer.indexOf('}'); + + for (let start = 0; start <= openingBrackets; start++) { + for (let end = 0; end <= closingBrackets; end++) { + try { + msg = JSON.parse(this._buffer.substring(start, end + 1)); + if (msg.op) { + self._buffer = self._buffer.substr(end + 1, this._buffer.length); + break; + } + } + catch (e) { + if (e instanceof SyntaxError) { + continue; + } + } + } + if (msg) { + break; + } + } + } + } + return msg; + } +} + +class Bridge extends EventEmitter { + + constructor(node, ws, statusLevel) { + super(); + this._ws = ws; + this._parser = new MessageParser(); + this._bridgeId = this._generateRandomId(); + this._servicesResponse = new Map(); + this._closed = false; + this._resourceProvider = new ResourceProvider(node, this._bridgeId); + this._registerConnectionEvent(ws); + this._rebuildOpMap(); + this._topicsPublished = new Map(); + this._setStatusLevel(statusLevel || 'error'); + debug(`Web bridge ${this._bridgeId} is created`); + } + + _registerConnectionEvent(ws) { + ws.on('message', (message) => { + this._receiveMessage(message); + }); + + ws.on('close', () => { + this.close(); + this.emit('close', this._bridgeId); + debug(`Web bridge ${this._bridgeId} is closed`); + }); + + ws.on('error', (error) => { + this.emit('error', error); + debug(`Web socket of bridge ${this._bridgeId} error: ${error}`); + }); + } + + close() { + if (!this._closed) { + this._resourceProvider.clean(); + this._servicesResponse.clear(); + this._topicsPublished.clear(); + this._closed = true; + } + } + + _generateRandomId() { + return uuidv4(); + } + + _exractMessageType(type) { + if (type.indexOf('/msg/') === -1) { + const splitted = type.split('/'); + return splitted[0] + '/msg/' + splitted[1]; + } + return type; + } + + _exractServiceType(type) { + if (type.indexOf('/srv/') === -1) { + const splitted = type.split('/'); + return splitted[0] + '/srv/' + splitted[1]; + } + return type; + } + + _receiveMessage(message) { + const command = this._parser.process(message); + if (!command) return; + + debug(`JSON command received: ${JSON.stringify(command)}`); + this.executeCommand(command); + } + + get bridgeId() { + return this._bridgeId; + } + + get closed() { + return this._closed; + } + + _registerOpMap(opCode, callback) { + this._opMap = this._opMap || {}; + + if (this._opMap[opCode]) { + debug(`Warning: existing callback of '${opCode}'' will be overwritten by new callback`); + } + this._opMap[opCode] = callback; + } + + _rebuildOpMap() { + this._registerOpMap('set_level', (command) => { + if (STATUS_LEVELS.indexOf(command.level) === -1) { + throw new Error(`Invalid status level ${command.level}; must be one of ${STATUS_LEVELS}`); + } + this._setStatusLevel(command.level); + }); + + this._registerOpMap('advertise', (command) => { + let topic = command.topic; + if (this._topicsPublished.has(topic) && (this._topicsPublished.get(topic) !== command.type)) { + throw new Error(`The topic ${topic} already exists with a different type ${this._topicsPublished.get(topic)}.`); + } + debug(`advertise a topic: ${topic}`); + this._topicsPublished.set(topic, command.type); + this._resourceProvider.createPublisher(this._exractMessageType(command.type), topic); + }); + + this._registerOpMap('unadvertise', (command) => { + let topic = command.topic; + this._validateTopicOrService(command.topic); + + if (!this._topicsPublished.has(topic)) { + let error = new Error(`The topic ${topic} does not exist`); + error.level = 'warning'; + throw error; + } + debug(`unadvertise a topic: ${topic}`); + this._topicsPublished.delete(topic); + this._resourceProvider.destroyPublisher(topic); + }); + + this._registerOpMap('publish', (command) => { + debug(`Publish a topic named ${command.topic} with ${JSON.stringify(command.msg)}`); + + if (!this._topicsPublished.has(command.topic)) { + let error = new Error(`The topic ${command.topic} does not exist`); + error.level = 'error'; + throw error; + } + let publisher = this._resourceProvider.getPublisherByTopicName(command.topic); + if (publisher) { + publisher.publish(command.msg); + } + }); + + this._registerOpMap('subscribe', (command) => { + debug(`subscribe a topic named ${command.topic}`); + + this._resourceProvider.createSubscription(this._exractMessageType(command.type), + command.topic, + this._sendSubscriptionResponse.bind(this)); + }); + + this._registerOpMap('unsubscribe', (command) => { + let topic = command.topic; + this._validateTopicOrService(topic); + + if (!this._resourceProvider.hasSubscription(topic)) { + let error = new Error(`The topic ${topic} does not exist.`); + error.level = 'warning'; + throw error; + } + debug(`unsubscribe a topic named ${topic}`); + this._resourceProvider.destroySubscription(command.topic); + }); + + this._registerOpMap('call_service', (command) => { + let serviceName = command.service; + let client = + this._resourceProvider.createClient(this._exractServiceType(command.type), serviceName); + + if (client) { + client.sendRequest(command.args, (response) => { + let serviceResponse = + {op: 'service_response', service: command.service, values: response, id: command.id, result: true}; + + this._ws.send(JSON.stringify(serviceResponse)); + }); + } + }); + + this._registerOpMap('advertise_service', (command) => { + let serviceName = command.service; + let service = this._resourceProvider.createService( + this._exractServiceType(command.type), + serviceName, + (request, response) => { + let id = this._generateRandomId(); + let serviceRequest = {op: 'call_service', service: command.service, args: request, id: id}; + this._servicesResponse.set(id, response); + this._ws.send(JSON.stringify(serviceRequest)); + }); + }); + + this._registerOpMap('service_response', (command) => { + let serviceName = command.service; + let id = command.id; + let response = this._servicesResponse.get(id); + if (response) { + response.send(command.values); + this._servicesResponse.delete(id); + } + }); + + this._registerOpMap('unadvertise_service', (command) => { + let serviceName = command.service; + this._validateTopicOrService(serviceName); + + if (!this._resourceProvider.hasService(serviceName)) { + let error = new Error(`The service ${serviceName} does not exist.`); + error.level = 'warning'; + throw error; + } + debug(`unadvertise a service: ${serviceName}`); + this._resourceProvider.destroyService(command.service); + }); + } + + executeCommand(command) { + try { + const op = this._opMap[command.op]; + if (!op) { + throw new Error(`Operation ${command.op} is not supported`); + } + op.apply(this, [command]); + this._sendBackOperationStatus(command.id, 'none', 'OK'); + } catch (e) { + e.id = command.id; + e.op = command.op; + this._sendBackErrorStatus(e); + } + } + + _sendSubscriptionResponse(topicName, message) { + debug('Send message to subscription.'); + let response = {op: 'publish', topic: topicName, msg: message}; + this._ws.send(JSON.stringify(response)); + } + + _sendBackErrorStatus(error) { + const msg = `${error.op}: ${error}`; + return this._sendBackOperationStatus(error.id, error.level || 'error', msg); + } + + _sendBackOperationStatus(id, level, msg) { + let command = { + op: 'status', + level: level || 'none', + msg: msg || '', + id: id, + }; + if (this._statusLevel < STATUS_LEVELS.indexOf(level)) { + debug('Suppressed: ' + JSON.stringify(command)); + return; + } + debug('Response: ' + JSON.stringify(command)); + this._ws.send(JSON.stringify(command)); + } + + _setStatusLevel(level) { + this._statusLevel = STATUS_LEVELS.indexOf(level); + debug(`Status level set to ${level} (${this._statusLevel})`); + } + + _validateTopicOrService(name) { + if (name.startsWith('/')) { + validator.validateFullTopicName(name); + } else { + validator.validateTopicName(name); + } + } + + get ws() { + return this._ws; + } +} + +module.exports = Bridge; diff --git a/src/lhw_gui/lib/ref_counting_handle.js b/src/lhw_gui/lib/ref_counting_handle.js new file mode 100644 index 0000000000000000000000000000000000000000..53365dc1c604cba56baaba399842c67e5b1c89b5 --- /dev/null +++ b/src/lhw_gui/lib/ref_counting_handle.js @@ -0,0 +1,60 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const debug = require('debug')('ros2-web-bridge:RefCountingHandle'); + +class RefCountingHandle { + constructor(object, destroyHandle) { + if (object) { + this._object = object; + this._count = 1; + this._destroyHandle = destroyHandle; + } + } + + get() { + return this._object; + } + + release() { + if (this._count > 0) { + if (--this._count === 0) { + this._destroyHandle(this._object); + this._object = undefined; + debug('Handle is destroyed.'); + } + } + } + + retain() { + this._count++; + } + + destroy() { + if (this._count > 0) { + this._destroyHandle(this._object); + this._count = 0; + this._object = undefined; + debug('Handle is destroyed.'); + } + } + + get count() { + return this._count; + } +} + +module.exports = RefCountingHandle; diff --git a/src/lhw_gui/lib/resource_provider.js b/src/lhw_gui/lib/resource_provider.js new file mode 100644 index 0000000000000000000000000000000000000000..252d121886de932f0553e7fad64da3e8993ae9e5 --- /dev/null +++ b/src/lhw_gui/lib/resource_provider.js @@ -0,0 +1,153 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const SubscriptionManager = require('./subscription_manager.js'); +const RefCountingHandle = require('./ref_counting_handle.js'); +const debug = require('debug')('ros2-web-bridge:ResourceProvider'); + +class ResourceProvider { + constructor(node, bridgeId) { + SubscriptionManager.init(node); + this._bridgeId = bridgeId; + this._node = node; + this._publishers = new Map(); + this._clients = new Map(); + this._services = new Map(); + } + + getPublisherByTopicName(topicName) { + return this._publishers.get(topicName).get(); + } + + getSubscriptionByTopicName(topicName) { + return SubscriptionManager.getInstance().getSubscriptionByTopicName(topicName).get(); + } + + getClientByServiceName(serviceName) { + return this._clients.get(serviceName).get(); + } + + getServiceByServiceName(serviceName) { + return this._services.get(serviceName).get(); + } + + createPublisher(messageType, topicName) { + let handle = this._publishers.get(topicName); + if (!handle) { + handle = new RefCountingHandle(this._node.createPublisher(messageType, topicName), + this._node.destroyPublisher.bind(this._node)); + this._publishers.set(topicName, handle); + debug(`Publisher has been created, and the topic name is ${topicName}.`); + } else { + handle.retain(); + } + return handle.get(); + } + + createSubscription(messageType, topicName, callback) { + return SubscriptionManager.getInstance().createSubscription(messageType, topicName, this._bridgeId, callback); + } + + createClient(serviceType, serviceName) { + let handle = this._clients.get(serviceName); + if (!handle) { + handle = new RefCountingHandle(this._node.createClient(serviceType, serviceName, {enableTypedArray: false}), + this._node.destroyClient.bind(this._node)); + this._clients.set(serviceName, handle); + debug(`Client has been created, and the service name is ${serviceName}.`); + } else { + handle.retain(); + } + return handle.get(); + } + + createService(serviceType, serviceName, callback) { + let handle = this._services.get(serviceName); + if (!handle) { + handle = new RefCountingHandle(this._node.createService(serviceType, serviceName, {enableTypedArray: false}, + (request, response) => { + callback(request, response); + }), this._node.destroyService.bind(this._node)); + this._services.set(serviceName, handle); + debug(`Service has been created, and the service name is ${serviceName}.`); + } else { + handle.retain(); + } + return handle.get(); + } + + destroyPublisher(topicName) { + if (this._publishers.has(topicName)) { + let handle = this._publishers.get(topicName); + handle.release(); + this._removeInvalidHandle(this._publishers, handle, topicName); + } + } + + destroySubscription(topicName) { + SubscriptionManager.getInstance().destroySubscription(topicName, this._bridgeId); + } + + _destroySubscriptionForBridge() { + SubscriptionManager.getInstance().destroyForBridgeId(this._bridgeId); + } + + destroyClient(serviceName) { + if (this._clients.has(serviceName)) { + let handle = this._clients.get(serviceName); + handle.release(); + this._removeInvalidHandle(this._clients, handle, serviceName); + } + } + + destroyService(serviceName) { + if (this._services.has(serviceName)) { + let handle = this._services.get(serviceName); + handle.release(); + this._removeInvalidHandle(this._services, handle, serviceName); + } + } + + hasService(serviceName) { + return this._services.has(serviceName); + } + + hasSubscription(topicName) { + return SubscriptionManager.getInstance().getSubscriptionByTopicName(topicName) !== undefined; + } + + clean() { + this._cleanHandleInMap(this._publishers); + this._cleanHandleInMap(this._services); + this._cleanHandleInMap(this._clients); + this._destroySubscriptionForBridge(); + } + + _removeInvalidHandle(map, handle, name) { + if (handle.count === 0) { + map.delete(name); + } + } + + _cleanHandleInMap(map) { + map.forEach(handle => { + handle.destroy(); + }); + map.clear(); + } +} + +module.exports = ResourceProvider; diff --git a/src/lhw_gui/lib/rosauth.js b/src/lhw_gui/lib/rosauth.js new file mode 100644 index 0000000000000000000000000000000000000000..e91eb36752e3b41e0fb3de3af016ee1e623300e3 --- /dev/null +++ b/src/lhw_gui/lib/rosauth.js @@ -0,0 +1,95 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +// Same authentication logic with https://github.com/GT-RAIL/rosauth/blob/develop/src/ros_mac_authentication.cpp + +const crypto = require('crypto'); + +let secretFile = ''; + +function sha512(text) { + const hash = crypto.createHash('sha512'); + hash.update(text); + return hash.digest('hex'); +} + +function getSecret() { + const path = require('path'); + const fs = require('fs'); + const file = path.resolve(__dirname, secretFile); + // eslint-disable-next-line + const content = fs.readFileSync(file).toString(); + getSecret = function() { return content; }; + return content; +} + +function gt(l, s) { + return (l.sec == s.sec && l.nanosec > s.nanosec) || l.sec > s.sec; +} + +const NANOSEC_IN_A_SEC = 1000 * 1000 * 1000; + +function diffTime(l, s) { + let nanodiff = l.nanosec - s.nanosec; + let secdiff = l.sec - s.sec; + if (l.nanosec < s.nanosec) { + nanodiff += NANOSEC_IN_A_SEC; + secdiff += 1; + } + return secdiff + nanodiff / NANOSEC_IN_A_SEC; +} + +function getJavaScriptTime() { + const t = new Date().getTime(); + return {sec: Math.floor(t / 1000), nanosec: (t % 1000) * 1000 * 1000}; +} + +function authenticate(msg) { + if (Number.isNaN(msg.t.sec) || Number.isNaN(msg.t.nanosec) || + Number.isNaN(msg.end.sec) || Number.isNaN(msg.end.nanosec) || + msg.t.sec < 0 || msg.end.sec < 0 || + msg.t.nanosec >= NANOSEC_IN_A_SEC || msg.end.nanosec >= NANOSEC_IN_A_SEC || + msg.t.nanosec < 0 || msg.end.nanosec < 0) { + return false; + } + + // We don't get time from ROS system + // because it might not be a system-clock timestamp + const t = getJavaScriptTime(); + let diff; + if (gt(msg.t, t)) { + diff = diffTime(msg.t, t); + } else { + diff = diffTime(t, msg.t); + } + + if (diff < 5 && gt(msg.end, t)) { + const text = getSecret() + msg.client + msg.dest + msg.rand + msg.t.sec + msg.level + msg.end.sec; + const hash = sha512(text); + return msg.mac === hash; + } + + return false; +} + +function setSecretFile(file) { + secretFile = file; +} + +module.exports = { + authenticate, + setSecretFile, +}; diff --git a/src/lhw_gui/lib/subscription_manager.js b/src/lhw_gui/lib/subscription_manager.js new file mode 100644 index 0000000000000000000000000000000000000000..0743758d754757cbde0f2d7bd603650aba0d6596 --- /dev/null +++ b/src/lhw_gui/lib/subscription_manager.js @@ -0,0 +1,121 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const RefCountingHandle = require('./ref_counting_handle.js'); +const debug = require('debug')('ros2-web-bridge:SubscriptionManager'); + +class HandleWithCallbacks extends RefCountingHandle { + constructor(object, destroyHandle) { + super(object, destroyHandle); + this._callbacks = new Map(); + } + + addCallback(id, callback) { + this._callbacks.set(id, callback); + } + + removeCallback(id) { + this._callbacks.delete(id); + } + + hasCallbackForId(id) { + return this._callbacks.has(id); + } + + get callbacks() { + return Array.from(this._callbacks.values()); + } +} + +class SubscriptionManager { + constructor(node) { + this._subscripions = new Map(); + this._node = node; + } + + getSubscriptionByTopicName(topicName) { + return this._subscripions.get(topicName); + } + + createSubscription(messageType, topicName, bridgeId, callback) { + let handle = this._subscripions.get(topicName); + + if (!handle) { + let subscription = this._node.createSubscription(messageType, topicName, {enableTypedArray: false}, (message) => { + this._subscripions.get(topicName).callbacks.forEach(callback => { + callback(topicName, message); + }); + }); + handle = new HandleWithCallbacks(subscription, this._node.destroySubscription.bind(this._node)); + handle.addCallback(bridgeId, callback); + this._subscripions.set(topicName, handle); + debug(`Subscription has been created, and the topic name is ${topicName}.`); + + return handle.get(); + } + + handle.addCallback(bridgeId, callback); + handle.retain(); + return handle.get(); + } + + destroySubscription(topicName, bridgeId) { + if (this._subscripions.has(topicName)) { + let handle = this._subscripions.get(topicName); + if (handle.hasCallbackForId(bridgeId)) { + handle.removeCallback(bridgeId); + handle.release(); + if (handle.count === 0) { + this._subscripions.delete(topicName); + } + } + } + } + + destroyForBridgeId(bridgeId) { + this._subscripions.forEach(handle => { + if (handle.hasCallbackForId(bridgeId)) { + handle.removeCallback(bridgeId); + handle.release(); + this._removeInvalidHandle(); + } + }); + } + + _removeInvalidHandle() { + this._subscripions.forEach((handle, topicName, map) => { + if (handle.count === 0) { + map.delete(topicName); + } + }); + } +} + +let subscriptionManager = { + _instance: undefined, + + init(node) { + if (!this._instance) { + this._instance = new SubscriptionManager(node); + } + }, + + getInstance() { + return this._instance; + } +}; + +module.exports = subscriptionManager; diff --git a/src/lhw_vision/lhw_vision/.gitignore b/src/lhw_vision/lhw_vision/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..85a7450e5c1a7da741805de36cf73f1b4d444257 --- /dev/null +++ b/src/lhw_vision/lhw_vision/.gitignore @@ -0,0 +1,5 @@ +static/* +*.jpg +*.png +*.jpg +*.png diff --git a/src/lhw_vision/lhw_vision/debug_image_node.py b/src/lhw_vision/lhw_vision/debug_image_node.py index 63e506f7e63f08f08e1a5b72dcbb5e6f9aab99da..4bf3e811e72c4e28ee0c844c2a95f2872adcb354 100644 --- a/src/lhw_vision/lhw_vision/debug_image_node.py +++ b/src/lhw_vision/lhw_vision/debug_image_node.py @@ -3,7 +3,7 @@ """ import rclpy from rclpy.node import Node -#from sensor_msgs.msg import Image, CompressedImage +from sensor_msgs.msg import Image, CompressedImage from sensor_msgs import msg from cv_bridge import CvBridge, CvBridgeError @@ -25,6 +25,7 @@ class DebugImagePublisher(Node): def timer_callback(self): image = np.array(cv2.imread(f"{VISION_ROOT}/static/debug_image.jpg")) image_msg = self.cv_bridge.cv2_to_imgmsg(image,encoding = "rgb8") + image_msg.header.stamp = self.get_clock().now().to_msg() self.image_publisher.publish(image_msg) compressed_image_msg = self.cv_bridge.cv2_to_compressed_imgmsg(image) diff --git a/src/lhw_vision/lhw_vision/image_to_world_node.py b/src/lhw_vision/lhw_vision/image_to_world_node.py index d9920f01ee75925279ec8e4a072cc88e41d787d0..5f692fb827a90f235e3384361b8497d92f929772 100644 --- a/src/lhw_vision/lhw_vision/image_to_world_node.py +++ b/src/lhw_vision/lhw_vision/image_to_world_node.py @@ -28,23 +28,24 @@ class ImageToWorld(Node): self.log.info("Now running Image to World converter") self.K = K self.timestamps = {} + def image_callback(self,image_msg: Image) -> None: timestamp = image_msg.header.stamp image = self.bridge.imgmsg_to_cv2(image_msg,desired_encoding='rgb8') - save_image(image, VISION_ROOT+'/pepper_im.jpg') + save_image(image, VISION_ROOT+'/static/pepper_im.jpg') if timestamp not in self.timestamps: self.timestamps[timestamp] = image else: self.convert_to_3d(self.timestamps.pop(timestamp),image) - def tracker_callback(self,entities_msg: Entities) -> Node: + def tracker_callback(self, entities_msg: Entities) -> Node: timestamp = entities_msg.time if timestamp not in self.timestamps: self.timestamps[timestamp] = entities_msg else: self.convert_to_3d(entities_msg,self.timestamps.pop(timestamp)) - def convert_to_3d(self,entities_msg: Entities, image: np.ndarray) -> Entities: + def convert_to_3d(self, entities_msg: Entities, image: np.ndarray) -> Entities: """ Given tracked entities and an Image, returns the 3d position of the entities Args: entities_msg: The tracked targets @@ -56,9 +57,10 @@ class ImageToWorld(Node): for entity in entities_msg.entities: coords = depth_to_xyz(entity.bbox,depth,self.K) entity.xyz = coords - self.log.info("le coords fejs" + str(coords)) + #self.log.info("le coords fejs" + str(coords)) entity.sources.append(entities_msg.source) entity.sources_ids.append(entity.id) + self.log.info(f"Sent {len(entities_msg.entities)} targets") entities_msg.source = 4 self.tracked_3d_targets.publish(entities_msg) diff --git a/src/lhw_vision/lhw_vision/pepper_im.jpg b/src/lhw_vision/lhw_vision/pepper_im.jpg deleted file mode 100644 index 92bc68606902024d811b04d75453aca067c50f56..0000000000000000000000000000000000000000 Binary files a/src/lhw_vision/lhw_vision/pepper_im.jpg and /dev/null differ diff --git a/src/lhw_vision/lhw_vision/static/debug_image.jpg b/src/lhw_vision/lhw_vision/static/debug_image.jpg deleted file mode 100644 index ed110db39b57689de6fad910b84d69219e0bb904..0000000000000000000000000000000000000000 Binary files a/src/lhw_vision/lhw_vision/static/debug_image.jpg and /dev/null differ diff --git a/src/lhw_vision/lhw_vision/tracker/kalman.py b/src/lhw_vision/lhw_vision/tracker/kalman.py index 19d74b0d259536d0e7f038680621852a4da10625..3051ce92e20d3a3199dd7330edafa5ca33c0f51a 100644 --- a/src/lhw_vision/lhw_vision/tracker/kalman.py +++ b/src/lhw_vision/lhw_vision/tracker/kalman.py @@ -16,7 +16,8 @@ from typing import Tuple import numpy as np class Kalman: - def __init__(self, + def __init__(self, + log, var_P0: float = 10., var_R: float = 2., var_Q: float = 2.): @@ -27,6 +28,7 @@ class Kalman: var_Q: The process noise covariance. Basically a pretty bad way to model changes in how the target moves over time. """ super().__init__() + self.log = log self.tracks = {} self.Q = np.zeros((8,8)) self.Q[4:,4:] = np.eye(4) @@ -76,8 +78,8 @@ class Kalman: Args: targets: All current targets. """ - for cls,vals in targets.items(): - for v in vals: + for cls, vals in targets.items(): + for v in vals.values(): ident,y,missing = v['tracked_id'],v['bbox'],v['missing'] if not missing: self.measurement_update(ident,y) diff --git a/src/lhw_vision/lhw_vision/tracker/match.py b/src/lhw_vision/lhw_vision/tracker/match.py deleted file mode 100644 index e2e8adfe45e3d3d56d2ef440a0a05a0a403e2273..0000000000000000000000000000000000000000 --- a/src/lhw_vision/lhw_vision/tracker/match.py +++ /dev/null @@ -1,28 +0,0 @@ -import numpy as np -from .utils import IoU - -def match(detections,targets): - """ - Detections - - """ - matches = {} - for cls,vals in detections.items(): - if cls not in targets: - continue - tars = targets[cls] - M = np.zeros((len(vals),len(tars))) - for v in range(len(vals)): - for t in range(len(tars)): - M[v,t] = IoU(vals[v]["bbox"],tars[t]["bbox"]) - M_best = (M==M.max(axis=0)) & ((M==M.max(axis=1))) & (M>0) - matches = np.argwhere(M_best) - for d,t in matches: - detections[cls][d]['target'] = t - return detections - - -if __name__ == "__main__": - detections = {"one":[{"bbox":[10,20,30,40]},{"bbox":[10,30,30,50]},{"bbox":[20,20,30,40]},{"bbox":[15,20,25,40]}]} - targets = {"one":[{"bbox":[10,20,30,40]},{"bbox":[10,30,30,50]},{"bbox":[20,20,30,40]},{"bbox":[15,20,25,40]}]} - print(match(detections,targets)) \ No newline at end of file diff --git a/src/lhw_vision/lhw_vision/tracker/reid.py b/src/lhw_vision/lhw_vision/tracker/reid.py index 732f2a5fa5247d881286fc753fb74a3b65a4583e..ccb95ea537af209a6f8cce3c7d8a8e8dfcca44c8 100644 --- a/src/lhw_vision/lhw_vision/tracker/reid.py +++ b/src/lhw_vision/lhw_vision/tracker/reid.py @@ -19,6 +19,7 @@ class ReID: """ Class for re-identifying people who have not been seen for a while """ def __init__(self, + log, memory_size=5000, embedding_dim=512, knn=6, @@ -31,6 +32,7 @@ class ReID: t (float): threshold for re-identification """ super().__init__() + self.log = log self.D = np.zeros((memory_size,memory_size)) self.id_map = {} self.reverse_map = np.zeros(memory_size,dtype=np.long) @@ -80,8 +82,13 @@ class ReID: def __call__(self,detections,image): if 'person' not in detections: return detections + if len(self.assigned_ids) == 0: + return detections people = detections['person'] for person in people: + if 'target_tracked_id' in person: + self.log.info('Already found target') + continue #embed the person z = self.embed(person['bbox'],image) all_people = self.people[self.assigned_ids] @@ -89,19 +96,28 @@ class ReID: d = np.sqrt(((z[None,:]-all_people)**2).sum(axis=1)) N = set(np.argsort(d)[:self.k]) R = set() - # compute the reciprocal R and R* - # (see https://openaccess.thecvf.com/content_cvpr_2017/papers/Zhong_Re-Ranking_Person_Re-Identification_CVPR_2017_paper.pdf) for details) - for q in N: - q_d = self.D[self.assigned_ids[q],self.assigned_ids] - q_inds = np.argsort(q_d)[:self.k] - if d[q] <= q_d[q_inds[-1]] or len(q_inds)<self.k: - R.add(q) - R.union(set(q_inds[:self.k//2])) - if not R: - continue - possible_ids = self.reverse_map[[self.assigned_ids[i] for i in R]] + if False: # Note from Johan: This works for ReID type matching, but works poorly in video. + # compute the reciprocal R and R* + # (see https://openaccess.thecvf.com/content_cvpr_2017/papers/Zhong_Re-Ranking_Person_Re-Identification_CVPR_2017_paper.pdf) for details) + for q in N: + q_d = self.D[self.assigned_ids[q],self.assigned_ids] + q_inds = np.argsort(q_d)[:self.k] + if d[q] <= q_d[q_inds[-1]] or len(q_inds)<self.k: + R.add(q) + R.union(set(q_inds[:self.k//2])) + if not R: + continue + possible_ids = self.reverse_map[[self.assigned_ids[i] for i in R]] + else: + self.log.info(f"{d=}") + #self.log.info(f"{self.D[self.assigned_ids][:,self.assigned_ids]=}") + if d.min() > 0.5*self.D[self.assigned_ids][:,self.assigned_ids].mean(): + self.log.info(f"{d.min()=}") + continue + possible_ids = self.reverse_map[[self.assigned_ids[i] for i in N]] + (person_ids, counts) = np.unique(possible_ids, return_counts=True) if np.max(counts) > len(possible_ids)*self.t: best_match = person_ids[np.argmax(counts)] - person['target'] = best_match + person['target_tracked_id'] = best_match return detections \ No newline at end of file diff --git a/src/lhw_vision/lhw_vision/tracker/tracker.py b/src/lhw_vision/lhw_vision/tracker/tracker.py index fad111b79e77f33177f2d1f7645f4788f8e41882..a079b80eff6a12db4b35b5b2dfd8432ee5d3aacd 100644 --- a/src/lhw_vision/lhw_vision/tracker/tracker.py +++ b/src/lhw_vision/lhw_vision/tracker/tracker.py @@ -4,7 +4,11 @@ import numpy as np from .reid import ReID from .kalman import Kalman -from .match import match +from .utils import IoU, not_at_border, big_enough + +from lhw_vision import VISION_ROOT +from lhw_vision.utils import save_image +import cv2 class VisionTracker: def __init__(self, @@ -27,14 +31,15 @@ class VisionTracker: var_P0: Initial covariance for Kalman filter """ super().__init__() - self.reid = ReID(memory_size=memory_size, + self.reid = ReID(log, + memory_size=memory_size, embedding_dim=embedding_dim, knn=knn, t=t) self.current_id = 0 self.targets = {} - self.kalman = Kalman( - var_P0=var_P0) + self.kalman = Kalman(log, + var_P0=var_P0) self.max_missing = max_missing self.log = log @@ -46,16 +51,47 @@ class VisionTracker: Returns: The modified detection with the tracked_id attached. """ - target = {'tracked_id':self.current_id,**detection} + tracked_id = self.current_id + target = {'tracked_id':tracked_id,'missing':0,**detection} cls = detection['type'] if cls in self.targets: - self.targets[cls].append(target) + self.targets[cls][tracked_id] = target else: - self.targets[cls] = [target] - target = self.targets[cls][-1] - self.current_id += 1 + self.targets[cls] = {tracked_id:target} + target = self.targets[cls][tracked_id] + self.current_id = self.current_id + 1 return target + def match(self, detections: dict, targets: dict) -> dict: + """ + Matches detections to targets. + + Args: + detections: The detected entities + targets: The previously tracked targets + Returns: + The detections, with all matches augmented with corresponding target + + """ + matches = {} + for cls,dets in detections.items(): + assert len(dets)> 0, f"{dets=}" + if cls in targets: + if len(targets[cls]) == 0: + continue + else: + continue + tars = list(targets[cls].values()) + M = np.zeros((len(dets),len(tars))) + for v in range(len(dets)): + for t in range(len(tars)): + M[v,t] = IoU(dets[v]["bbox"],tars[t]["bbox"]) + M_best = (M==M.max(axis=0)) & ((M==M.max(axis=1))) & (M>0) + matches = np.argwhere(M_best) + for d,t in matches: + detections[cls][d]['target_tracked_id'] = tars[t]['tracked_id'] + return detections + def propagate(self, detections: dict, image: np.ndarray) -> None: """ Propagates all detection to previous targets, or if no target is found, creates new targets. If the detection is a person we additionally add the to our memory to remember them for later. @@ -66,33 +102,47 @@ class VisionTracker: """ for cls, cls_detections in detections.items(): for detection in cls_detections: - if 'target' in detection: - target = self.targets[cls][detection['target']] + if 'target_tracked_id' in detection: + target = self.targets[cls][detection['target_tracked_id']] detection['tracked_id'] = target['tracked_id'] - target = detection #TODO: might want to do this fusion more smoothly + target = {'missing':0,**detection} #TODO: might want to do this fusion more smoothly + self.targets[cls][target['tracked_id']] = target else: target = self.init_id(detection) - target['missing'] = 0 if cls == 'person': self.reid.add(target,image) def tic(self): """Checks for old items and removes them """ - targets = {cls:[] for cls in self.targets.keys()} + targets = {cls:{} for cls in self.targets.keys()} for cls, tars in self.targets.items(): - for tar in tars: + for tar in tars.values(): if tar['missing'] <= self.max_missing: - targets[cls].append(tar) + targets[cls][tar['tracked_id']] = tar self.targets = targets def toc(self): """ Increments missing time for all targets """ - for cls, tars in self.targets.items(): - for tar in tars: + for _, tars in self.targets.items(): + for tar in tars.values(): tar['missing'] += 1 + def prune_detections(self, detections, image): + pruned_detections = {cls:[] for cls in detections.keys()} + h,w,rgb = image.shape + for cls, cls_detections in detections.items(): + for detection in cls_detections: + a = detection['confidence']>0.2 + b = not_at_border(detection['bbox'],h,w,40) + c = big_enough(detection['bbox']) + if a and b and c: + pruned_detections[cls].append(detection) + if len(pruned_detections[cls]) == 0: + pruned_detections.pop(cls) + return pruned_detections + def track(self, detections: dict, image: np.ndarray) -> dict: """ Main function of the tracker. Matches detections with tracked targets and may additionally remember previously seen people. Returns all tracked targets. @@ -104,14 +154,26 @@ class VisionTracker: All tracked targets. (Note: This can contain missing targets if self.max_missing > 0, in that case the kalman predictions will be returned.) """ self.toc() - detections = match(detections,self.targets) + detections = self.prune_detections(detections,image) + detections = self.match(detections,self.targets) detections = self.reid(detections,image) - self.log.debug(f"{detections=}") self.propagate(detections,image) - self.log.debug(f"{self.targets=}") self.kalman.update(self.targets) self.tic() + if True: + self.visualize(image) return self.targets + def visualize(self,image): + from PIL import Image, ImageDraw + pil_im = Image.fromarray(image) + draw = ImageDraw.Draw(pil_im) + + for cls,tars in self.targets.items(): + for tracked_id,tar in tars.items(): + bb = list(tar['bbox']) + draw.rectangle(bb) + draw.text(bb[:2], f"{cls} {tar['tracked_id']}") + pil_im.save(f"{VISION_ROOT}/static/bb_im.jpg") def get_targets(self): return self.targets diff --git a/src/lhw_vision/lhw_vision/tracker/utils.py b/src/lhw_vision/lhw_vision/tracker/utils.py index dcdce0027adcf406e9d89e41f733aad2da34d36c..5a4297e36c67c1e65a34b56c58a1b46813195275 100644 --- a/src/lhw_vision/lhw_vision/tracker/utils.py +++ b/src/lhw_vision/lhw_vision/tracker/utils.py @@ -17,15 +17,21 @@ def IoU(boxA, boxB): # compute the intersection over union by taking the intersection # area and dividing it by the sum of prediction + ground-truth # areas - the interesection area - assert boxAArea > 0,'boxAArea = 0' - assert boxBArea > 0,'boxBArea = 0' + assert boxAArea > 0,f'{boxAArea =}, {boxA=}' + assert boxBArea > 0,f'{boxBArea =}, {boxB=}' iou = interArea / float(boxAArea + boxBArea - interArea) # return the intersection over union value return iou -#def IoU(boxesA,boxesB): -# _,B = boxesA.shape -# boxesA = boxesA[4,None,B] -# boxesB = boxesB[4,B,None] +def not_at_border(bbox,h,w,border_width=100): + center = ((bbox[2]+bbox[0])/2,(bbox[3]-bbox[1])/2) + a=border_width<center[0] + b=center[0] < w - border_width + c=border_width<center[1] + d=center[1] < h - border_width + return a and b and c and d + +def big_enough(bbox, enough=1000): + return ((bbox[2]-bbox[0])*(bbox[3]-bbox[1])) > enough \ No newline at end of file diff --git a/src/lhw_vision/lhw_vision/tracker_node.py b/src/lhw_vision/lhw_vision/tracker_node.py index c4979feae41fd2d4b53ee447e39695fead309814..4f0de398112d3bf0b2d0ac609a7e64955108238e 100644 --- a/src/lhw_vision/lhw_vision/tracker_node.py +++ b/src/lhw_vision/lhw_vision/tracker_node.py @@ -20,7 +20,8 @@ class Tracker(Node): def __init__(self): super().__init__("tracker") self.log = self.get_logger() - self.timestamps = {} + self.images = {} + self.entities = {} self.tracker = VisionTracker(self.log) self.image_sub = self.create_subscription(Image, 'image', self.image_callback, 10) @@ -30,29 +31,33 @@ class Tracker(Node): self.bridge = CvBridge() self.log.info("Now running Tracker") - def image_callback(self,image_msg): + def image_callback(self,image_msg: Image): + """ Callback upon pepper image being updated. If entities with a corresponding timestamp has been received, self.track is called. + If not, then add to self.images + Args: + image_msg: Image from Pepper + """ timestamp = image_msg.header.stamp image = self.bridge.imgmsg_to_cv2(image_msg,desired_encoding='rgb8') - #save_image(image, VISION_ROOT+'/pepper_im.jpg') - if timestamp not in self.timestamps: - self.timestamps[timestamp] = image + self.log.info(str(timestamp)) + if timestamp not in self.entities: + self.images[timestamp] = image else: - entities = self.timestamps.pop(timestamp) + entities = self.entities.pop(timestamp) self.track(entities, image, timestamp) def yolo_callback(self,entities_msg): + self.log.info("In yolo callback") #FIXME: the timestamps dict may grow indefinitely if not all images are turned into yolo entities, should have some way of dealing with it timestamp = entities_msg.time entities = {ent.type:[] for ent in entities_msg.entities} + self.log.info(f"Yolo detected {len(entities_msg.entities)} targets") for ent in entities_msg.entities: - #self.log.info(str(ent)) - #TODO: [{field: getattr(ent, field) for field in ent.__slots__}] is weird and returns a _ before the names for some reason?? ros2 source code is impossible to understand https://github.com/ros2/rosidl_python/blob/0f5c8f360be92566ad86f4b29f3db1febfca2242/rosidl_generator_py/resource/_msg.py.em#L113 entities[ent.type] += [dict(bbox=ent.bbox,id=ent.id,type=ent.type,confidence=ent.confidence,sources=list(ent.sources)+[entities_msg.source],sources_ids=ent.sources_ids)] - self.log.info(str(entities)) - if timestamp not in self.timestamps: - self.timestamps[timestamp] = entities + if timestamp not in self.images: + self.entities[timestamp] = entities else: - image = self.timestamps.pop(timestamp) + image = self.images.pop(timestamp) self.track(entities, image, timestamp) def track(self, entities: Entities, image: np.ndarray, timestamp: Time) -> Entities: @@ -66,14 +71,13 @@ class Tracker(Node): """ height, width = image.shape[:2] targets = self.tracker.track(entities,image) - self.log.info(str(targets)) msg = Entities() msg.entities = [Entity(bbox=tar['bbox'],type=tar['type'],id=tar['tracked_id'], sources=tar['sources'],sources_ids=[tar['id']],confidence=tar['confidence']) - for tars in targets.values() for tar in tars if tar['missing']==0] + for tars in targets.values() for tar in tars.values() if tar['missing']==0] msg.source = msg.sources_types.TRACKER - msg.source_height = int(height)#?? - msg.source_width = int(width)#?? + msg.source_height = int(height) + msg.source_width = int(width) msg.time = timestamp self.tracked_targets.publish(msg) diff --git a/src/lhw_vision/test/test_vision.py b/src/lhw_vision/test/test_vision.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391