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