cPP neuralnet model and custom yaml library from Junhyeok's code impo…

…rted.
stevenjj · Aug 30, 2019 · 0c123d1 · 0c123d1
1 parent 07a1a7a
commit 0c123d1
Show file tree

Hide file tree

Showing 100 changed files with 11,732 additions and 3 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -85,6 +85,8 @@ SET (HELPER_SOURCES
 	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/yaml_data_saver.cpp
 	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/gmm_fit.cpp
 	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/orientation_utils.cpp
+	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/IOUtilities.cpp
+	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/NeuralNetModel.cpp
 )
 
 SET (ROS_BRIDGE_SOURCES
@@ -147,12 +149,18 @@ SET(PROJECT_SOURCES
 	${COLLISION_ENVIRONMENT_SOURCES}
 	${IK_MODULE_SOURCES}
 	${CUBIC_INTERPOLATION_MODULE_SOURCES}
-	${PLANNER_SOURCES})
+	${PLANNER_SOURCES}
+	)
+
+# Compile the custom yaml parser
+add_subdirectory(customYamlParser)
 
 # Grab all the libraries
 SET(PROJECT_LIBRARIES
 	${catkin_LIBRARIES}
-	${YAML_CPP_LIBRARIES})
+	${YAML_CPP_LIBRARIES}
+	myYaml)
+
 
 add_subdirectory(test)
 add_subdirectory(nodes)
diff --git a/customYamlParser/CMakeLists.txt b/customYamlParser/CMakeLists.txt
@@ -0,0 +1,11 @@
+file(GLOB_RECURSE my_yaml_sources "*.cpp")
+#file(GLOB yaml_headers "include/myYaml/*.h")
+#file(GLOB yaml_contrib_headers "include/myYaml/contrib/*.h")
+#file(GLOB yaml_node_headers "include/myYaml/node/*.h")
+#file(GLOB yaml_node_detail_headers "include/myYaml/node/detail/*.h")
+
+add_library(myYaml SHARED ${my_yaml_sources})
+#                          ${yaml_headers}
+#                          ${yaml_contrib_headers}
+#                          ${yaml_node_headers}
+#                          ${yaml_node_detail_headers})
diff --git a/customYamlParser/src/binary.cpp b/customYamlParser/src/binary.cpp
@@ -0,0 +1,93 @@
+#include "myYaml/include/myYaml/binary.h"
+
+namespace myYAML {
+static const char encoding[] =
+    "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
+std::string EncodeBase64(const unsigned char *data, std::size_t size) {
+  const char PAD = '=';
+
+  std::string ret;
+  ret.resize(4 * size / 3 + 3);
+  char *out = &ret[0];
+
+  std::size_t chunks = size / 3;
+  std::size_t remainder = size % 3;
+
+  for (std::size_t i = 0; i < chunks; i++, data += 3) {
+    *out++ = encoding[data[0] >> 2];
+    *out++ = encoding[((data[0] & 0x3) << 4) | (data[1] >> 4)];
+    *out++ = encoding[((data[1] & 0xf) << 2) | (data[2] >> 6)];
+    *out++ = encoding[data[2] & 0x3f];
+  }
+
+  switch (remainder) {
+    case 0:
+      break;
+    case 1:
+      *out++ = encoding[data[0] >> 2];
+      *out++ = encoding[((data[0] & 0x3) << 4)];
+      *out++ = PAD;
+      *out++ = PAD;
+      break;
+    case 2:
+      *out++ = encoding[data[0] >> 2];
+      *out++ = encoding[((data[0] & 0x3) << 4) | (data[1] >> 4)];
+      *out++ = encoding[((data[1] & 0xf) << 2)];
+      *out++ = PAD;
+      break;
+  }
+
+  ret.resize(out - &ret[0]);
+  return ret;
+}
+
+static const unsigned char decoding[] = {
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 62,  255,
+    255, 255, 63,  52,  53,  54,  55,  56,  57,  58,  59,  60,  61,  255, 255,
+    255, 0,   255, 255, 255, 0,   1,   2,   3,   4,   5,   6,   7,   8,   9,
+    10,  11,  12,  13,  14,  15,  16,  17,  18,  19,  20,  21,  22,  23,  24,
+    25,  255, 255, 255, 255, 255, 255, 26,  27,  28,  29,  30,  31,  32,  33,
+    34,  35,  36,  37,  38,  39,  40,  41,  42,  43,  44,  45,  46,  47,  48,
+    49,  50,  51,  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+    255,
+};
+
+std::vector<unsigned char> DecodeBase64(const std::string &input) {
+  typedef std::vector<unsigned char> ret_type;
+  if (input.empty())
+    return ret_type();
+
+  ret_type ret(3 * input.size() / 4 + 1);
+  unsigned char *out = &ret[0];
+
+  unsigned value = 0;
+  for (std::size_t i = 0; i < input.size(); i++) {
+    unsigned char d = decoding[static_cast<unsigned>(input[i])];
+    if (d == 255)
+      return ret_type();
+
+    value = (value << 6) | d;
+    if (i % 4 == 3) {
+      *out++ = value >> 16;
+      if (i > 0 && input[i - 1] != '=')
+        *out++ = value >> 8;
+      if (input[i] != '=')
+        *out++ = value;
+    }
+  }
+
+  ret.resize(out - &ret[0]);
+  return ret;
+}
+}
diff --git a/customYamlParser/src/collectionstack.h b/customYamlParser/src/collectionstack.h
@@ -0,0 +1,39 @@
+#ifndef COLLECTIONSTACK_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define COLLECTIONSTACK_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <stack>
+#include <cassert>
+
+namespace myYAML {
+struct CollectionType {
+  enum value { NoCollection, BlockMap, BlockSeq, FlowMap, FlowSeq, CompactMap };
+};
+
+class CollectionStack {
+ public:
+  CollectionType::value GetCurCollectionType() const {
+    if (collectionStack.empty())
+      return CollectionType::NoCollection;
+    return collectionStack.top();
+  }
+
+  void PushCollectionType(CollectionType::value type) {
+    collectionStack.push(type);
+  }
+  void PopCollectionType(CollectionType::value type) {
+    assert(type == GetCurCollectionType());
+    collectionStack.pop();
+  }
+
+ private:
+  std::stack<CollectionType::value> collectionStack;
+};
+}
+
+#endif  // COLLECTIONSTACK_H_62B23520_7C8E_11DE_8A39_0800200C9A66
diff --git a/customYamlParser/src/contrib/graphbuilder.cpp b/customYamlParser/src/contrib/graphbuilder.cpp
@@ -0,0 +1,17 @@
+#include "graphbuilderadapter.h"
+
+#include "myYaml/include/myYaml/parser.h"  // IWYU pragma: keep
+
+namespace myYAML {
+class GraphBuilderInterface;
+
+void* BuildGraphOfNextDocument(Parser& parser,
+                               GraphBuilderInterface& graphBuilder) {
+  GraphBuilderAdapter eventHandler(graphBuilder);
+  if (parser.HandleNextDocument(eventHandler)) {
+    return eventHandler.RootNode();
+  } else {
+    return NULL;
+  }
+}
+}
diff --git a/customYamlParser/src/contrib/graphbuilderadapter.cpp b/customYamlParser/src/contrib/graphbuilderadapter.cpp
@@ -0,0 +1,94 @@
+#include "graphbuilderadapter.h"
+#include "myYaml/include/myYaml/contrib/graphbuilder.h"
+
+namespace myYAML {
+struct Mark;
+
+int GraphBuilderAdapter::ContainerFrame::sequenceMarker;
+
+void GraphBuilderAdapter::OnNull(const Mark &mark, anchor_t anchor) {
+  void *pParent = GetCurrentParent();
+  void *pNode = m_builder.NewNull(mark, pParent);
+  RegisterAnchor(anchor, pNode);
+
+  DispositionNode(pNode);
+}
+
+void GraphBuilderAdapter::OnAlias(const Mark &mark, anchor_t anchor) {
+  void *pReffedNode = m_anchors.Get(anchor);
+  DispositionNode(m_builder.AnchorReference(mark, pReffedNode));
+}
+
+void GraphBuilderAdapter::OnScalar(const Mark &mark, const std::string &tag,
+                                   anchor_t anchor, const std::string &value) {
+  void *pParent = GetCurrentParent();
+  void *pNode = m_builder.NewScalar(mark, tag, pParent, value);
+  RegisterAnchor(anchor, pNode);
+
+  DispositionNode(pNode);
+}
+
+void GraphBuilderAdapter::OnSequenceStart(const Mark &mark,
+                                          const std::string &tag,
+                                          anchor_t anchor,
+                                          EmitterStyle::value /* style */) {
+  void *pNode = m_builder.NewSequence(mark, tag, GetCurrentParent());
+  m_containers.push(ContainerFrame(pNode));
+  RegisterAnchor(anchor, pNode);
+}
+
+void GraphBuilderAdapter::OnSequenceEnd() {
+  void *pSequence = m_containers.top().pContainer;
+  m_containers.pop();
+
+  DispositionNode(pSequence);
+}
+
+void GraphBuilderAdapter::OnMapStart(const Mark &mark, const std::string &tag,
+                                     anchor_t anchor,
+                                     EmitterStyle::value /* style */) {
+  void *pNode = m_builder.NewMap(mark, tag, GetCurrentParent());
+  m_containers.push(ContainerFrame(pNode, m_pKeyNode));
+  m_pKeyNode = NULL;
+  RegisterAnchor(anchor, pNode);
+}
+
+void GraphBuilderAdapter::OnMapEnd() {
+  void *pMap = m_containers.top().pContainer;
+  m_pKeyNode = m_containers.top().pPrevKeyNode;
+  m_containers.pop();
+  DispositionNode(pMap);
+}
+
+void *GraphBuilderAdapter::GetCurrentParent() const {
+  if (m_containers.empty()) {
+    return NULL;
+  }
+  return m_containers.top().pContainer;
+}
+
+void GraphBuilderAdapter::RegisterAnchor(anchor_t anchor, void *pNode) {
+  if (anchor) {
+    m_anchors.Register(anchor, pNode);
+  }
+}
+
+void GraphBuilderAdapter::DispositionNode(void *pNode) {
+  if (m_containers.empty()) {
+    m_pRootNode = pNode;
+    return;
+  }
+
+  void *pContainer = m_containers.top().pContainer;
+  if (m_containers.top().isMap()) {
+    if (m_pKeyNode) {
+      m_builder.AssignInMap(pContainer, m_pKeyNode, pNode);
+      m_pKeyNode = NULL;
+    } else {
+      m_pKeyNode = pNode;
+    }
+  } else {
+    m_builder.AppendToSequence(pContainer, pNode);
+  }
+}
+}
diff --git a/customYamlParser/src/contrib/graphbuilderadapter.h b/customYamlParser/src/contrib/graphbuilderadapter.h
@@ -0,0 +1,79 @@
+#ifndef GRAPHBUILDERADAPTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+#define GRAPHBUILDERADAPTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66
+
+#if defined(_MSC_VER) ||                                            \
+    (defined(__GNUC__) && (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || \
+     (__GNUC__ >= 4))  // GCC supports "pragma once" correctly since 3.4
+#pragma once
+#endif
+
+#include <cstdlib>
+#include <map>
+#include <stack>
+
+#include "myYaml/include/myYaml/anchor.h"
+#include "myYaml/include/myYaml/contrib/anchordict.h"
+#include "myYaml/include/myYaml/contrib/graphbuilder.h"
+#include "myYaml/include/myYaml/emitterstyle.h"
+#include "myYaml/include/myYaml/eventhandler.h"
+
+namespace myYAML {
+class GraphBuilderInterface;
+struct Mark;
+}  // namespace myYAML
+
+namespace myYAML {
+class GraphBuilderAdapter : public EventHandler {
+ public:
+  GraphBuilderAdapter(GraphBuilderInterface& builder)
+      : m_builder(builder), m_pRootNode(NULL), m_pKeyNode(NULL) {}
+
+  virtual void OnDocumentStart(const Mark& mark) { (void)mark; }
+  virtual void OnDocumentEnd() {}
+
+  virtual void OnNull(const Mark& mark, anchor_t anchor);
+  virtual void OnAlias(const Mark& mark, anchor_t anchor);
+  virtual void OnScalar(const Mark& mark, const std::string& tag,
+                        anchor_t anchor, const std::string& value);
+
+  virtual void OnSequenceStart(const Mark& mark, const std::string& tag,
+                               anchor_t anchor, EmitterStyle::value style);
+  virtual void OnSequenceEnd();
+
+  virtual void OnMapStart(const Mark& mark, const std::string& tag,
+                          anchor_t anchor, EmitterStyle::value style);
+  virtual void OnMapEnd();
+
+  void* RootNode() const { return m_pRootNode; }
+
+ private:
+  struct ContainerFrame {
+    ContainerFrame(void* pSequence)
+        : pContainer(pSequence), pPrevKeyNode(&sequenceMarker) {}
+    ContainerFrame(void* pMap, void* pPrevKeyNode)
+        : pContainer(pMap), pPrevKeyNode(pPrevKeyNode) {}
+
+    void* pContainer;
+    void* pPrevKeyNode;
+
+    bool isMap() const { return pPrevKeyNode != &sequenceMarker; }
+
+   private:
+    static int sequenceMarker;
+  };
+  typedef std::stack<ContainerFrame> ContainerStack;
+  typedef AnchorDict<void*> AnchorMap;
+
+  GraphBuilderInterface& m_builder;
+  ContainerStack m_containers;
+  AnchorMap m_anchors;
+  void* m_pRootNode;
+  void* m_pKeyNode;
+
+  void* GetCurrentParent() const;
+  void RegisterAnchor(anchor_t anchor, void* pNode);
+  void DispositionNode(void* pNode);
+};
+}
+
+#endif  // GRAPHBUILDERADAPTER_H_62B23520_7C8E_11DE_8A39_0800200C9A66