don't import for bounded array (#210)

don't import for bounded array (#210)

* don't import for bounded array

* splut looong if condition on 2 lines

# generated from rosidl_generator_py/resource/_msg.py.em
# generated code does not contain a copyright notice

import logging
import traceback

class Metaclass(type):
    """Metaclass of message '@(spec.base_type.type)'."""

    _CONVERT_FROM_PY = None
    _CONVERT_TO_PY = None
    _TYPE_SUPPORT = None

    __constants = {
@[for constant in spec.constants]@
        '@(constant.name)': @constant_value_to_py(constant.type, constant.value),
@[end for]@
    }

    @@classmethod
    def __import_type_support__(cls):
        try:
            from rosidl_generator_py import import_type_support
            module = import_type_support('@(package_name)')
        except ImportError:
            logger = logging.getLogger('rosidl_generator_py.@(spec.base_type.type)')
            logger.debug(
                'Failed to import needed modules for type support:\n' + traceback.format_exc())
        else:
            cls._CONVERT_FROM_PY = module.convert_from_py_msg_@(module_name)
            cls._CONVERT_TO_PY = module.convert_to_py_msg_@(module_name)
            cls._TYPE_SUPPORT = module.type_support_msg_@(module_name)
@{
importable_typesupports = {}
for field in spec.fields:
    if not field.type.is_primitive_type():
        key = '%s.msg.%s' % (field.type.pkg_name, field.type.type)
        if key not in importable_typesupports:
            importable_typesupports[key] = [field.type.pkg_name, field.type.type]
for key in sorted(importable_typesupports.keys()):
    (pkg_name, field_name) = importable_typesupports[key]
    print('%sfrom %s.msg import %s' % (' ' * 4 * 3, pkg_name, field_name))
    print('%sif %s.__class__._TYPE_SUPPORT is None:' % (' ' * 4 * 3, field_name))
    print('%s%s.__class__.__import_type_support__()' % (' ' * 4 * 4, field_name))
}@

    @@classmethod
    def __prepare__(cls, name, bases, **kwargs):
        # list constant names here so that they appear in the help text of
        # the message class under "Data and other attributes defined here:"
        # as well as populate each message instance
        return {
@[for constant in spec.constants]@
            '@(constant.name)': cls.__constants['@(constant.name)'],
@[end for]@
@[for field in spec.fields]@
@[  if field.default_value]@
            '@(field.name.upper())__DEFAULT': @value_to_py(field.type, field.default_value),
@[  end if]@
@[end for]@
        }
@[for constant in spec.constants]@

    @@property
    def @(constant.name)(self):
        """Message constant '@(constant.name)'."""
        return Metaclass.__constants['@(constant.name)']
@[end for]@
@[for field in spec.fields]@
@[  if field.default_value]@

    @@property
    def @(field.name.upper())__DEFAULT(cls):
        """Default value for message field '@(field.name)'."""
        return @value_to_py(field.type, field.default_value)
@[  end if]@
@[end for]@

class @(spec.base_type.type)(metaclass=Metaclass):
@[if not spec.constants]@
    """Message class '@(spec.base_type.type)'."""
@[else]@
    """
    Message class '@(spec.base_type.type)'.

    Constants:
@[  for constant in spec.constants]@
      @(constant.name)
@[  end for]@
    """
@[end if]@

    __slots__ = [
@[for field in spec.fields]@
        '_@(field.name)',
@[end for]@
    ]
@
@[if len(spec.fields) > 0]@

    def __init__(self, **kwargs):
        assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
            "Invalid arguments passed to constructor: %r" % kwargs.keys()
@[  for field in spec.fields]@
@[    if field.default_value]@
        self.@(field.name) = kwargs.get(
            '@(field.name)', @(spec.base_type.type).@(field.name.upper())__DEFAULT)
@[    else]@
@[      if not field.type.is_primitive_type() and (not field.type.is_array or
          (field.type.array_size and not field.type.is_upper_bound))]@
        from @(field.type.pkg_name).msg import @(field.type.type)
@[      end if]@
@[      if field.type.array_size and not field.type.is_upper_bound]@
@[        if field.type.type == 'byte']@
        self.@(field.name) = kwargs.get(
            '@(field.name)',
            list([bytes([0]) for x in range(@(field.type.array_size))])
        )
@[        elif field.type.type == 'char']@
        self.@(field.name) = kwargs.get(
            '@(field.name)',
            list([chr(0) for x in range(@(field.type.array_size))])
        )
@[        else]@
        self.@(field.name) = kwargs.get(
            '@(field.name)',
            list([@(get_python_type(field.type))() for x in range(@(field.type.array_size))])
        )
@[        end if]@
@[      elif field.type.is_array]@
        self.@(field.name) = kwargs.get('@(field.name)', list())
@[      elif field.type.type == 'byte']@
        self.@(field.name) = kwargs.get('@(field.name)', bytes([0]))
@[      elif field.type.type == 'char']@
        self.@(field.name) = kwargs.get('@(field.name)', chr(0))
@[      else]@
        self.@(field.name) = kwargs.get('@(field.name)', @(get_python_type(field.type))())
@[      end if]@
@[    end if]@
@[  end for]@
@[end if]@

    def __repr__(self):
        typename = self.__class__.__module__.split('.')
        typename.pop()
        typename.append(self.__class__.__name__)
        args = [s[1:] + '=' + repr(getattr(self, s, None)) for s in self.__slots__]
        return '%s(%s)' % ('.'.join(typename), ', '.join(args))
@[for field in spec.fields]@

    @@property
    def @(field.name)(self):
        """Message field '@(field.name)'."""
        return self._@(field.name)

    @@@(field.name).setter
    def @(field.name)(self, value):
@[  if not field.type.is_primitive_type()]@
        from @(field.type.pkg_name).msg import @(field.type.type)
@[  end if]@
@[  if field.type.is_array]@
        from collections import Sequence
        from collections import Set
        from collections import UserList
        from collections import UserString
@[  elif field.type.string_upper_bound]@
        from collections import UserString
@[  elif field.type.type == 'byte']@
        from collections import ByteString
@[  elif field.type.type in ['char']]@
        from collections import UserString
@[  end if]@
        assert \
@[  if field.type.is_array]@
            ((isinstance(value, Sequence) or
              isinstance(value, Set) or
              isinstance(value, UserList)) and
             not isinstance(value, str) and
             not isinstance(value, UserString) and
@[    if field.type.type == 'string' and field.type.string_upper_bound]@
             all([len(val) <= @field.type.string_upper_bound for val in value]) and
@[    end if]@
@[    if field.type.array_size]@
@[      if field.type.is_upper_bound]@
             len(value) <= @(field.type.array_size) and
@[      else]@
             len(value) == @(field.type.array_size) and
@[      end if]@
@[    end if]@
             all([isinstance(v, @(get_python_type(field.type))) for v in value]) and
@[      if field.type.type.startswith('int')]@
@{
nbits = int(field.type.type[3:])
bound = 2**(nbits - 1)
}@
             all([val >= -@(bound) and val < @(bound) for val in value]))
@[      elif field.type.type.startswith('uint')]@
@{
nbits = int(field.type.type[4:])
bound = 2**nbits
}@
             all([val >= 0 and val < @(bound) for val in value]))
@[      elif field.type.type == 'char']@
             all([ord(val) >= -128 and ord(val) < 128 for val in value]))
@[      else]@
             True)
@[      end if]@
@[  elif field.type.string_upper_bound]@
            ((isinstance(value, str) or isinstance(value, UserString)) and
             len(value) <= @(field.type.string_upper_bound))
@[  elif not field.type.is_primitive_type()]@
            isinstance(value, @(field.type.type))
@[  elif field.type.type == 'byte']@
            ((isinstance(value, bytes) or isinstance(value, ByteString)) and
             len(value) == 1)
@[  elif field.type.type == 'char']@
            ((isinstance(value, str) or isinstance(value, UserString)) and
             len(value) == 1 and ord(value) >= -128 and ord(value) < 128)
@[  elif field.type.type in [
        'bool',
        'float32', 'float64',
        'int8', 'uint8',
        'int16', 'uint16',
        'int32', 'uint32',
        'int64', 'uint64',
        'string',
    ]]@
            isinstance(value, @(get_python_type(field.type)))
@[    if field.type.type.startswith('int')]@
@{
nbits = int(field.type.type[3:])
bound = 2**(nbits - 1)
}@
        assert value >= -@(bound) and value < @(bound)
@[    elif field.type.type.startswith('uint')]@
@{
nbits = int(field.type.type[4:])
bound = 2**nbits
}@
        assert value >= 0 and value < @(bound)
@[    end if]@
@[  else]@
            False
@[  end if]@
        self._@(field.name) = value
@[end for]@

# generated from rosidl_generator_py/resource/_msg.py.em
# generated code does not contain a copyright notice

import logging
import traceback

class Metaclass(type):
    """Metaclass of message '@(spec.base_type.type)'."""

    _CONVERT_FROM_PY = None
    _CONVERT_TO_PY = None
    _TYPE_SUPPORT = None

    __constants = {
@[for constant in spec.constants]@
        '@(constant.name)': @constant_value_to_py(constant.type, constant.value),
@[end for]@
    }

    @@classmethod
    def __import_type_support__(cls):
        try:
            from rosidl_generator_py import import_type_support
            module = import_type_support('@(package_name)')
        except ImportError:
            logger = logging.getLogger('rosidl_generator_py.@(spec.base_type.type)')
            logger.debug(
                'Failed to import needed modules for type support:\n' + traceback.format_exc())
        else:
            cls._CONVERT_FROM_PY = module.convert_from_py_msg_@(module_name)
            cls._CONVERT_TO_PY = module.convert_to_py_msg_@(module_name)
            cls._TYPE_SUPPORT = module.type_support_msg_@(module_name)
@{
importable_typesupports = {}
for field in spec.fields:
    if not field.type.is_primitive_type():
        key = '%s.msg.%s' % (field.type.pkg_name, field.type.type)
        if key not in importable_typesupports:
            importable_typesupports[key] = [field.type.pkg_name, field.type.type]
for key in sorted(importable_typesupports.keys()):
    (pkg_name, field_name) = importable_typesupports[key]
    print('%sfrom %s.msg import %s' % (' ' * 4 * 3, pkg_name, field_name))
    print('%sif %s.__class__._TYPE_SUPPORT is None:' % (' ' * 4 * 3, field_name))
    print('%s%s.__class__.__import_type_support__()' % (' ' * 4 * 4, field_name))
}@

    @@classmethod
    def __prepare__(cls, name, bases, **kwargs):
        # list constant names here so that they appear in the help text of
        # the message class under "Data and other attributes defined here:"
        # as well as populate each message instance
        return {
@[for constant in spec.constants]@
            '@(constant.name)': cls.__constants['@(constant.name)'],
@[end for]@
@[for field in spec.fields]@
@[  if field.default_value]@
            '@(field.name.upper())__DEFAULT': @value_to_py(field.type, field.default_value),
@[  end if]@
@[end for]@
        }
@[for constant in spec.constants]@

    @@property
    def @(constant.name)(self):
        """Message constant '@(constant.name)'."""
        return Metaclass.__constants['@(constant.name)']
@[end for]@
@[for field in spec.fields]@
@[  if field.default_value]@

    @@property
    def @(field.name.upper())__DEFAULT(cls):
        """Default value for message field '@(field.name)'."""
        return @value_to_py(field.type, field.default_value)
@[  end if]@
@[end for]@

class @(spec.base_type.type)(metaclass=Metaclass):
@[if not spec.constants]@
    """Message class '@(spec.base_type.type)'."""
@[else]@
    """
    Message class '@(spec.base_type.type)'.

    Constants:
@[  for constant in spec.constants]@
      @(constant.name)
@[  end for]@
    """
@[end if]@

    __slots__ = [
@[for field in spec.fields]@
        '_@(field.name)',
@[end for]@
    ]
@
@[if len(spec.fields) > 0]@

    def __init__(self, **kwargs):
        assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
            "Invalid arguments passed to constructor: %r" % kwargs.keys()
@[  for field in spec.fields]@
@[    if field.default_value]@
        self.@(field.name) = kwargs.get(
            '@(field.name)', @(spec.base_type.type).@(field.name.upper())__DEFAULT)
@[    else]@
@[      if not field.type.is_primitive_type() and (not field.type.is_array or field.type.array_size)]@
        from @(field.type.pkg_name).msg import @(field.type.type)
@[      end if]@
@[      if field.type.array_size and not field.type.is_upper_bound]@
@[        if field.type.type == 'byte']@
        self.@(field.name) = kwargs.get(
            '@(field.name)',
            list([bytes([0]) for x in range(@(field.type.array_size))])
        )
@[        elif field.type.type == 'char']@
        self.@(field.name) = kwargs.get(
            '@(field.name)',
            list([chr(0) for x in range(@(field.type.array_size))])
        )
@[        else]@
        self.@(field.name) = kwargs.get(
            '@(field.name)',
            list([@(get_python_type(field.type))() for x in range(@(field.type.array_size))])
        )
@[        end if]@
@[      elif field.type.is_array]@
        self.@(field.name) = kwargs.get('@(field.name)', list())
@[      elif field.type.type == 'byte']@
        self.@(field.name) = kwargs.get('@(field.name)', bytes([0]))
@[      elif field.type.type == 'char']@
        self.@(field.name) = kwargs.get('@(field.name)', chr(0))
@[      else]@
        self.@(field.name) = kwargs.get('@(field.name)', @(get_python_type(field.type))())
@[      end if]@
@[    end if]@
@[  end for]@
@[end if]@

    def __repr__(self):
        typename = self.__class__.__module__.split('.')
        typename.pop()
        typename.append(self.__class__.__name__)
        args = [s[1:] + '=' + repr(getattr(self, s, None)) for s in self.__slots__]
        return '%s(%s)' % ('.'.join(typename), ', '.join(args))
@[for field in spec.fields]@

    @@property
    def @(field.name)(self):
        """Message field '@(field.name)'."""
        return self._@(field.name)

    @@@(field.name).setter
    def @(field.name)(self, value):
@[  if not field.type.is_primitive_type()]@
        from @(field.type.pkg_name).msg import @(field.type.type)
@[  end if]@
@[  if field.type.is_array]@
        from collections import Sequence
        from collections import Set
        from collections import UserList
        from collections import UserString
@[  elif field.type.string_upper_bound]@
        from collections import UserString
@[  elif field.type.type == 'byte']@
        from collections import ByteString
@[  elif field.type.type in ['char']]@
        from collections import UserString
@[  end if]@
        assert \
@[  if field.type.is_array]@
            ((isinstance(value, Sequence) or
              isinstance(value, Set) or
              isinstance(value, UserList)) and
             not isinstance(value, str) and
             not isinstance(value, UserString) and
@[    if field.type.type == 'string' and field.type.string_upper_bound]@
             all([len(val) <= @field.type.string_upper_bound for val in value]) and
@[    end if]@
@[    if field.type.array_size]@
@[      if field.type.is_upper_bound]@
             len(value) <= @(field.type.array_size) and
@[      else]@
             len(value) == @(field.type.array_size) and
@[      end if]@
@[    end if]@
             all([isinstance(v, @(get_python_type(field.type))) for v in value]) and
@[      if field.type.type.startswith('int')]@
@{
nbits = int(field.type.type[3:])
bound = 2**(nbits - 1)
}@
             all([val >= -@(bound) and val < @(bound) for val in value]))
@[      elif field.type.type.startswith('uint')]@
@{
nbits = int(field.type.type[4:])
bound = 2**nbits
}@
             all([val >= 0 and val < @(bound) for val in value]))
@[      elif field.type.type == 'char']@
             all([ord(val) >= -128 and ord(val) < 128 for val in value]))
@[      else]@
             True)
@[      end if]@
@[  elif field.type.string_upper_bound]@
            ((isinstance(value, str) or isinstance(value, UserString)) and
             len(value) <= @(field.type.string_upper_bound))
@[  elif not field.type.is_primitive_type()]@
            isinstance(value, @(field.type.type))
@[  elif field.type.type == 'byte']@
            ((isinstance(value, bytes) or isinstance(value, ByteString)) and
             len(value) == 1)
@[  elif field.type.type == 'char']@
            ((isinstance(value, str) or isinstance(value, UserString)) and
             len(value) == 1 and ord(value) >= -128 and ord(value) < 128)
@[  elif field.type.type in [
        'bool',
        'float32', 'float64',
        'int8', 'uint8',
        'int16', 'uint16',
        'int32', 'uint32',
        'int64', 'uint64',
        'string',
    ]]@
            isinstance(value, @(get_python_type(field.type)))
@[    if field.type.type.startswith('int')]@
@{
nbits = int(field.type.type[3:])
bound = 2**(nbits - 1)
}@
        assert value >= -@(bound) and value < @(bound)
@[    elif field.type.type.startswith('uint')]@
@{
nbits = int(field.type.type[4:])
bound = 2**nbits
}@
        assert value >= 0 and value < @(bound)
@[    end if]@
@[  else]@
            False
@[  end if]@
        self._@(field.name) = value
@[end for]@


Adding small hack to eol template because of lucid https://github.com/moby/moby/blob/e2e5d4bc9da5ba17bb2822909611f8300fdd80f0/contrib/mkimage/debootstrap#L250

@(TEMPLATE(
    'snippet/add_generated_comment.Dockerfile.em',
    user_name=user_name,
    tag_name=tag_name,
    source_template_name=template_name,
))@
@(TEMPLATE(
    'snippet/from_base_image.Dockerfile.em',
    template_packages=template_packages,
    os_name=os_name,
    os_code_name=os_code_name,
    arch=arch,
    base_image=base_image,
    maintainer_name=maintainer_name,
))@

# setup source.list to eol
RUN sed -i -e 's/archive/old-releases/g' /etc/apt/sources.list

@[if 'packages' in locals()]@
@[if packages]@

# install packages
RUN apt-get update && apt-get install -y --no-install-recommends \
    @(' \\\n    '.join(packages))@  \
    && rm -rf /var/lib/apt/lists/* \
    && mkdir /var/lib/apt/lists/partial

@[end if]@
@[end if]@
# setup keys
RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 421C365BD9FF1F717815A3895523BAEEB01FA116

# setup sources.list
RUN echo "deb http://packages.ros.org/ros/ubuntu @os_code_name main" > /etc/apt/sources.list.d/ros-latest.list

# setup environment
ENV LANG C.UTF-8
ENV LC_ALL C.UTF-8

# install ros packages
ENV ROS_DISTRO @rosdistro_name
RUN apt-get update && apt-get install -y \
    @(' \\\n    '.join(ros_packages))@  \
    && rm -rf /var/lib/apt/lists/* \
    && mkdir /var/lib/apt/lists/partial

@[if 'entrypoint_name' in locals()]@
@[if entrypoint_name]@
@{
entrypoint_file = entrypoint_name.split('/')[-1]
}@
# setup entrypoint
COPY ./@entrypoint_file /ros_entrypoint.sh

ENTRYPOINT ["/ros_entrypoint.sh"]
@[end if]@
@[end if]@
@{
cmds = [
'bash',
]
}@
CMD ["@(' && '.join(cmds))"]

@(TEMPLATE(
    'snippet/add_generated_comment.Dockerfile.em',
    user_name=user_name,
    tag_name=tag_name,
    source_template_name=template_name,
))@
@(TEMPLATE(
    'snippet/from_base_image.Dockerfile.em',
    template_packages=template_packages,
    os_name=os_name,
    os_code_name=os_code_name,
    arch=arch,
    base_image=base_image,
    maintainer_name=maintainer_name,
))@

# setup source.list to eol
RUN sed -i -e 's/archive/old-releases/g' /etc/apt/sources.list

@[if 'packages' in locals()]@
@[if packages]@

# install packages
RUN apt-get update && apt-get install -y --no-install-recommends \
    @(' \\\n    '.join(packages))@  \
    && rm -rf /var/lib/apt/lists/*

@[end if]@
@[end if]@
# setup keys
RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 421C365BD9FF1F717815A3895523BAEEB01FA116

# setup sources.list
RUN echo "deb http://packages.ros.org/ros/ubuntu @os_code_name main" > /etc/apt/sources.list.d/ros-latest.list

# setup environment
ENV LANG C.UTF-8
ENV LC_ALL C.UTF-8

# install ros packages
ENV ROS_DISTRO @rosdistro_name
RUN apt-get update && apt-get install -y \
    @(' \\\n    '.join(ros_packages))@  \
    && rm -rf /var/lib/apt/lists/*

@[if 'entrypoint_name' in locals()]@
@[if entrypoint_name]@
@{
entrypoint_file = entrypoint_name.split('/')[-1]
}@
# setup entrypoint
COPY ./@entrypoint_file /ros_entrypoint.sh

ENTRYPOINT ["/ros_entrypoint.sh"]
@[end if]@
@[end if]@
@{
cmds = [
'bash',
]
}@
CMD ["@(' && '.join(cmds))"]


msg: rtps: improve verbosity when the the agent is not capable of parsing a specific ID

@###############################################
@#
@# EmPy template for generating RtpsTopics.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@#  - msgs (List) list of all msg files
@#  - ids (List) list of all RTPS msg ids
@###############################################
@{
import os

import genmsg.msgs

from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/

send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
package = package[0]
}@
/****************************************************************************
 *
 * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
 * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "RtpsTopics.h"

bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue, const std::string& ns)
{
@[if recv_topics]@
    // Initialise subscribers
    std::cout << "\033[0;36m---   Subscribers   ---\033[0m" << std::endl;
@[for topic in recv_topics]@
    if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) {
        std::cout << "- @(topic) subscriber started" << std::endl;
    } else {
        std::cerr << "Failed starting @(topic) subscriber" << std::endl;
        return false;
    }
@[end for]@
    std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl;
@[end if]@
@[if send_topics]@
    // Initialise publishers
    std::cout << "\033[0;36m----   Publishers  ----\033[0m" << std::endl;
@[for topic in send_topics]@
    if (_@(topic)_pub.init(ns)) {
        std::cout << "- @(topic) publisher started" << std::endl;
@[    if topic == 'Timesync' or topic == 'timesync']@
        _timesync->start(&_@(topic)_pub);
@[    end if]@
    } else {
        std::cerr << "ERROR starting @(topic) publisher" << std::endl;
        return false;
    }
@[end for]@
    std::cout << "\033[0;36m-----------------------\033[0m" << std::endl;
@[end if]@
    return true;
}

@[if send_topics]@
void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
{
    switch (topic_ID)
    {
@[for topic in send_topics]@
        case @(rtps_message_id(ids, topic)): // @(topic)
        {
            @(topic)_msg_t st;
            eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
            eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
            st.deserialize(cdr_des);
@[    if topic == 'Timesync' or topic == 'timesync']@
            _timesync->processTimesyncMsg(&st);

            if (getMsgSysID(&st) == 1) {
@[    end if]@
            // apply timestamp offset
            uint64_t timestamp = getMsgTimestamp(&st);
            _timesync->subtractOffset(timestamp);
            setMsgTimestamp(&st, timestamp);
            _@(topic)_pub.publish(&st);
@[    if topic == 'Timesync' or topic == 'timesync']@
            }
@[    end if]@
        }
        break;
@[end for]@
        default:
            printf("\033[1;33m[   micrortps_agent   ]\tUnexpected topic ID '%hhu' to publish Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", topic_ID, topic_ID);
        break;
    }
}
@[end if]@
@[if recv_topics]@

bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
{
    bool ret = false;
    switch (topic_ID)
    {
@[for topic in recv_topics]@
        case @(rtps_message_id(ids, topic)): // @(topic)
            if (_@(topic)_sub.hasMsg())
            {
                @(topic)_msg_t msg = _@(topic)_sub.getMsg();
@[    if topic == 'Timesync' or topic == 'timesync']@
                if (getMsgSysID(&msg) == 0) {
@[    end if]@
                // apply timestamp offset
                uint64_t timestamp = getMsgTimestamp(&msg);
                _timesync->addOffset(timestamp);
                setMsgTimestamp(&msg, timestamp);
                msg.serialize(scdr);
                ret = true;
@[    if topic == 'Timesync' or topic == 'timesync']@
                }
@[    end if]@
                _@(topic)_sub.unlockMsg();
            }
        break;
@[end for]@
        default:
            printf("\033[1;33m[   micrortps_agent   ]\tUnexpected topic ID '%hhu' to getMsg. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", topic_ID, topic_ID);
        break;
    }

    return ret;
}
@[end if]@

@###############################################
@#
@# EmPy template for generating RtpsTopics.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@#  - msgs (List) list of all msg files
@#  - ids (List) list of all RTPS msg ids
@###############################################
@{
import os

import genmsg.msgs

from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/

send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
package = package[0]
}@
/****************************************************************************
 *
 * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
 * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "RtpsTopics.h"

bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue, const std::string& ns)
{
@[if recv_topics]@
    // Initialise subscribers
    std::cout << "\033[0;36m---   Subscribers   ---\033[0m" << std::endl;
@[for topic in recv_topics]@
    if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) {
        std::cout << "- @(topic) subscriber started" << std::endl;
    } else {
        std::cerr << "Failed starting @(topic) subscriber" << std::endl;
        return false;
    }
@[end for]@
    std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl;
@[end if]@
@[if send_topics]@
    // Initialise publishers
    std::cout << "\033[0;36m----   Publishers  ----\033[0m" << std::endl;
@[for topic in send_topics]@
    if (_@(topic)_pub.init(ns)) {
        std::cout << "- @(topic) publisher started" << std::endl;
@[    if topic == 'Timesync' or topic == 'timesync']@
        _timesync->start(&_@(topic)_pub);
@[    end if]@
    } else {
        std::cerr << "ERROR starting @(topic) publisher" << std::endl;
        return false;
    }
@[end for]@
    std::cout << "\033[0;36m-----------------------\033[0m" << std::endl;
@[end if]@
    return true;
}

@[if send_topics]@
void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
{
    switch (topic_ID)
    {
@[for topic in send_topics]@
        case @(rtps_message_id(ids, topic)): // @(topic)
        {
            @(topic)_msg_t st;
            eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
            eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
            st.deserialize(cdr_des);
@[    if topic == 'Timesync' or topic == 'timesync']@
            _timesync->processTimesyncMsg(&st);

            if (getMsgSysID(&st) == 1) {
@[    end if]@
            // apply timestamp offset
            uint64_t timestamp = getMsgTimestamp(&st);
            _timesync->subtractOffset(timestamp);
            setMsgTimestamp(&st, timestamp);
            _@(topic)_pub.publish(&st);
@[    if topic == 'Timesync' or topic == 'timesync']@
            }
@[    end if]@
        }
        break;
@[end for]@
        default:
            printf("\033[1;33m[   micrortps_agent   ]\tUnexpected topic ID to publish\033[0m\n");
        break;
    }
}
@[end if]@
@[if recv_topics]@

bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
{
    bool ret = false;
    switch (topic_ID)
    {
@[for topic in recv_topics]@
        case @(rtps_message_id(ids, topic)): // @(topic)
            if (_@(topic)_sub.hasMsg())
            {
                @(topic)_msg_t msg = _@(topic)_sub.getMsg();
@[    if topic == 'Timesync' or topic == 'timesync']@
                if (getMsgSysID(&msg) == 0) {
@[    end if]@
                // apply timestamp offset
                uint64_t timestamp = getMsgTimestamp(&msg);
                _timesync->addOffset(timestamp);
                setMsgTimestamp(&msg, timestamp);
                msg.serialize(scdr);
                ret = true;
@[    if topic == 'Timesync' or topic == 'timesync']@
                }
@[    end if]@
                _@(topic)_sub.unlockMsg();
            }
        break;
@[end for]@
        default:
            printf("\033[1;33m[   micrortps_agent   ]\tUnexpected topic ID '%hhu' to getMsg\033[0m\n", topic_ID);
        break;
    }

    return ret;
}
@[end if]@