fix returning loan

fix returning loan

// generated from rosidl_typesupport_opensplice_cpp/resource/srv__type_support.cpp.em
// generated code does not contain a copyright notice

@#######################################################################
@# EmPy template for generating <srv>__type_support.cpp files
@#
@# Context:
@#  - spec (rosidl_parser.ServiceSpecification)
@#    Parsed specification of the .srv file
@#  - get_header_filename_from_msg_name (function)
@#######################################################################
@
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>

#include "rosidl_generator_c/service_type_support.h"
// This is defined in the rosidl_typesupport_opensplice_cpp package and
// is in the include/rosidl_typesupport_opensplice_cpp/impl folder.
#include "rosidl_generator_cpp/message_type_support.hpp"
#include "rosidl_generator_cpp/service_type_support.hpp"
#include "rosidl_typesupport_opensplice_cpp/visibility_control.h"
#include "rmw/rmw.h"

@{header_file_name = get_header_filename_from_msg_name(spec.srv_name)}@
#include "@(spec.pkg_name)/srv/@(header_file_name)__struct.hpp"
@{req_header_file_name = get_header_filename_from_msg_name(spec.srv_name + '_Request')}@
@{res_header_file_name = get_header_filename_from_msg_name(spec.srv_name + '_Response')}@
#include "@(spec.pkg_name)/srv/dds_opensplice/@(req_header_file_name)__type_support.hpp"
#include "@(spec.pkg_name)/srv/dds_opensplice/@(res_header_file_name)__type_support.hpp"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_@(spec.srv_name)_Request_.h"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_@(spec.srv_name)_Response_.h"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_Sample_@(spec.srv_name)_Request_.h"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_Sample_@(spec.srv_name)_Response_.h"
#include "rosidl_typesupport_opensplice_cpp/identifier.hpp"
#include "rosidl_typesupport_opensplice_cpp/service_type_support.h"
#include "rosidl_typesupport_opensplice_cpp/requester.hpp"
#include "rosidl_typesupport_opensplice_cpp/responder.hpp"

@{
# Same as @(spec.pkg_name)::srv::dds_::@(spec.srv_name)
__dds_msg_type_prefix = "{spec.pkg_name}::srv::dds_::{spec.srv_name}".format(spec=spec)
# Same as @(spec.pkg_name)::srv::dds_::Sample_@(spec.srv_name)
__dds_sample_type_prefix = "{spec.pkg_name}::srv::dds_::Sample_{spec.srv_name}".format(spec=spec)
}@

namespace rosidl_typesupport_opensplice_cpp
{

@[for suffix in ['_Request', '_Response']]@
template<>
class Sample<@(__dds_msg_type_prefix)@(suffix)_>
  : public @(__dds_sample_type_prefix)@(suffix)_
{
public:
  @(__dds_msg_type_prefix)@(suffix)_ & data()
  {
@[  if suffix == '_Request']@
    return request_;
@[  else]@
    return response_;
@[  end if]@
  }

  operator @(__dds_msg_type_prefix)@(suffix)_() {
    return reinterpret_cast<@(__dds_msg_type_prefix)@(suffix)_ &>(*this);
  }
};

template<>
class TemplateDataReader<@(__dds_sample_type_prefix)@(suffix)_>
  : public @(__dds_sample_type_prefix)@(suffix)_DataReader
{
public:
  static const char * take_sample(
    DDS::DataReader * datareader,
    Sample<@(__dds_msg_type_prefix)@(suffix)_> & sample,
    bool * taken)
  noexcept
  {
    @(__dds_sample_type_prefix)@(suffix)_DataReader * typed_datareader = _narrow(datareader);

    @(__dds_sample_type_prefix)@(suffix)_Seq dds_messages;
    DDS::SampleInfoSeq sample_infos;
    DDS::ReturnCode_t status = typed_datareader->take(
      dds_messages,
      sample_infos,
      1,
      DDS::ANY_SAMPLE_STATE,
      DDS::ANY_VIEW_STATE,
      // See ros2/rclcpp#192 for a justification of this option:
      // https://github.com/ros2/rclcpp/issues/192#issuecomment-183491185
      DDS::ANY_INSTANCE_STATE);

    switch (status) {
      case DDS::RETCODE_ERROR:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "an internal error has occurred";
      case DDS::RETCODE_ALREADY_DELETED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataReader has already been deleted";
      case DDS::RETCODE_OUT_OF_RESOURCES:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "out of resources";
      case DDS::RETCODE_NOT_ENABLED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataReader is not enabled";
      case DDS::RETCODE_PRECONDITION_NOT_MET:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "a precondition is not met, one of: "
               "max_samples > maximum and max_samples != LENGTH_UNLIMITED, or "
               "the two sequences do not have matching parameters (length, maximum, release), or "
               "maximum > 0 and release is false.";
      case DDS::RETCODE_NO_DATA:
        *taken = false;
        return nullptr;
      case DDS::RETCODE_OK:
        break;
      default:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with "
               "unknown return code";
    }

    if (sample_infos.length() > 0 && sample_infos[0].valid_data) {
      sample = reinterpret_cast<Sample<@(__dds_msg_type_prefix)@(suffix)_> &>(dds_messages[0]);
    }
    status = typed_datareader->return_loan(dds_messages, sample_infos);
    switch (status) {
      case DDS::RETCODE_ERROR:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
               "an internal error has occurred";
      case DDS::RETCODE_ALREADY_DELETED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataReader has already been deleted";
      case DDS::RETCODE_OUT_OF_RESOURCES:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
               "out of resources";
      case DDS::RETCODE_NOT_ENABLED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataReader is not enabled";
      case DDS::RETCODE_PRECONDITION_NOT_MET:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
               "a precondition is not met, one of: "
               "the data_values and info_seq do not belong to a single related pair, or "
               "the data_values and info_seq were not obtained from this "
               "@(__dds_sample_type_prefix)@(suffix)_DataReader";
      case DDS::RETCODE_OK:
        *taken = true;
        return nullptr;
      default:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with "
               "unknown return code";
    }

    *taken = false;
    return nullptr;
  }
};

template<>
class TemplateDataReader<Sample<@(__dds_msg_type_prefix)@(suffix)_>>
  : public TemplateDataReader<@(__dds_sample_type_prefix)@(suffix)_>
{
};

template<>
class TemplateDataWriter<@(__dds_sample_type_prefix)@(suffix)_>
  : public @(__dds_sample_type_prefix)@(suffix)_DataWriter
{
public:
  static const char * write_sample(
    DDS::DataWriter * datawriter,
    Sample<@(__dds_msg_type_prefix)@(suffix)_> & sample)
  noexcept
  {
    @(__dds_sample_type_prefix)@(suffix)_DataWriter * typed_datawriter = _narrow(datawriter);

    DDS::ReturnCode_t status = typed_datawriter->write(sample, DDS::HANDLE_NIL);
    switch (status) {
      case DDS::RETCODE_ERROR:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "an internal error has occurred";
      case DDS::RETCODE_BAD_PARAMETER:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "bad handle or instance_data parameter";
      case DDS::RETCODE_ALREADY_DELETED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataWriter has already been deleted";
      case DDS::RETCODE_OUT_OF_RESOURCES:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "out of resources";
      case DDS::RETCODE_NOT_ENABLED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataWriter is not enabled";
      case DDS::RETCODE_PRECONDITION_NOT_MET:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "the handle has not been registered with this "
               "@(__dds_sample_type_prefix)@(suffix)_DataWriter";
      case DDS::RETCODE_TIMEOUT:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "writing resulted in blocking and then exceeded the timeout set by the "
               "max_blocking_time of the ReliabilityQosPolicy";
      case DDS::RETCODE_OK:
        return nullptr;
      default:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: unknown return code";
    }
  }
};

template<>
class TemplateDataWriter<Sample<@(__dds_msg_type_prefix)@(suffix)_>>
  : public TemplateDataWriter<@(__dds_sample_type_prefix)@(suffix)_>
{
};

@[end for]@
}  // namespace rosidl_typesupport_opensplice_cpp

namespace @(spec.pkg_name)
{

namespace srv
{

namespace typesupport_opensplice_cpp
{

const char *
register_types__@(spec.srv_name)(
  void * untyped_participant, const char * request_type_name, const char * response_type_name)
{
  DDS::DomainParticipant * participant = static_cast<DDS::DomainParticipant *>(untyped_participant);

  @(__dds_sample_type_prefix)_Request_TypeSupport ros_request_ts;
  DDS::ReturnCode_t status = ros_request_ts.register_type(participant, request_type_name);
  switch (status) {
    case DDS::RETCODE_ERROR:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "an internal error has occurred";
    case DDS::RETCODE_BAD_PARAMETER:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "bad domain participant or type name parameter";
    case DDS::RETCODE_OUT_OF_RESOURCES:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "out of resources";
    case DDS::RETCODE_PRECONDITION_NOT_MET:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "already registered with a different TypeSupport class";
    case DDS::RETCODE_OK:
      break;
    default:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: unkown return code";
  }

  @(__dds_sample_type_prefix)_Response_TypeSupport ros_response_ts;
  status = ros_response_ts.register_type(participant, response_type_name);
  switch (status) {
    case DDS::RETCODE_ERROR:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "an internal error has occurred";
    case DDS::RETCODE_BAD_PARAMETER:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "bad domain participant or type name parameter";
    case DDS::RETCODE_OUT_OF_RESOURCES:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "out of resources";
    case DDS::RETCODE_PRECONDITION_NOT_MET:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "precondition not met, already registered with a different TypeSupport class";
    case DDS::RETCODE_OK:
      break;
    default:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: unkown return code";
  }
  return nullptr;
}

const char *
create_requester__@(spec.srv_name)(
  void * untyped_participant, const char * service_name,
  void ** untyped_requester, void ** untyped_reader,
  const void * untyped_datareader_qos,
  const void * untyped_datawriter_qos,
  void * (*allocator)(size_t))
{
  auto _allocator = allocator ? allocator : &malloc;
  const std::string service_type_name("@(__dds_sample_type_prefix)");
  const std::string request_type_name("@(__dds_sample_type_prefix)_Request_");
  const std::string response_type_name("@(__dds_sample_type_prefix)_Response_");

  DDS::DomainParticipant * participant =
    static_cast<DDS::DomainParticipant *>(untyped_participant);

  const DDS::DataReaderQos * datareader_qos = static_cast<const DDS::DataReaderQos *>(untyped_datareader_qos);
  const DDS::DataWriterQos * datawriter_qos = static_cast<const DDS::DataWriterQos *>(untyped_datawriter_qos);

  const char * error_string = register_types__@(spec.srv_name)(
    participant, request_type_name.c_str(), response_type_name.c_str());
  if (error_string) {
    return error_string;
  }

  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  RequesterT * requester = static_cast<RequesterT *>(_allocator(sizeof(RequesterT)));
  if (!requester) {
    return "failed to allocate memory for requester";
  }
  // Assumption: this constructor will not throw. This is a reasonable assumption
  // since we do all things which usually can cause problem in init().
  // However, it could still throw a bad_alloc or something like that.
  try {
    new (requester) RequesterT(participant, service_name, service_type_name);
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during construction of RequesterT";
  }
  error_string = requester->init(datareader_qos, datawriter_qos);
  if (error_string) {
    return error_string;
  }

  *untyped_requester = requester;
  *untyped_reader = requester->get_response_datareader();

  return nullptr;
}

const char *
create_responder__@(spec.srv_name)(
  void * untyped_participant, const char * service_name,
  void ** untyped_responder, void ** untyped_reader,
  const void * untyped_datareader_qos,
  const void * untyped_datawriter_qos,
  void * (*allocator)(size_t))
{
  auto _allocator = allocator ? allocator : &malloc;
  const std::string service_type_name("@(__dds_sample_type_prefix)");
  const std::string request_type_name("@(__dds_sample_type_prefix)_Request_");
  const std::string response_type_name("@(__dds_sample_type_prefix)_Response_");

  DDS::DomainParticipant * participant =
    static_cast<DDS::DomainParticipant *>(untyped_participant);

  const DDS::DataReaderQos * datareader_qos = static_cast<const DDS::DataReaderQos *>(untyped_datareader_qos);
  const DDS::DataWriterQos * datawriter_qos = static_cast<const DDS::DataWriterQos *>(untyped_datawriter_qos);

  const char * error_string = register_types__@(spec.srv_name)(
    participant, request_type_name.c_str(), response_type_name.c_str());
  if (error_string) {
    return error_string;
  }

  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  ResponderT * responder = static_cast<ResponderT *>(_allocator(sizeof(ResponderT)));
  if (!responder) {
    return "failed to allocate memory for responder";
  }
  // Assumption: this constructor will not throw. This is a reasonable assumption
  // since we do all things which usually can cause problem in init().
  // However, it could still throw a bad_alloc or something like that.
  try {
    new (responder) ResponderT(participant, service_name, service_type_name);
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during construction of ResponderT";
  }
  error_string = responder->init(datareader_qos, datawriter_qos);
  if (error_string) {
    return error_string;
  }

  *untyped_responder = responder;
  *untyped_reader = responder->get_request_datareader();

  return nullptr;
}

const char *
send_request__@(spec.srv_name)(
  void * untyped_requester, const void * untyped_ros_request, int64_t * sequence_number)
{
  using SampleT = rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Request_>;
  using ROSRequestT = @(spec.pkg_name)::srv::@(spec.srv_name)_Request;

  SampleT request;
  auto ros_request = reinterpret_cast<const ROSRequestT *>(untyped_ros_request);
  @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_ros_message_to_dds(*ros_request, request.data());

  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  auto requester = reinterpret_cast<RequesterT *>(untyped_requester);

  const char * error_string = requester->send_request(request);
  if (error_string) {
    return error_string;
  }
  *sequence_number = request.sequence_number_;

  return nullptr;
}

const char *
take_request__@(spec.srv_name)(
  void * untyped_responder, rmw_request_id_t * request_header, void * untyped_ros_request,
  bool * taken)
{
  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  using ROSRequestT = @(spec.pkg_name)::srv::@(spec.srv_name)_Request;

  auto ros_request = static_cast<ROSRequestT *>(untyped_ros_request);

  auto responder = reinterpret_cast<ResponderT *>(untyped_responder);

  rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Request_> request;
  const char * error_string = responder->take_request(request, taken);
  if (error_string) {
    return error_string;
  }

  if (*taken) {
    @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_dds_message_to_ros(request.data(), *ros_request);

    request_header->sequence_number = request.sequence_number_;
    std::memcpy(
      &request_header->writer_guid[0], &request.client_guid_0_, sizeof(request.client_guid_0_));
    std::memcpy(
      &request_header->writer_guid[0] + sizeof(request.client_guid_0_),
      &request.client_guid_1_, sizeof(request.client_guid_1_));

    *taken = true;
    return nullptr;
  }
  *taken = false;
  return nullptr;
}

const char *
send_response__@(spec.srv_name)(
  void * untyped_responder, const rmw_request_id_t * request_header,
  const void * untyped_ros_response)
{
  using ROSResponseT = @(spec.pkg_name)::srv::@(spec.srv_name)_Response;
  rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Response_> response;
  auto ros_response = reinterpret_cast<const ROSResponseT *>(untyped_ros_response);
  @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_ros_message_to_dds(*ros_response, response.data());

  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  auto responder = reinterpret_cast<ResponderT *>(untyped_responder);

  const char * error_string = responder->send_response(*request_header, response);
  if (error_string) {
    return error_string;
  }
  return nullptr;
}

const char *
take_response__@(spec.srv_name)(
  void * untyped_requester, rmw_request_id_t * request_header, void * untyped_ros_response,
  bool * taken)
{
  using ROSResponseT = @(spec.pkg_name)::srv::@(spec.srv_name)_Response;
  auto ros_response = static_cast<ROSResponseT *>(untyped_ros_response);

  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  auto requester = reinterpret_cast<RequesterT *>(untyped_requester);

  rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Response_> response;
  const char * error_string = requester->take_response(response, taken);
  if (error_string) {
    return error_string;
  }
  if (*taken) {
    request_header->sequence_number = response.sequence_number_;

    @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_dds_message_to_ros(
      response.data(), *ros_response);
    return nullptr;
  }

  return nullptr;
}

const char *
destroy_requester__@(spec.srv_name)(void * untyped_requester, void (* deallocator)(void *))
{
  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  auto requester = static_cast<RequesterT *>(untyped_requester);
  const char * teardown_status = requester->teardown();
  try {
    requester->~RequesterT();
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during destruction of RequesterT";
  }
  if (teardown_status != nullptr) {
    return teardown_status;
  }
  auto _deallocator = deallocator ? deallocator : &free;
  _deallocator(requester);
  return nullptr;
}

const char *
destroy_responder__@(spec.srv_name)(void * untyped_responder, void (* deallocator)(void *))
{
  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  auto responder = static_cast<ResponderT *>(untyped_responder);
  const char * teardown_status = responder->teardown();
  try {
    responder->~ResponderT();
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during destruction of ResponderT";
  }
  if (teardown_status != nullptr) {
    return teardown_status;
  }
  auto _deallocator = deallocator ? deallocator : &free;
  _deallocator(responder);
  return nullptr;
}

const char *
server_is_available__@(spec.srv_name)(
  void * requester, const rmw_node_t * node, bool * is_available,
  rmw_ret_t (*count_publishers)(const rmw_node_t *, const char *, size_t *),
  rmw_ret_t (*count_subscribers)(const rmw_node_t *, const char *, size_t *))
{
  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
    @(__dds_msg_type_prefix)_Request_,
    @(__dds_msg_type_prefix)_Response_
  >;

  auto typed_requester = reinterpret_cast<RequesterT *>(requester);

  return typed_requester->server_is_available(
    node, is_available, count_publishers, count_subscribers);
}

static service_type_support_callbacks_t callbacks = {
  "@(spec.pkg_name)",
  "@(spec.srv_name)",
  &create_requester__@(spec.srv_name),
  &destroy_requester__@(spec.srv_name),
  &create_responder__@(spec.srv_name),
  &destroy_responder__@(spec.srv_name),
  &send_request__@(spec.srv_name),
  &take_request__@(spec.srv_name),
  &send_response__@(spec.srv_name),
  &take_response__@(spec.srv_name),
  &server_is_available__@(spec.srv_name),
};

static rosidl_service_type_support_t handle = {
  rosidl_typesupport_opensplice_cpp::typesupport_opensplice_identifier,
  &callbacks
};

}  // namespace typesupport_opensplice_cpp

}  // namespace srv

}  // namespace @(spec.pkg_name)

namespace rosidl_typesupport_opensplice_cpp
{

template<>
ROSIDL_TYPESUPPORT_OPENSPLICE_CPP_EXPORT
const rosidl_service_type_support_t *
get_service_type_support_handle_opensplice<@(spec.pkg_name)::srv::@(spec.srv_name)>()
{
  return &@(spec.pkg_name)::srv::typesupport_opensplice_cpp::handle;
}

}  // namespace rosidl_typesupport_opensplice_cpp

// generated from rosidl_typesupport_opensplice_cpp/resource/srv__type_support.cpp.em
// generated code does not contain a copyright notice

@#######################################################################
@# EmPy template for generating <srv>__type_support.cpp files
@#
@# Context:
@#  - spec (rosidl_parser.ServiceSpecification)
@#    Parsed specification of the .srv file
@#  - get_header_filename_from_msg_name (function)
@#######################################################################
@
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>

#include "rosidl_generator_c/service_type_support.h"
// This is defined in the rosidl_typesupport_opensplice_cpp package and
// is in the include/rosidl_typesupport_opensplice_cpp/impl folder.
#include "rosidl_generator_cpp/message_type_support.hpp"
#include "rosidl_generator_cpp/service_type_support.hpp"
#include "rosidl_typesupport_opensplice_cpp/visibility_control.h"
#include "rmw/rmw.h"

@{header_file_name = get_header_filename_from_msg_name(spec.srv_name)}@
#include "@(spec.pkg_name)/srv/@(header_file_name)__struct.hpp"
@{req_header_file_name = get_header_filename_from_msg_name(spec.srv_name + '_Request')}@
@{res_header_file_name = get_header_filename_from_msg_name(spec.srv_name + '_Response')}@
#include "@(spec.pkg_name)/srv/dds_opensplice/@(req_header_file_name)__type_support.hpp"
#include "@(spec.pkg_name)/srv/dds_opensplice/@(res_header_file_name)__type_support.hpp"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_@(spec.srv_name)_Request_.h"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_@(spec.srv_name)_Response_.h"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_Sample_@(spec.srv_name)_Request_.h"
#include "@(spec.pkg_name)/srv/dds_opensplice/ccpp_Sample_@(spec.srv_name)_Response_.h"
#include "rosidl_typesupport_opensplice_cpp/identifier.hpp"
#include "rosidl_typesupport_opensplice_cpp/service_type_support.h"
#include "rosidl_typesupport_opensplice_cpp/requester.hpp"
#include "rosidl_typesupport_opensplice_cpp/responder.hpp"

@{
# Same as @(spec.pkg_name)::srv::dds_::@(spec.srv_name)
__dds_msg_type_prefix = "{spec.pkg_name}::srv::dds_::{spec.srv_name}".format(spec=spec)
# Same as @(spec.pkg_name)::srv::dds_::Sample_@(spec.srv_name)
__dds_sample_type_prefix = "{spec.pkg_name}::srv::dds_::Sample_{spec.srv_name}".format(spec=spec)
}@

namespace rosidl_typesupport_opensplice_cpp
{

@[for suffix in ['_Request', '_Response']]@
template<>
class Sample<@(__dds_msg_type_prefix)@(suffix)_>
  : public @(__dds_sample_type_prefix)@(suffix)_
{
public:
  @(__dds_msg_type_prefix)@(suffix)_ & data()
  {
@[  if suffix == '_Request']@
    return request_;
@[  else]@
    return response_;
@[  end if]@
  }

  operator @(__dds_msg_type_prefix)@(suffix)_() {
    return reinterpret_cast<@(__dds_msg_type_prefix)@(suffix)_ &>(*this);
  }
};

template<>
class TemplateDataReader<@(__dds_sample_type_prefix)@(suffix)_>
  : public @(__dds_sample_type_prefix)@(suffix)_DataReader
{
public:
  static const char * take_sample(
    DDS::DataReader * datareader,
    Sample<@(__dds_msg_type_prefix)@(suffix)_> & sample,
    bool * taken)
  noexcept
  {
    @(__dds_sample_type_prefix)@(suffix)_DataReader * typed_datareader = _narrow(datareader);

    @(__dds_sample_type_prefix)@(suffix)_Seq dds_messages;
    DDS::SampleInfoSeq sample_infos;
    DDS::ReturnCode_t status = typed_datareader->take(
      dds_messages,
      sample_infos,
      1,
      DDS::ANY_SAMPLE_STATE,
      DDS::ANY_VIEW_STATE,
      // See ros2/rclcpp#192 for a justification of this option:
      // https://github.com/ros2/rclcpp/issues/192#issuecomment-183491185
      DDS::ANY_INSTANCE_STATE);

    switch (status) {
      case DDS::RETCODE_ERROR:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "an internal error has occurred";
      case DDS::RETCODE_ALREADY_DELETED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataReader has already been deleted";
      case DDS::RETCODE_OUT_OF_RESOURCES:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "out of resources";
      case DDS::RETCODE_NOT_ENABLED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataReader is not enabled";
      case DDS::RETCODE_PRECONDITION_NOT_MET:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with: "
               "a precondition is not met, one of: "
               "max_samples > maximum and max_samples != LENGTH_UNLIMITED, or "
               "the two sequences do not have matching parameters (length, maximum, release), or "
               "maximum > 0 and release is false.";
      case DDS::RETCODE_NO_DATA:
        *taken = false;
        return nullptr;
      case DDS::RETCODE_OK:
        break;
      default:
        return "@(__dds_sample_type_prefix)@(suffix)_DataReader.take failed with "
               "unknown return code";
    }

    if (sample_infos.length() > 0 && sample_infos[0].valid_data) {
      sample = reinterpret_cast<Sample<@(__dds_msg_type_prefix)@(suffix)_> &>(dds_messages[0]);
      status = typed_datareader->return_loan(dds_messages, sample_infos);
      switch (status) {
        case DDS::RETCODE_ERROR:
          return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
                 "an internal error has occurred";
        case DDS::RETCODE_ALREADY_DELETED:
          return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
                 "this @(__dds_sample_type_prefix)@(suffix)_DataReader has already been deleted";
        case DDS::RETCODE_OUT_OF_RESOURCES:
          return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
                 "out of resources";
        case DDS::RETCODE_NOT_ENABLED:
          return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
                 "this @(__dds_sample_type_prefix)@(suffix)_DataReader is not enabled";
        case DDS::RETCODE_PRECONDITION_NOT_MET:
          return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with: "
                 "a precondition is not met, one of: "
                 "the data_values and info_seq do not belong to a single related pair, or "
                 "the data_values and info_seq were not obtained from this "
                 "@(__dds_sample_type_prefix)@(suffix)_DataReader";
        case DDS::RETCODE_OK:
          *taken = true;
          return nullptr;
        default:
          return "@(__dds_sample_type_prefix)@(suffix)_DataReader.return_loan failed with "
                 "unknown return code";
      }
    }

    *taken = false;
    return nullptr;
  }
};

template<>
class TemplateDataReader<Sample<@(__dds_msg_type_prefix)@(suffix)_>>
  : public TemplateDataReader<@(__dds_sample_type_prefix)@(suffix)_>
{
};

template<>
class TemplateDataWriter<@(__dds_sample_type_prefix)@(suffix)_>
  : public @(__dds_sample_type_prefix)@(suffix)_DataWriter
{
public:
  static const char * write_sample(
    DDS::DataWriter * datawriter,
    Sample<@(__dds_msg_type_prefix)@(suffix)_> & sample)
  noexcept
  {
    @(__dds_sample_type_prefix)@(suffix)_DataWriter * typed_datawriter = _narrow(datawriter);

    DDS::ReturnCode_t status = typed_datawriter->write(sample, DDS::HANDLE_NIL);
    switch (status) {
      case DDS::RETCODE_ERROR:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "an internal error has occurred";
      case DDS::RETCODE_BAD_PARAMETER:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "bad handle or instance_data parameter";
      case DDS::RETCODE_ALREADY_DELETED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataWriter has already been deleted";
      case DDS::RETCODE_OUT_OF_RESOURCES:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "out of resources";
      case DDS::RETCODE_NOT_ENABLED:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "this @(__dds_sample_type_prefix)@(suffix)_DataWriter is not enabled";
      case DDS::RETCODE_PRECONDITION_NOT_MET:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "the handle has not been registered with this "
               "@(__dds_sample_type_prefix)@(suffix)_DataWriter";
      case DDS::RETCODE_TIMEOUT:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: "
               "writing resulted in blocking and then exceeded the timeout set by the "
               "max_blocking_time of the ReliabilityQosPolicy";
      case DDS::RETCODE_OK:
        return nullptr;
      default:
        return "@(__dds_sample_type_prefix)@(suffix)_DataWriter.write: unknown return code";
    }
  }
};

template<>
class TemplateDataWriter<Sample<@(__dds_msg_type_prefix)@(suffix)_>>
  : public TemplateDataWriter<@(__dds_sample_type_prefix)@(suffix)_>
{
};

@[end for]@
}  // namespace rosidl_typesupport_opensplice_cpp

namespace @(spec.pkg_name)
{

namespace srv
{

namespace typesupport_opensplice_cpp
{

const char *
register_types__@(spec.srv_name)(
  void * untyped_participant, const char * request_type_name, const char * response_type_name)
{
  DDS::DomainParticipant * participant = static_cast<DDS::DomainParticipant *>(untyped_participant);

  @(__dds_sample_type_prefix)_Request_TypeSupport ros_request_ts;
  DDS::ReturnCode_t status = ros_request_ts.register_type(participant, request_type_name);
  switch (status) {
    case DDS::RETCODE_ERROR:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "an internal error has occurred";
    case DDS::RETCODE_BAD_PARAMETER:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "bad domain participant or type name parameter";
    case DDS::RETCODE_OUT_OF_RESOURCES:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "out of resources";
    case DDS::RETCODE_PRECONDITION_NOT_MET:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: "
             "already registered with a different TypeSupport class";
    case DDS::RETCODE_OK:
      break;
    default:
      return "@(__dds_sample_type_prefix)_Request_TypeSupport.register_type: unkown return code";
  }

  @(__dds_sample_type_prefix)_Response_TypeSupport ros_response_ts;
  status = ros_response_ts.register_type(participant, response_type_name);
  switch (status) {
    case DDS::RETCODE_ERROR:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "an internal error has occurred";
    case DDS::RETCODE_BAD_PARAMETER:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "bad domain participant or type name parameter";
    case DDS::RETCODE_OUT_OF_RESOURCES:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "out of resources";
    case DDS::RETCODE_PRECONDITION_NOT_MET:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: "
             "precondition not met, already registered with a different TypeSupport class";
    case DDS::RETCODE_OK:
      break;
    default:
      return "@(__dds_sample_type_prefix)_Response_TypeSupport.register_type: unkown return code";
  }
  return nullptr;
}

const char *
create_requester__@(spec.srv_name)(
  void * untyped_participant, const char * service_name,
  void ** untyped_requester, void ** untyped_reader,
  const void * untyped_datareader_qos,
  const void * untyped_datawriter_qos,
  void * (*allocator)(size_t))
{
  auto _allocator = allocator ? allocator : &malloc;
  const std::string service_type_name("@(__dds_sample_type_prefix)");
  const std::string request_type_name("@(__dds_sample_type_prefix)_Request_");
  const std::string response_type_name("@(__dds_sample_type_prefix)_Response_");

  DDS::DomainParticipant * participant =
    static_cast<DDS::DomainParticipant *>(untyped_participant);

  const DDS::DataReaderQos * datareader_qos = static_cast<const DDS::DataReaderQos *>(untyped_datareader_qos);
  const DDS::DataWriterQos * datawriter_qos = static_cast<const DDS::DataWriterQos *>(untyped_datawriter_qos);

  const char * error_string = register_types__@(spec.srv_name)(
    participant, request_type_name.c_str(), response_type_name.c_str());
  if (error_string) {
    return error_string;
  }

  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  RequesterT * requester = static_cast<RequesterT *>(_allocator(sizeof(RequesterT)));
  if (!requester) {
    return "failed to allocate memory for requester";
  }
  // Assumption: this constructor will not throw. This is a reasonable assumption
  // since we do all things which usually can cause problem in init().
  // However, it could still throw a bad_alloc or something like that.
  try {
    new (requester) RequesterT(participant, service_name, service_type_name);
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during construction of RequesterT";
  }
  error_string = requester->init(datareader_qos, datawriter_qos);
  if (error_string) {
    return error_string;
  }

  *untyped_requester = requester;
  *untyped_reader = requester->get_response_datareader();

  return nullptr;
}

const char *
create_responder__@(spec.srv_name)(
  void * untyped_participant, const char * service_name,
  void ** untyped_responder, void ** untyped_reader,
  const void * untyped_datareader_qos,
  const void * untyped_datawriter_qos,
  void * (*allocator)(size_t))
{
  auto _allocator = allocator ? allocator : &malloc;
  const std::string service_type_name("@(__dds_sample_type_prefix)");
  const std::string request_type_name("@(__dds_sample_type_prefix)_Request_");
  const std::string response_type_name("@(__dds_sample_type_prefix)_Response_");

  DDS::DomainParticipant * participant =
    static_cast<DDS::DomainParticipant *>(untyped_participant);

  const DDS::DataReaderQos * datareader_qos = static_cast<const DDS::DataReaderQos *>(untyped_datareader_qos);
  const DDS::DataWriterQos * datawriter_qos = static_cast<const DDS::DataWriterQos *>(untyped_datawriter_qos);

  const char * error_string = register_types__@(spec.srv_name)(
    participant, request_type_name.c_str(), response_type_name.c_str());
  if (error_string) {
    return error_string;
  }

  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  ResponderT * responder = static_cast<ResponderT *>(_allocator(sizeof(ResponderT)));
  if (!responder) {
    return "failed to allocate memory for responder";
  }
  // Assumption: this constructor will not throw. This is a reasonable assumption
  // since we do all things which usually can cause problem in init().
  // However, it could still throw a bad_alloc or something like that.
  try {
    new (responder) ResponderT(participant, service_name, service_type_name);
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during construction of ResponderT";
  }
  error_string = responder->init(datareader_qos, datawriter_qos);
  if (error_string) {
    return error_string;
  }

  *untyped_responder = responder;
  *untyped_reader = responder->get_request_datareader();

  return nullptr;
}

const char *
send_request__@(spec.srv_name)(
  void * untyped_requester, const void * untyped_ros_request, int64_t * sequence_number)
{
  using SampleT = rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Request_>;
  using ROSRequestT = @(spec.pkg_name)::srv::@(spec.srv_name)_Request;

  SampleT request;
  auto ros_request = reinterpret_cast<const ROSRequestT *>(untyped_ros_request);
  @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_ros_message_to_dds(*ros_request, request.data());

  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  auto requester = reinterpret_cast<RequesterT *>(untyped_requester);

  const char * error_string = requester->send_request(request);
  if (error_string) {
    return error_string;
  }
  *sequence_number = request.sequence_number_;

  return nullptr;
}

const char *
take_request__@(spec.srv_name)(
  void * untyped_responder, rmw_request_id_t * request_header, void * untyped_ros_request,
  bool * taken)
{
  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  using ROSRequestT = @(spec.pkg_name)::srv::@(spec.srv_name)_Request;

  auto ros_request = static_cast<ROSRequestT *>(untyped_ros_request);

  auto responder = reinterpret_cast<ResponderT *>(untyped_responder);

  rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Request_> request;
  const char * error_string = responder->take_request(request, taken);
  if (error_string) {
    return error_string;
  }

  if (*taken) {
    @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_dds_message_to_ros(request.data(), *ros_request);

    request_header->sequence_number = request.sequence_number_;
    std::memcpy(
      &request_header->writer_guid[0], &request.client_guid_0_, sizeof(request.client_guid_0_));
    std::memcpy(
      &request_header->writer_guid[0] + sizeof(request.client_guid_0_),
      &request.client_guid_1_, sizeof(request.client_guid_1_));

    *taken = true;
    return nullptr;
  }
  *taken = false;
  return nullptr;
}

const char *
send_response__@(spec.srv_name)(
  void * untyped_responder, const rmw_request_id_t * request_header,
  const void * untyped_ros_response)
{
  using ROSResponseT = @(spec.pkg_name)::srv::@(spec.srv_name)_Response;
  rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Response_> response;
  auto ros_response = reinterpret_cast<const ROSResponseT *>(untyped_ros_response);
  @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_ros_message_to_dds(*ros_response, response.data());

  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  auto responder = reinterpret_cast<ResponderT *>(untyped_responder);

  const char * error_string = responder->send_response(*request_header, response);
  if (error_string) {
    return error_string;
  }
  return nullptr;
}

const char *
take_response__@(spec.srv_name)(
  void * untyped_requester, rmw_request_id_t * request_header, void * untyped_ros_response,
  bool * taken)
{
  using ROSResponseT = @(spec.pkg_name)::srv::@(spec.srv_name)_Response;
  auto ros_response = static_cast<ROSResponseT *>(untyped_ros_response);

  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  auto requester = reinterpret_cast<RequesterT *>(untyped_requester);

  rosidl_typesupport_opensplice_cpp::Sample<@(__dds_msg_type_prefix)_Response_> response;
  const char * error_string = requester->take_response(response, taken);
  if (error_string) {
    return error_string;
  }
  if (*taken) {
    request_header->sequence_number = response.sequence_number_;

    @(spec.pkg_name)::srv::typesupport_opensplice_cpp::convert_dds_message_to_ros(
      response.data(), *ros_response);
    return nullptr;
  }

  return nullptr;
}

const char *
destroy_requester__@(spec.srv_name)(void * untyped_requester, void (* deallocator)(void *))
{
  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;

  auto requester = static_cast<RequesterT *>(untyped_requester);
  const char * teardown_status = requester->teardown();
  try {
    requester->~RequesterT();
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during destruction of RequesterT";
  }
  if (teardown_status != nullptr) {
    return teardown_status;
  }
  auto _deallocator = deallocator ? deallocator : &free;
  _deallocator(requester);
  return nullptr;
}

const char *
destroy_responder__@(spec.srv_name)(void * untyped_responder, void (* deallocator)(void *))
{
  using ResponderT = rosidl_typesupport_opensplice_cpp::Responder<
      @(__dds_msg_type_prefix)_Request_,
      @(__dds_msg_type_prefix)_Response_>;
  auto responder = static_cast<ResponderT *>(untyped_responder);
  const char * teardown_status = responder->teardown();
  try {
    responder->~ResponderT();
  } catch (...) {
    // TODO(wjwwood): catch errors and add their type and what() to the error.
    // This cannot be done when we just return const char *, so a more complex
    // error reporting will be required for this function if we continue to
    // throw exceptions below.
    return "C++ exception caught during destruction of ResponderT";
  }
  if (teardown_status != nullptr) {
    return teardown_status;
  }
  auto _deallocator = deallocator ? deallocator : &free;
  _deallocator(responder);
  return nullptr;
}

const char *
server_is_available__@(spec.srv_name)(
  void * requester, const rmw_node_t * node, bool * is_available,
  rmw_ret_t (*count_publishers)(const rmw_node_t *, const char *, size_t *),
  rmw_ret_t (*count_subscribers)(const rmw_node_t *, const char *, size_t *))
{
  using RequesterT = rosidl_typesupport_opensplice_cpp::Requester<
    @(__dds_msg_type_prefix)_Request_,
    @(__dds_msg_type_prefix)_Response_
  >;

  auto typed_requester = reinterpret_cast<RequesterT *>(requester);

  return typed_requester->server_is_available(
    node, is_available, count_publishers, count_subscribers);
}

static service_type_support_callbacks_t callbacks = {
  "@(spec.pkg_name)",
  "@(spec.srv_name)",
  &create_requester__@(spec.srv_name),
  &destroy_requester__@(spec.srv_name),
  &create_responder__@(spec.srv_name),
  &destroy_responder__@(spec.srv_name),
  &send_request__@(spec.srv_name),
  &take_request__@(spec.srv_name),
  &send_response__@(spec.srv_name),
  &take_response__@(spec.srv_name),
  &server_is_available__@(spec.srv_name),
};

static rosidl_service_type_support_t handle = {
  rosidl_typesupport_opensplice_cpp::typesupport_opensplice_identifier,
  &callbacks
};

}  // namespace typesupport_opensplice_cpp

}  // namespace srv

}  // namespace @(spec.pkg_name)

namespace rosidl_typesupport_opensplice_cpp
{

template<>
ROSIDL_TYPESUPPORT_OPENSPLICE_CPP_EXPORT
const rosidl_service_type_support_t *
get_service_type_support_handle_opensplice<@(spec.pkg_name)::srv::@(spec.srv_name)>()
{
  return &@(spec.pkg_name)::srv::typesupport_opensplice_cpp::handle;
}

}  // namespace rosidl_typesupport_opensplice_cpp


GOPATH now works during first build and DEPENDS variables fixed.

# vim: ft=cmake :

function(_rosgo_setup_global_variable)
    set(libdir "${CATKIN_DEVEL_PREFIX}/lib")
    set(root "${libdir}/go")
    file(MAKE_DIRECTORY ${root})
    execute_process(COMMAND go env GOARCH OUTPUT_VARIABLE goarch OUTPUT_STRIP_TRAILING_WHITESPACE)
    execute_process(COMMAND go env GOOS OUTPUT_VARIABLE goos OUTPUT_STRIP_TRAILING_WHITESPACE)
    set_property(GLOBAL PROPERTY _ROSGO_ROOT "${root}")
    set_property(GLOBAL PROPERTY _ROSGO_BIN "${libdir}")
    set_property(GLOBAL PROPERTY _ROSGO_SRC "${root}/src")
    set_property(GLOBAL PROPERTY _ROSGO_PKG "${root}/pkg/${goos}_${goarch}")
    get_property(gopath GLOBAL PROPERTY _ROSGO_PATH)
    if("${gopath}" STREQUAL "")
        set_property(GLOBAL PROPERTY _ROSGO_PATH "${root}")
    endif()
    set_property(GLOBAL APPEND PROPERTY _ROSGO_PATH "${PROJECT_SOURCE_DIR}")
endfunction()

# This will be evaluated per each project that depend `rosgo_build_tools`.
_rosgo_setup_global_variable()

function(_rosgo_make_gopath gopath_result)
  if("$ENV{GOPATH}" STREQUAL "")
    set(${gopath_result} ${CATKIN_DEVEL_PREFIX}/lib/go PARENT_SCOPE)
  else()
    set(${gopath_result} ${CATKIN_DEVEL_PREFIX}/lib/go:$ENV{GOPATH} PARENT_SCOPE)
  endif()
endfunction()

# Clear old symlinks and create new ones that point original sources.
function(_rosgo_mirror_go_files package var)
    get_filename_component(orig_dir "${PROJECT_SOURCE_DIR}/src/${package}" ABSOLUTE)
    get_filename_component(link_dir "${CATKIN_DEVEL_PREFIX}/lib/go/src/${package}" ABSOLUTE)

    file(MAKE_DIRECTORY "${link_dir}")

    file(GLOB orig_paths "${orig_dir}/*.go")
    set(filenames "")
    foreach(p ${orig_paths})
        get_filename_component(f ${p} NAME)
        list(APPEND filenames "${f}")
    endforeach()

    file(GLOB last_items "${link_dirs}/*.go")
    foreach(item ${last_items})
        if(IS_SYMLINK "${item}")
            file(REMOVE "${item}")
        endif()
    endforeach()

    set(links "")
    foreach(filename ${filenames})
        set(orig "${orig_dir}/${filename}")
        set(link "${link_dir}/${filename}")
        add_custom_command(
            OUTPUT "${link}"
            COMMAND ${CMAKE_COMMAND} -E create_symlink "${orig}" "${link}"
            )
        list(APPEND links "${link}")
    endforeach()
    set(${var} ${links} PARENT_SCOPE)
endfunction()

function(rosgo_add_executable)
    set(options)
    set(one_value_args TARGET)
    set(multi_value_args DEPENDS)
    cmake_parse_arguments(rosgo_add_executable "${options}" "${one_value_args}"
                          "${multi_value_args}" "${ARGN}")
    list(GET rosgo_add_executable_UNPARSED_ARGUMENTS 0 package)
    if("${rosgo_add_executable_TARGET}" STREQUAL "")
        string(REPLACE "/" "_" target "${PROJECT_NAME}_${package}")
        if(NOT ${target} STREQUAL ${PROJECT_NAME}_NOTFOUND)
            set(rosgo_add_executable_TARGET ${target})
        endif()
    endif()

    _rosgo_mirror_go_files(${package} src_links)

    string(REPLACE "/" ";" exe_path "${package}")
    list(GET exe_path -1 exe_name)
    set(exe "${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/${exe_name}")

    _rosgo_make_gopath(gopath)

    add_custom_target(
            ${rosgo_add_executable_TARGET} ALL
            COMMAND env GOPATH=${gopath} go build -o ${exe} ${package}
            DEPENDS ${rosgo_add_executable_DEPENDS} ${src_links})
endfunction()

function(rosgo_add_library)
    set(options)
    set(one_value_args TARGET)
    set(multi_value_args DEPENDS)
    cmake_parse_arguments(rosgo_add_library "${options}" "${one_value_args}"
                          "${multi_value_args}" "${ARGN}")
    list(GET rosgo_add_library_UNPARSED_ARGUMENTS 0 package)
    if("${rosgo_add_library_TARGET}" STREQUAL "")
        string(REPLACE "/" "_" target "${PROJECT_NAME}_${package}")
        if(NOT ${target} STREQUAL ${PROJECT_NAME}_NOTFOUND)
            set(rosgo_add_library_TARGET ${target})
        endif()
    endif()

    _rosgo_mirror_go_files(${package} src_links)
    get_property(gopkg GLOBAL PROPERTY _ROSGO_PKG)

    _rosgo_make_gopath(gopath)

    add_custom_target(
            ${rosgo_add_library_TARGET} ALL
            COMMAND env GOPATH=${gopath} go build -o ${gopkg}/${package}.a ${package}
            DEPENDS ${rosgo_add_library_DEPENDS} ${src_links})
endfunction()

function(rosgo_add_test)
    set(options)
    set(one_value_args)
    set(multi_value_args DEPENDS)
    cmake_parse_arguments(rosgo_add_test "${options}" "${one_value_args}"
                          "${multi_value_args}" "${ARGN}")
    list(GET rosgo_add_test_UNPARSED_ARGUMENTS 0 package)
    string(REPLACE "/" "_" target "${package}")

    _rosgo_mirror_go_files(${package} src_links)

    _rosgo_make_gopath(gopath)

    add_custom_target(
        run_tests_${PROJECT_NAME}_gotest_${target}
        COMMAND env GOPATH=${gopath} go test ${package}
        DEPENDS ${rosgo_add_test_DEPENDS} ${src_links})

    # Register this test to workspace-wide run_tests target
    if(NOT TARGET run_tests_${PROJECT_NAME}_gotest)
        add_custom_target(run_tests_${PROJECT_NAME}_gotest)
        add_dependencies(run_tests run_tests_${PROJECT_NAME}_gotest)
    endif()
    add_dependencies(run_tests_${PROJECT_NAME}_gotest
                     run_tests_${PROJECT_NAME}_gotest_${target})
endfunction()

# vim: ft=cmake :

function(_rosgo_setup_global_variable)
    set(libdir "${CATKIN_DEVEL_PREFIX}/lib")
    set(root "${libdir}/go")
    file(MAKE_DIRECTORY ${root})
    execute_process(COMMAND go env GOARCH OUTPUT_VARIABLE goarch OUTPUT_STRIP_TRAILING_WHITESPACE)
    execute_process(COMMAND go env GOOS OUTPUT_VARIABLE goos OUTPUT_STRIP_TRAILING_WHITESPACE)
    set_property(GLOBAL PROPERTY _ROSGO_ROOT "${root}")
    set_property(GLOBAL PROPERTY _ROSGO_BIN "${libdir}")
    set_property(GLOBAL PROPERTY _ROSGO_SRC "${root}/src")
    set_property(GLOBAL PROPERTY _ROSGO_PKG "${root}/pkg/${goos}_${goarch}")
    get_property(gopath GLOBAL PROPERTY _ROSGO_PATH)
    if("${gopath}" STREQUAL "")
        set_property(GLOBAL PROPERTY _ROSGO_PATH "${root}")
    endif()
    set_property(GLOBAL APPEND PROPERTY _ROSGO_PATH "${PROJECT_SOURCE_DIR}")
endfunction()

# This will be evaluated per each project that depend `rosgo_build_tools`.
_rosgo_setup_global_variable()

# Clear old symlinks and create new ones that point original sources.
function(_rosgo_mirror_go_files package var)
    get_filename_component(orig_dir "${PROJECT_SOURCE_DIR}/src/${package}" ABSOLUTE)
    get_filename_component(link_dir "${CATKIN_DEVEL_PREFIX}/lib/go/src/${package}" ABSOLUTE)

    file(MAKE_DIRECTORY "${link_dir}")

    file(GLOB orig_paths "${orig_dir}/*.go")
    set(filenames "")
    foreach(p ${orig_paths})
        get_filename_component(f ${p} NAME)
        list(APPEND filenames "${f}")
    endforeach()

    file(GLOB last_items "${link_dirs}/*.go")
    foreach(item ${last_items})
        if(IS_SYMLINK "${item}")
            file(REMOVE "${item}")
        endif()
    endforeach()

    set(links "")
    foreach(filename ${filenames})
        set(orig "${orig_dir}/${filename}")
        set(link "${link_dir}/${filename}")
        add_custom_command(
            OUTPUT "${link}"
            COMMAND ${CMAKE_COMMAND} -E create_symlink "${orig}" "${link}"
            )
        list(APPEND links "${link}")
    endforeach()
    set(${var} ${links} PARENT_SCOPE)
endfunction()

function(rosgo_add_executable)
    set(options)
    set(one_value_args TARGET)
    set(multi_value_args DEPENDS)
    cmake_parse_arguments(rosgo_add_executable "${options}" "${one_value_args}"
                          "${multi_value_args}" "${ARGN}")
    list(GET rosgo_add_executable_UNPARSED_ARGUMENTS 0 package)
    if("${rosgo_add_executable_TARGET}" STREQUAL "")
        string(REPLACE "/" "_" target "${PROJECT_NAME}_${package}")
        if(NOT ${target} STREQUAL ${PROJECT_NAME}_NOTFOUND)
            set(rosgo_add_executable_TARGET ${target})
        endif()
    endif()

    _rosgo_mirror_go_files(${package} src_links)

    string(REPLACE "/" ";" exe_path "${package}")
    list(GET exe_path -1 exe_name)
    set(exe "${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/${exe_name}")

    add_custom_target(
            ${rosgo_add_executable_TARGET} ALL
            COMMAND env GOPATH=$ENV{GOPATH} go build -o ${exe} ${package}
            DEPENDS ${DEPENDS} ${src_links})
endfunction()

function(rosgo_add_library)
    set(options)
    set(one_value_args TARGET)
    set(multi_value_args DEPENDS)
    cmake_parse_arguments(rosgo_add_library "${options}" "${one_value_args}"
                          "${multi_value_args}" "${ARGN}")
    list(GET rosgo_add_library_UNPARSED_ARGUMENTS 0 package)
    if("${rosgo_add_library_TARGET}" STREQUAL "")
        string(REPLACE "/" "_" target "${PROJECT_NAME}_${package}")
        if(NOT ${target} STREQUAL ${PROJECT_NAME}_NOTFOUND)
            set(rosgo_add_library_TARGET ${target})
        endif()
    endif()

    _rosgo_mirror_go_files(${package} src_links)
    get_property(gopkg GLOBAL PROPERTY _ROSGO_PKG)

    add_custom_target(
            ${rosgo_add_library_TARGET} ALL
            COMMAND env GOPATH=$ENV{GOPATH} go build -o ${gopkg}/${package}.a ${package}
            DEPENDS ${DEPENDS} ${src_links})
endfunction()

function(rosgo_add_test)
    set(options)
    set(one_value_args)
    set(multi_value_args DEPENDS)
    cmake_parse_arguments(rosgo_add_test "${options}" "${one_value_args}"
                          "${multi_value_args}" "${ARGN}")
    list(GET rosgo_add_test_UNPARSED_ARGUMENTS 0 package)
    string(REPLACE "/" "_" target "${package}")

    _rosgo_mirror_go_files(${package} src_links)

    add_custom_target(
        run_tests_${PROJECT_NAME}_gotest_${target}
        COMMAND env GOPATH=$ENV{GOPATH} go test ${package}
        DEPENDS ${rosgo_add_test_DEPENDS} ${src_links})

    # Register this test to workspace-wide run_tests target
    if(NOT TARGET run_tests_${PROJECT_NAME}_gotest)
        add_custom_target(run_tests_${PROJECT_NAME}_gotest)
        add_dependencies(run_tests run_tests_${PROJECT_NAME}_gotest)
    endif()
    add_dependencies(run_tests_${PROJECT_NAME}_gotest
                     run_tests_${PROJECT_NAME}_gotest_${target})
endfunction()


cast to std::array (#321)

cast to std::array (#321)

* cast to std::array

* take member as a ref

* use auto

// generated from rosidl_typesupport_introspection_cpp/resource/msg__type_support.cpp.em
// generated code does not contain a copyright notice

@#######################################################################
@# EmPy template for generating <msg>__type_support.cpp files
@#
@# Context:
@#  - spec (rosidl_parser.MessageSpecification)
@#    Parsed specification of the .msg file
@#  - subfolder (string)
@#    The subfolder / subnamespace of the message
@#    Either 'msg', 'srv' or 'action'
@#  - get_header_filename_from_msg_name (function)
@#######################################################################
@
#include <array>
// providing offsetof()
#include <cstddef>
#include <vector>

#include "rosidl_generator_c/message_type_support_struct.h"
#include "rosidl_typesupport_cpp/message_type_support.hpp"
#include "rosidl_typesupport_interface/macros.h"

#include "@(spec.base_type.pkg_name)/@(subfolder)/@(get_header_filename_from_msg_name(spec.base_type.type))__struct.hpp"
#include "rosidl_typesupport_introspection_cpp/field_types.hpp"
#include "rosidl_typesupport_introspection_cpp/identifier.hpp"
#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
#include "rosidl_typesupport_introspection_cpp/message_type_support_decl.hpp"
#include "rosidl_typesupport_introspection_cpp/visibility_control.h"

namespace @(spec.base_type.pkg_name)
{

namespace @(subfolder)
{

namespace rosidl_typesupport_introspection_cpp
{

@[if spec.fields]@
@[  for field in spec.fields]@
@[    if not field.type.is_primitive_type() and field.type.is_array]@
size_t size_function__@(spec.base_type.type)__@(field.name)(const void * untyped_member)
{
@[      if field.type.array_size and not field.type.is_upper_bound]@
  (void)untyped_member;
  return @(field.type.array_size);
@[      else]@
  const auto * member =
    reinterpret_cast<const std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
  return member->size();
@[      end if]@
}

const void * get_const_function__@(spec.base_type.type)__@(field.name)(const void * untyped_member, size_t index)
{
@[      if field.type.array_size and not field.type.is_upper_bound]@
  const auto & member =
    *reinterpret_cast<const std::array<@(field.type.pkg_name)::msg::@(field.type.type), @(field.type.array_size)> *>(untyped_member);
@[      else]@
  const auto & member =
    *reinterpret_cast<const std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
@[      end if]@
  return &member[index];
}

void * get_function__@(spec.base_type.type)__@(field.name)(void * untyped_member, size_t index)
{
@[      if field.type.array_size and not field.type.is_upper_bound]@
  auto & member =
    *reinterpret_cast<std::array<@(field.type.pkg_name)::msg::@(field.type.type), @(field.type.array_size)> *>(untyped_member);
@[      else]@
  auto & member =
    *reinterpret_cast<std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
@[      end if]@
  return &member[index];
}

@[      if not field.type.array_size or field.type.is_upper_bound]@
void resize_function__@(spec.base_type.type)__@(field.name)(void * untyped_member, size_t size)
{
  auto * member =
    reinterpret_cast<std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
  member->resize(size);
}

@[      end if]@
@[    end if]@
@[  end for]@
static const ::rosidl_typesupport_introspection_cpp::MessageMember @(spec.base_type.type)_message_member_array[@(len(spec.fields))] = {
@{
for index, field in enumerate(spec.fields):
    print('  {')

    # const char * name_
    print('    "%s",  // name' % field.name)
    if field.type.is_primitive_type():
        # uint8_t type_id_
        print('    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_%s,  // type' % field.type.type.upper())
        # size_t string_upper_bound
        print('    %u,  // upper bound of string' % (field.type.string_upper_bound if field.type.string_upper_bound is not None else 0))
        # const rosidl_generator_cpp::MessageTypeSupportHandle * members_
        print('    nullptr,  // members of sub message')
    else:
        # uint8_t type_id_
        print('    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE,  // type')
        # size_t string_upper_bound
        print('    0,  // upper bound of string')
        # const rosidl_message_type_support_t * members_
        print('    ::rosidl_typesupport_introspection_cpp::get_message_type_support_handle<%s::msg::%s>(),  // members of sub message' % (field.type.pkg_name, field.type.type))
    # bool is_array_
    print('    %s,  // is array' % ('true' if field.type.is_array else 'false'))
    # size_t array_size_
    print('    %u,  // array size' % (field.type.array_size if field.type.array_size else 0))
    # bool is_upper_bound_
    print('    %s,  // is upper bound' % ('true' if field.type.is_upper_bound else 'false'))
    # unsigned long offset_
    print('    offsetof(%s::%s::%s, %s),  // bytes offset in struct' % (spec.base_type.pkg_name, subfolder, spec.base_type.type, field.name))
    # void * default_value_
    print('    nullptr,  // default value')  # TODO default value to be set

    function_suffix = '%s__%s' % (spec.base_type.type, field.name) if not field.type.is_primitive_type() and field.type.is_array else None

    # size_t(const void *) size_function
    print('    %s,  // size() function pointer' % ('size_function__%s' % function_suffix if function_suffix else 'nullptr'))
    # const void *(const void *, size_t) get_const_function
    print('    %s,  // get_const(index) function pointer' % ('get_const_function__%s' % function_suffix if function_suffix else 'nullptr'))
    # void *(void *, size_t) get_function
    print('    %s,  // get(index) function pointer' % ('get_function__%s' % function_suffix if function_suffix else 'nullptr'))
    # void(void *, size_t) resize_function
    print('    %s  // resize(index) function pointer' % ('resize_function__%s' % function_suffix if function_suffix and (not field.type.array_size or field.type.is_upper_bound) else 'nullptr'))

    if index < len(spec.fields) - 1:
        print('  },')
    else:
        print('  }')
}@
};

@[end if]@
static const ::rosidl_typesupport_introspection_cpp::MessageMembers @(spec.base_type.type)_message_members = {
  "@(spec.base_type.pkg_name)",  // package name
  "@(spec.base_type.type)",  // message name
  @(len(spec.fields)),  // number of fields
  sizeof(@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type)),
@[if spec.fields]@
  @(spec.base_type.type)_message_member_array  // message members
@[else]@
  0  // message members
@[end if]@
};

static const rosidl_message_type_support_t @(spec.base_type.type)_message_type_support_handle = {
  ::rosidl_typesupport_introspection_cpp::typesupport_identifier,
  &@(spec.base_type.type)_message_members,
  get_message_typesupport_handle_function,
};

}  // namespace rosidl_typesupport_introspection_cpp

}  // namespace @(subfolder)

}  // namespace @(spec.base_type.pkg_name)

namespace rosidl_typesupport_introspection_cpp
{

template<>
ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC
const rosidl_message_type_support_t *
get_message_type_support_handle<@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type)>()
{
  return &::@(spec.base_type.pkg_name)::@(subfolder)::rosidl_typesupport_introspection_cpp::@(spec.base_type.type)_message_type_support_handle;
}

}  // namespace rosidl_typesupport_introspection_cpp

#ifdef __cplusplus
extern "C"
{
#endif

ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC
const rosidl_message_type_support_t *
ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_introspection_cpp, @(spec.base_type.pkg_name), @(subfolder), @(spec.base_type.type))() {
  return &::@(spec.base_type.pkg_name)::@(subfolder)::rosidl_typesupport_introspection_cpp::@(spec.base_type.type)_message_type_support_handle;
}

#ifdef __cplusplus
}
#endif

// generated from rosidl_typesupport_introspection_cpp/resource/msg__type_support.cpp.em
// generated code does not contain a copyright notice

@#######################################################################
@# EmPy template for generating <msg>__type_support.cpp files
@#
@# Context:
@#  - spec (rosidl_parser.MessageSpecification)
@#    Parsed specification of the .msg file
@#  - subfolder (string)
@#    The subfolder / subnamespace of the message
@#    Either 'msg', 'srv' or 'action'
@#  - get_header_filename_from_msg_name (function)
@#######################################################################
@
// providing offsetof()
#include <cstddef>
#include <vector>

#include "rosidl_generator_c/message_type_support_struct.h"
#include "rosidl_typesupport_cpp/message_type_support.hpp"
#include "rosidl_typesupport_interface/macros.h"

#include "@(spec.base_type.pkg_name)/@(subfolder)/@(get_header_filename_from_msg_name(spec.base_type.type))__struct.hpp"
#include "rosidl_typesupport_introspection_cpp/field_types.hpp"
#include "rosidl_typesupport_introspection_cpp/identifier.hpp"
#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
#include "rosidl_typesupport_introspection_cpp/message_type_support_decl.hpp"
#include "rosidl_typesupport_introspection_cpp/visibility_control.h"

namespace @(spec.base_type.pkg_name)
{

namespace @(subfolder)
{

namespace rosidl_typesupport_introspection_cpp
{

@[if spec.fields]@
@[  for field in spec.fields]@
@[    if not field.type.is_primitive_type() and field.type.is_array]@
size_t size_function__@(spec.base_type.type)__@(field.name)(const void * untyped_member)
{
@[      if field.type.array_size and not field.type.is_upper_bound]@
  (void)untyped_member;
  return @(field.type.array_size);
@[      else]@
  const std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> * member =
    reinterpret_cast<const std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
  return member->size();
@[      end if]@
}

const void * get_const_function__@(spec.base_type.type)__@(field.name)(const void * untyped_member, size_t index)
{
@[      if field.type.array_size and not field.type.is_upper_bound]@
  const @(field.type.pkg_name)::msg::@(field.type.type) * member =
    reinterpret_cast<const @(field.type.pkg_name)::msg::@(field.type.type) *>(untyped_member);
@[      else]@
  const std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> & member =
    *reinterpret_cast<const std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
@[      end if]@
  return &member[index];
}

void * get_function__@(spec.base_type.type)__@(field.name)(void * untyped_member, size_t index)
{
@[      if field.type.array_size and not field.type.is_upper_bound]@
  @(field.type.pkg_name)::msg::@(field.type.type) * member =
    reinterpret_cast<@(field.type.pkg_name)::msg::@(field.type.type) *>(untyped_member);
@[      else]@
  std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> & member =
    *reinterpret_cast<std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
@[      end if]@
  return &member[index];
}

@[      if not field.type.array_size or field.type.is_upper_bound]@
void resize_function__@(spec.base_type.type)__@(field.name)(void * untyped_member, size_t size)
{
  std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> * member =
    reinterpret_cast<std::vector<@(field.type.pkg_name)::msg::@(field.type.type)> *>(untyped_member);
  member->resize(size);
}

@[      end if]@
@[    end if]@
@[  end for]@
static const ::rosidl_typesupport_introspection_cpp::MessageMember @(spec.base_type.type)_message_member_array[@(len(spec.fields))] = {
@{
for index, field in enumerate(spec.fields):
    print('  {')

    # const char * name_
    print('    "%s",  // name' % field.name)
    if field.type.is_primitive_type():
        # uint8_t type_id_
        print('    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_%s,  // type' % field.type.type.upper())
        # size_t string_upper_bound
        print('    %u,  // upper bound of string' % (field.type.string_upper_bound if field.type.string_upper_bound is not None else 0))
        # const rosidl_generator_cpp::MessageTypeSupportHandle * members_
        print('    nullptr,  // members of sub message')
    else:
        # uint8_t type_id_
        print('    ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE,  // type')
        # size_t string_upper_bound
        print('    0,  // upper bound of string')
        # const rosidl_message_type_support_t * members_
        print('    ::rosidl_typesupport_introspection_cpp::get_message_type_support_handle<%s::msg::%s>(),  // members of sub message' % (field.type.pkg_name, field.type.type))
    # bool is_array_
    print('    %s,  // is array' % ('true' if field.type.is_array else 'false'))
    # size_t array_size_
    print('    %u,  // array size' % (field.type.array_size if field.type.array_size else 0))
    # bool is_upper_bound_
    print('    %s,  // is upper bound' % ('true' if field.type.is_upper_bound else 'false'))
    # unsigned long offset_
    print('    offsetof(%s::%s::%s, %s),  // bytes offset in struct' % (spec.base_type.pkg_name, subfolder, spec.base_type.type, field.name))
    # void * default_value_
    print('    nullptr,  // default value')  # TODO default value to be set

    function_suffix = '%s__%s' % (spec.base_type.type, field.name) if not field.type.is_primitive_type() and field.type.is_array else None

    # size_t(const void *) size_function
    print('    %s,  // size() function pointer' % ('size_function__%s' % function_suffix if function_suffix else 'nullptr'))
    # const void *(const void *, size_t) get_const_function
    print('    %s,  // get_const(index) function pointer' % ('get_const_function__%s' % function_suffix if function_suffix else 'nullptr'))
    # void *(void *, size_t) get_function
    print('    %s,  // get(index) function pointer' % ('get_function__%s' % function_suffix if function_suffix else 'nullptr'))
    # void(void *, size_t) resize_function
    print('    %s  // resize(index) function pointer' % ('resize_function__%s' % function_suffix if function_suffix and (not field.type.array_size or field.type.is_upper_bound) else 'nullptr'))

    if index < len(spec.fields) - 1:
        print('  },')
    else:
        print('  }')
}@
};

@[end if]@
static const ::rosidl_typesupport_introspection_cpp::MessageMembers @(spec.base_type.type)_message_members = {
  "@(spec.base_type.pkg_name)",  // package name
  "@(spec.base_type.type)",  // message name
  @(len(spec.fields)),  // number of fields
  sizeof(@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type)),
@[if spec.fields]@
  @(spec.base_type.type)_message_member_array  // message members
@[else]@
  0  // message members
@[end if]@
};

static const rosidl_message_type_support_t @(spec.base_type.type)_message_type_support_handle = {
  ::rosidl_typesupport_introspection_cpp::typesupport_identifier,
  &@(spec.base_type.type)_message_members,
  get_message_typesupport_handle_function,
};

}  // namespace rosidl_typesupport_introspection_cpp

}  // namespace @(subfolder)

}  // namespace @(spec.base_type.pkg_name)

namespace rosidl_typesupport_introspection_cpp
{

template<>
ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC
const rosidl_message_type_support_t *
get_message_type_support_handle<@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type)>()
{
  return &::@(spec.base_type.pkg_name)::@(subfolder)::rosidl_typesupport_introspection_cpp::@(spec.base_type.type)_message_type_support_handle;
}

}  // namespace rosidl_typesupport_introspection_cpp

#ifdef __cplusplus
extern "C"
{
#endif

ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC
const rosidl_message_type_support_t *
ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME(rosidl_typesupport_introspection_cpp, @(spec.base_type.pkg_name), @(subfolder), @(spec.base_type.type))() {
  return &::@(spec.base_type.pkg_name)::@(subfolder)::rosidl_typesupport_introspection_cpp::@(spec.base_type.type)_message_type_support_handle;
}

#ifdef __cplusplus
}
#endif


cleanups

cleanups

#!/bin/bash

# DO NOT REMOVE THIS BLOCK UNLESS YOU DON'T WANT TO USE CUSTOM SCRIPTS
@[if DEVELSPACE]@
export ROSWSS_SCRIPTS="@(PROJECT_SOURCE_DIR)/scripts:$ROSWSS_SCRIPTS"
@[else]@
export ROSWSS_SCRIPTS="@(CMAKE_INSTALL_PREFIX)/@(CATKIN_PACKAGE_SHARE_DESTINATION)/scripts:$ROSWSS_SCRIPTS"
@[end if]@

# SET HERE YOUR WORKSPACE PREFIX
export ROSWSS_PREFIX="thor"
export ROSWSS_ROOT_RELATIVE_PATH="../.."
export ROSWSS_INSTALL_DIR="rosinstall"

# SETUP YOUR ENVIRONMENT HERE (all fields are optional)
export ONBOARD_LAUNCH_PKG="thor_mang_onboard_launch"
export UI_LAUNCH_PKG="thor_mang_ui_launch"
export GAZEBO_LAUNCH_PKG="thor_mang_gazebo"
export GAZEBO_DEFAULT_LAUNCH_FILE="johnny5.launch"
export GAZEBO_WORLDS_PKG="thor_mang_gazebo"
export AUTOSTART_LAUNCH_PKG="thor_mang_robot_bringup"
export ROBOT_MASTER_HOSTNANE="motion"
export ROBOT_HOSTNAMES="motion perception"
export ROBOT_USER="thor"

# THOR PCs/LAUNCHES
# Syntax:
#   add_remote_pc "<script_name>" "<host_name>" "<screen_name>" "<command>"
add_remote_pc "motion" "motion" "motion_start" "~/thor/src/thor/thor_mang_robot_bringup/scripts/motion.sh"
add_remote_pc "perception" "perception_start" "perception" "~/thor/src/thor/thor_mang_robot_bringup/scripts/perception.sh"
add_remote_pc "onboard" "motion" "onboard_start" "~/thor/src/thor/thor_mang_robot_bringup/scripts/onboard.sh"
add_remote_pc "robot_basics" "motion" "robot_basics_start" "~/thor/src/thor/thor_mang_robot_bringup/scripts/robot_basics.sh"

#!/bin/bash

# DO NOT REMOVE THIS BLOCK UNLESS YOU DON'T WANT TO USE CUSTOM SCRIPTS
@[if DEVELSPACE]@
export ROSWSS_SCRIPTS="@(PROJECT_SOURCE_DIR)/scripts:$ROSWSS_SCRIPTS"
@[else]@
export ROSWSS_SCRIPTS="@(CMAKE_INSTALL_PREFIX)/@(CATKIN_PACKAGE_SHARE_DESTINATION)/scripts:$ROSWSS_SCRIPTS"
@[end if]@

# SET HERE YOUR WORKSPACE PREFIX
export ROSWSS_PREFIX="thor"
export ROSWSS_ROOT_RELATIVE_PATH="../.."
export ROSWSS_INSTALL_DIR="rosinstall"

# SETUP YOUR ENVIRONMENT HERE
export ONBOARD_LAUNCH_PKG="thor_mang_onboard_launch"
export UI_LAUNCH_PKG="thor_mang_ui_launch"
export GAZEBO_LAUNCH_PKG="thor_mang_gazebo"
export GAZEBO_DEFAULT_LAUNCH_FILE="johnny5.launch"
export GAZEBO_WORLDS_PKG="thor_mang_gazebo"
export AUTOSTART_LAUNCH_PKG="thor_mang_robot_bringup"
export ROBOT_MASTER_HOSTNANE="motion"
export ROBOT_HOSTNAMES="motion perception"
export ROBOT_USER="thor"

# THOR PCs/LAUNCHES
# Syntax:
#   add_remote_pc <script_name> <host_name> <screen_name> <command>
add_remote_pc "motion" "motion" "motion_start" "~/thor/src/thor/thor_mang_robot_bringup/scripts/motion.sh"
add_remote_pc "perception" "perception_start" "perception" "~/thor/src/thor/thor_mang_robot_bringup/scripts/perception.sh"
add_remote_pc "onboard" "motion" "onboard_start" "~/thor/src/thor/thor_mang_robot_bringup/scripts/onboard.sh"
add_remote_pc "robot_basics" "motion" "robot_basics_start" "~/thor/src/thor/thor_mang_robot_bringup/scripts/robot_basics.sh"