ros_message.cpp

Go to the documentation of this file.

/***** MIT License ****
 *
 *   Copyright (c) 2016-2022 Davide Faconti
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy
 *   of this software and associated documentation files (the "Software"), to deal
 *   in the Software without restriction, including without limitation the rights
 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *   copies of the Software, and to permit persons to whom the Software is
 *   furnished to do so, subject to the following conditions:
 *
 *   The above copyright notice and this permission notice shall be included in all
 *   copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *   SOFTWARE.
 */
 
#include <string_view>
#include <sstream>
#include <regex>
#include "rosx_introspection/ros_message.hpp"
 
namespace RosMsgParser
{
 
ROSMessage::ROSMessage(const std::string& msg_def)
{
  std::istringstream messageDescriptor(msg_def);
  std::match_results<std::string::const_iterator> what;
 
  for (std::string line; std::getline(messageDescriptor, line, '\n');)
  {
    std::string::const_iterator begin = line.begin(), end = line.end();
 
    // Skip empty line or one that is a comment
    if (std::regex_search(begin, end, what, std::regex("(^\\s*$|^\\s*#)")))
    {
      continue;
    }
 
    TrimStringLeft(line);
 
    if (line.compare(0, 5, "MSG: ") == 0)
    {
      line.erase(0, 5);
      _type = ROSType(line);
    }
    else
    {
      auto new_field = ROSField(line);
      _fields.push_back(new_field);
    }
  }
}
 
std::vector<std::string> SplitMultipleMessageDefinitions(const std::string& multi_def)
{
  std::istringstream ss_msg(multi_def);
 
  std::vector<std::string> parts;
  std::string part;
 
  for (std::string line; std::getline(ss_msg, line, '\n');)
  {
    if (line.find("========") == 0)
    {
      parts.emplace_back(std::move(part));
      part = {};
    }
    else
    {
      part.append(line);
      part.append("\n");
    }
  }
  parts.emplace_back(std::move(part));
 
  return parts;
}
 
std::vector<ROSMessage::Ptr> ParseMessageDefinitions(const std::string& multi_def,
                                                     const ROSType& root_type)
{
  auto parts = SplitMultipleMessageDefinitions(multi_def);
  std::vector<ROSType> known_type;
  std::vector<ROSMessage::Ptr> parsed_msgs;
 
  const ROSType no_type;
 
  // iterating in reverse to fill known_type in the right order
  // i.e. with no missing dependencies
  for (int i = parts.size() - 1; i >= 0; i--)
  {
    auto msg = std::make_shared<ROSMessage>(parts[i]);
 
    if (i == 0)
    {
      // type NOT found in the definition, but provided as argument
      if (msg->type() == no_type && root_type != no_type)
      {
        msg->setType(root_type);
      }
      else if (msg->type() == no_type && root_type == no_type)
      {
        std::cout << multi_def << std::endl;
        throw std::runtime_error("Message type unspecified");
      }
    }
 
    // add to vector
    parsed_msgs.push_back(msg);
    known_type.push_back(msg->type());
  }
 
  // adjust types with undefined package type
  for (auto& msg : parsed_msgs)
  {
    for (ROSField& field : msg->fields())
    {
      // if package name is missing, try to find msgName in the list of known_type
      if (field.type().pkgName().empty())
      {
        std::vector<ROSType> guessed_type;
 
        for (const ROSType& known_type : known_type)
        {
          if (field.type().msgName() == known_type.msgName())
          {
            guessed_type.push_back(known_type);
          }
        }
 
        if (!guessed_type.empty())
        {
          bool better_match = false;
 
          // attempt to match ambiguous ros msg within package before
          // using external known type
          for (const ROSType& guessed_type : guessed_type)
          {
            if (guessed_type.pkgName() == root_type.pkgName())
            {
              field.changeType(guessed_type);
              better_match = true;
              break;
            }
          }
 
          // if nothing from the same package, take a guess with the first matching msg
          // name
          if (!better_match)
          {
            field.changeType(guessed_type[0]);
          }
        }
      }
    }
  }
 
  std::reverse(parsed_msgs.begin(), parsed_msgs.end());
  return parsed_msgs;
}
 
MessageSchema::Ptr BuildMessageSchema(const std::string& topic_name,
                                      const std::vector<ROSMessage::Ptr>& parsed_msgs)
{
  auto schema = std::make_shared<MessageSchema>();
  schema->topic_name = topic_name;
  schema->root_msg = parsed_msgs.front();
 
  for (const auto& msg : parsed_msgs)
  {
    schema->msg_library.insert({ msg->type(), msg });
  }
 
  std::function<void(ROSMessage::Ptr, FieldTreeNode*)> recursiveTreeCreator;
 
  recursiveTreeCreator = [&](ROSMessage::Ptr msg, FieldTreeNode* field_node) {
    // note: should use reserve here, NOT resize
    const size_t NUM_FIELDS = msg->fields().size();
    field_node->children().reserve(NUM_FIELDS);
 
    for (const ROSField& field : msg->fields())
    {
      if (field.isConstant())
      {
        continue;
      }
      // Let's add first a child to string_node
      field_node->addChild(&field);
      FieldTreeNode* new_string_node = &(field_node->children().back());
 
      // builtin types will not trigger a recursion
      if (field.type().isBuiltin() == false)
      {
        auto new_msg = field.getMessagePtr(schema->msg_library);
        if (!new_msg)
        {
          throw std::runtime_error("Missing ROSType in library");
        }
 
        recursiveTreeCreator(new_msg, new_string_node);
      }
    }  // end of for fields
  };   // end of recursiveTreeCreator
 
  // build root and start recursion
  auto root_field = new ROSField(schema->root_msg->type(), topic_name);
  schema->field_tree.root()->setValue(root_field);
 
  recursiveTreeCreator(schema->root_msg, schema->field_tree.root());
 
  return schema;
}
 
}  // namespace RosMsgParser