Refactor: Moving common functionality to a library

subt_ign/include/subt_ign/Common.hh

@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef SUBT_IGN_COMMON_HH_
+#define SUBT_IGN_COMMON_HH_
+
+#include <subt_ign/CommonTypes.hh>
+#include <subt_ign/VisibilityTypes.hh>
+
+#include <ignition/math/Pose3.hh>
+
+namespace subt
+{
+  /// \brief Mapping between artifact model names and types.
+  const std::array<
+      const std::pair<std::string, ArtifactType>, 14> kArtifactNames
+      {
+        {
+          {"backpack",           ArtifactType::TYPE_BACKPACK},
+          {"drill",              ArtifactType::TYPE_DRILL},
+          {"duct",               ArtifactType::TYPE_DUCT},
+          {"electrical_box",     ArtifactType::TYPE_ELECTRICAL_BOX},
+          {"extinguisher",       ArtifactType::TYPE_EXTINGUISHER},
+          {"phone",              ArtifactType::TYPE_PHONE},
+          {"radio",              ArtifactType::TYPE_RADIO},
+          {"rescue_randy",       ArtifactType::TYPE_RESCUE_RANDY},
+          {"toolbox",            ArtifactType::TYPE_TOOLBOX},
+          {"valve",              ArtifactType::TYPE_VALVE},
+          {"vent",               ArtifactType::TYPE_VENT},
+          {"gas",                ArtifactType::TYPE_GAS},
+          {"helmet",             ArtifactType::TYPE_HELMET},
+          {"rope",               ArtifactType::TYPE_ROPE}
+        }
+      };
+
+  /// \brief Mapping between enum types and strings.
+  const std::array<
+      const std::pair<ArtifactType, std::string>, 14> kArtifactTypes
+      {
+        {
+          {ArtifactType::TYPE_BACKPACK      , "TYPE_BACKPACK"},
+          {ArtifactType::TYPE_DRILL         , "TYPE_DRILL"},
+          {ArtifactType::TYPE_DUCT          , "TYPE_DUCT"},
+          {ArtifactType::TYPE_ELECTRICAL_BOX, "TYPE_ELECTRICAL_BOX"},
+          {ArtifactType::TYPE_EXTINGUISHER  , "TYPE_EXTINGUISHER"},
+          {ArtifactType::TYPE_PHONE         , "TYPE_PHONE"},
+          {ArtifactType::TYPE_RADIO         , "TYPE_RADIO"},
+          {ArtifactType::TYPE_RESCUE_RANDY  , "TYPE_RESCUE_RANDY"},
+          {ArtifactType::TYPE_TOOLBOX       , "TYPE_TOOLBOX"},
+          {ArtifactType::TYPE_VALVE         , "TYPE_VALVE"},
+          {ArtifactType::TYPE_VENT          , "TYPE_VENT"},
+          {ArtifactType::TYPE_GAS           , "TYPE_GAS"},
+          {ArtifactType::TYPE_HELMET        , "TYPE_HELMET"},
+          {ArtifactType::TYPE_ROPE          , "TYPE_ROPE"}
+        }
+      };
+
+  /// \brief Create an ArtifactType from a string.
+  /// \param[in] _name The artifact in string format.
+  /// \param[out] _type The artifact type.
+  /// \return True when the conversion succeed or false otherwise.
+  bool ArtifactFromString(const std::string &_name,
+                          subt::ArtifactType &_type);
+
+  /// \brief Create an ArtifactType from a string.
+  /// \param[in] _name The artifact in string format.
+  /// \param[out] _type The artifact type.
+  /// \return True when the conversion succeed or false otherwise.
+  bool ArtifactFromPartialString(const std::string &_name,
+                                 subt::ArtifactType &_type);
+
+  /// \brief Create an ArtifactType from an integer.
+  //
+  /// \param[in] _typeInt The artifact in int format.
+  /// \param[out] _type The artifact type.
+  /// \return True when the conversion succeed or false otherwise.
+  bool ArtifactFromInt(const uint32_t &_typeInt,
+                       subt::ArtifactType &_type);
+
+  /// \brief Create a string from ArtifactType.
+  //
+  /// \param[in] _type The artifact type.
+  /// \param[out] _strType The artifact string.
+  /// \return True when the conversion succeed or false otherwise.
+  bool StringFromArtifact(const subt::ArtifactType &_type,
+                          std::string &_strType);
+
+  /// \brief Retrieve a fully-qualifed path based on name 
+  /// 
+  /// This accounts for the SubT world file directory 
+  /// structure.
+  //
+  /// \param[in] _worldName The name of the world
+  ///    For example: cave_circuit_practice_02
+  /// \param[out] _worldPath The world path without extension.
+  /// \return True when the path is expanded successfully.
+  bool FullWorldPath(const std::string &_worldName,
+                     std::string &_worldPath);
+
+  /// \brief Structure to represent an artifact as parsed
+  struct Artifact
+  {
+    /// \brief Artifact name
+    std::string name;
+
+    /// \brief Artifact type
+    subt::ArtifactType type;
+
+    /// \brief Artifact type as a string
+    std::string typeStr;
+
+    /// \brief Artifact pose
+    ignition::math::Pose3d pose;
+
+    /// \brief Return a string representation of the artifact
+    std::string String() const
+    {
+      std::stringstream ss;
+      ss << "<Artifact:"
+         << " name=" << this->name
+         << " type=" << this->typeStr
+         << " pose=(" << this->pose << ")"
+         << ">";
+      return ss.str();
+    }
+  };
+}
+#endif

subt_ign/include/subt_ign/CommonTypes.hh

@@ -79,7 +79,8 @@ namespace subt
     TYPE_VENT,
     TYPE_GAS,
     TYPE_HELMET,
-    TYPE_ROPE
+    TYPE_ROPE,
+    Count
   };
 
   /// \def TeamMemberPtr

subt_ign/src/ArtifactValidator.cc

@@ -63,7 +63,7 @@ ign service -s /artifact/move_to \
 #include <sdf/Model.hh>
 #include <sdf/World.hh>
 
-#include "subt_ign/CommonTypes.hh"
+#include "subt_ign/Common.hh"
 
 IGNITION_ADD_PLUGIN(
   subt::ArtifactValidator,
@@ -77,68 +77,9 @@ using namespace gazebo;
 using namespace systems;
 using namespace subt;
 
-/// \brief Simple structure to represent an artifact
-struct Artifact
-{
-  /// \brief Artifact name
-  std::string name;
-
-  /// \brief Artifact type
-  subt::ArtifactType type;
-
-  /// \brief Artifact type as a string
-  std::string typeStr;
-
-  /// \brief Artifact pose
-  ignition::math::Pose3d pose;
-
-  /// \brief Return a string representation of the artifact
-  std::string String() const
-  {
-    std::stringstream ss;
-    ss << "<Artifact:"
-       << " name=" << this->name
-       << " type=" << this->typeStr
-       << " pose=(" << this->pose << ")"
-       << ">";
-    return ss.str();
-  }
-};
-
 /// \brief Private data for the artifact validator.
 class subt::ArtifactValidatorPrivate
 {
-  /// \brief Map of artifact types to string representations.
-  public: const std::array<
-      const std::pair<subt::ArtifactType, std::string>, 14> kArtifactTypes
-      {
-        {
-          {subt::ArtifactType::TYPE_BACKPACK      , "TYPE_BACKPACK"},
-          {subt::ArtifactType::TYPE_DRILL         , "TYPE_DRILL"},
-          {subt::ArtifactType::TYPE_DUCT          , "TYPE_DUCT"},
-          {subt::ArtifactType::TYPE_ELECTRICAL_BOX, "TYPE_ELECTRICAL_BOX"},
-          {subt::ArtifactType::TYPE_EXTINGUISHER  , "TYPE_EXTINGUISHER"},
-          {subt::ArtifactType::TYPE_PHONE         , "TYPE_PHONE"},
-          {subt::ArtifactType::TYPE_RADIO         , "TYPE_RADIO"},
-          {subt::ArtifactType::TYPE_RESCUE_RANDY  , "TYPE_RESCUE_RANDY"},
-          {subt::ArtifactType::TYPE_TOOLBOX       , "TYPE_TOOLBOX"},
-          {subt::ArtifactType::TYPE_VALVE         , "TYPE_VALVE"},
-          {subt::ArtifactType::TYPE_VENT          , "TYPE_VENT"},
-          {subt::ArtifactType::TYPE_GAS           , "TYPE_GAS"},
-          {subt::ArtifactType::TYPE_HELMET        , "TYPE_HELMET"},
-          {subt::ArtifactType::TYPE_ROPE          , "TYPE_ROPE"}
-        }
-      };
-
-  /// \brief Get the artifact enumeration from the string value.
-  /// \param[in] _name - string representation of artifact
-  /// \param[out] _type - enumeration representation of artifact
-  public: bool ArtifactFromString(
-              const std::string &_name, ArtifactType &_type);
-
-  public: bool StringFromArtifact(const ArtifactType &_type,
-              std::string &_typeStr);
-
   /// \brief Parse artifacts from the SDF root.
   public: void ParseArtifacts();
 
@@ -252,65 +193,9 @@ bool ArtifactValidatorPrivate::OnPrev(const ignition::msgs::StringMsg& /*_req*/,
   return ret;
 }
 
-/////////////////////////////////////////////////
-bool ArtifactValidatorPrivate::ArtifactFromString(const std::string &_name,
-    ArtifactType &_type)
-{
-  auto pos = std::find_if(
-    std::begin(this->kArtifactTypes),
-    std::end(this->kArtifactTypes),
-    [&_name](const typename std::pair<ArtifactType, std::string> &_pair)
-    {
-      return (std::get<1>(_pair) == _name);
-    });
-
-  if (pos == std::end(this->kArtifactTypes))
-    return false;
-
-  _type = std::get<0>(*pos);
-  return true;
-}
-
-/////////////////////////////////////////////////
-bool ArtifactValidatorPrivate::StringFromArtifact(const ArtifactType &_type,
-    std::string &_typeStr)
-{
-  auto pos = std::find_if(
-    std::begin(this->kArtifactTypes),
-    std::end(this->kArtifactTypes),
-    [&_type](const typename std::pair<ArtifactType, std::string> &_pair)
-    {
-      return (std::get<0>(_pair) == _type);
-    });
-
-  if (pos == std::end(this->kArtifactTypes))
-    return false;
-
-  _typeStr = std::get<1>(*pos);
-  return true;
-}
-
-
 /////////////////////////////////////////////////
 void ArtifactValidatorPrivate::ParseArtifacts()
 {
-  std::map<std::string, subt::ArtifactType> sdfToType = {
-    { "backpack", subt::ArtifactType::TYPE_BACKPACK },
-    { "drill", subt::ArtifactType::TYPE_DRILL},
-    { "duct", subt::ArtifactType::TYPE_DUCT},
-    { "electrical_box", subt::ArtifactType::TYPE_ELECTRICAL_BOX},
-    { "extinguisher", subt::ArtifactType::TYPE_EXTINGUISHER},
-    { "phone", subt::ArtifactType::TYPE_PHONE},
-    { "radio", subt::ArtifactType::TYPE_RADIO},
-    { "rescue_randy", subt::ArtifactType::TYPE_RESCUE_RANDY },
-    { "toolbox", subt::ArtifactType::TYPE_TOOLBOX },
-    { "valve", subt::ArtifactType::TYPE_VALVE},
-    { "vent", subt::ArtifactType::TYPE_VENT},
-    { "gas", subt::ArtifactType::TYPE_GAS},
-    { "helmet", subt::ArtifactType::TYPE_HELMET},
-    { "rope", subt::ArtifactType::TYPE_ROPE}
-  };
-
   // Assuming 1 world per SDF, which is true for SubT.
   auto world = this->sdfRoot.WorldByIndex(0);
 
@@ -328,7 +213,7 @@ void ArtifactValidatorPrivate::ParseArtifacts()
       artifacts[name] = newArtifact;
     }
 
-    for (auto [str, type] : sdfToType)
+    for (auto [str, type] : kArtifactNames)
     {
       if (name.find(str) != std::string::npos)
       {
@@ -376,33 +261,14 @@ void ArtifactValidator::Configure(const ignition::gazebo::Entity & /*_entity*/,
   }
 
   this->dataPtr->worldName = worldName;
-  auto vals = ignition::common::Split(worldName, '_');
-  auto worldNum = vals.back();
 
-  if (worldName.find("tunnel_circuit_") != std::string::npos &&
-      worldName.find("practice") == std::string::npos)
-  {
-    worldName = "tunnel_circuit/" + worldNum + "/" + worldName +".sdf";
-  }
-  else if (worldName.find("urban_circuit_") != std::string::npos &&
-             worldName.find("practice") == std::string::npos)
-  {
-    worldName = "urban_circuit/" + worldNum + "/" + worldName + ".sdf";
-  }
-  else if (worldName.find("cave_circuit_") != std::string::npos &&
-             worldName.find("practice") == std::string::npos)
-  {
-    worldName = "cave_circuit/" + worldNum + "/" + worldName + ".sdf";
-  }
-  else
-  {
-    worldName = worldName + ".sdf";
-  }
+  std::string fullPath;
+  subt::FullWorldPath(worldName, fullPath);
 
   common::SystemPaths systemPaths;
   systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH");
   systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR);
-  std::string filePath = systemPaths.FindFile(worldName);
+  std::string filePath = systemPaths.FindFile(fullPath);
   ignmsg << "Loading SDF world file[" << filePath << "].\n";
 
   auto errors = this->dataPtr->sdfRoot.Load(filePath);