fixup

trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/node.h

@@ -3,7 +3,6 @@
 
 #include <trajopt_ifopt/trajopt/variable_sets/var.h>
 #include <unordered_map>
-#include <boost/uuid/uuid.hpp>
 
 namespace trajopt_ifopt
 {
@@ -12,33 +11,35 @@ class Node
 {
 public:
   Node(std::string node_name = "Node");
-
-  boost::uuids::uuid getUUID() const;
+  Node(const Node&) = delete;
+  Node& operator=(const Node&) = delete;
+  Node(Node&&) = default;
+  Node& operator=(Node&&) = default;
 
   const std::string& getName() const;
 
-  NodesVariables* const getParent() const;
+  NodesVariables* getParent() const;
 
-  Var addVar(const std::string& name);
+  std::shared_ptr<const Var> addVar(const std::string& name);
 
-  Var addVar(const std::string& name, const std::vector<std::string>& child_names);
+  std::shared_ptr<const Var> addVar(const std::string& name, const std::vector<std::string>& child_names);
 
-  Var getVar(const std::string& name) const;
+  std::shared_ptr<const Var> getVar(const std::string& name) const;
 
   bool hasVar(const std::string& name) const;
 
   Eigen::Index size() const;
 
+  void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
+
+  void incrementIndex(Eigen::Index value);
+
 protected:
   friend class NodesVariables;
-  boost::uuids::uuid uuid_{};
   std::string name_;
-  std::unordered_map<std::string, Var> vars_;
+  std::unordered_map<std::string, std::shared_ptr<Var>> vars_;
   Eigen::Index length_{ 0 };
   NodesVariables* parent_{ nullptr };
-
-  void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
-  void incrementIndex(Eigen::Index value);
 };
 
 }  // namespace trajopt_ifopt

trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/nodes_variables.h

@@ -52,14 +52,14 @@ class NodesVariables : public ifopt::VariableSet
    * @brief Add node to the variable set
    * @param node The node to append
    */
-  void AddNode(Node node);
+  void AddNode(std::unique_ptr<Node> node);
 
   /**
    * @brief Get node based on index
    * @param opt_idx The node index
    * @return The node
    */
-  const Node& GetNode(std::size_t opt_idx) const;
+  const std::shared_ptr<const Node>& GetNode(std::size_t opt_idx) const;
 
   /**
    * @brief Pure optimization variables that define the nodes.
@@ -91,7 +91,7 @@ class NodesVariables : public ifopt::VariableSet
   /**
    * @returns All the nodes that can be used to reconstruct the spline.
    */
-  std::vector<Node> GetNodes() const;
+  std::vector<std::shared_ptr<const Node>> GetNodes() const;
 
   /**
    * @brief Adds a dependent observer that gets notified when the nodes change.
@@ -114,7 +114,7 @@ class NodesVariables : public ifopt::VariableSet
 
   Eigen::VectorXd values_;
   VecBound bounds_;  ///< the bounds on the node values.
-  std::vector<Node> nodes_;
+  std::vector<std::shared_ptr<Node>> nodes_;
   Eigen::Index n_dim_{ 0 };
   std::vector<std::shared_ptr<NodesObserver>> observers_;
 

trajopt_ifopt/trajopt/include/trajopt_ifopt/trajopt/variable_sets/var.h

@@ -77,18 +77,6 @@ namespace trajopt_ifopt
 //   return var_rep_->child_names;
 // }
 
-struct VarRep
-{
-  VarRep(Eigen::Index _index, std::string _name);
-  VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector<std::string> _names);
-
-  Eigen::Index index{ -1 };
-  Eigen::Index length{ -1 };
-  std::string identifier;
-  std::vector<std::string> names;
-  Eigen::VectorXd values;
-};
-
 /**
  * @brief This is the class which the constraints should be storing
  * @details This class contains all informtion necessary for filling the jacobian
@@ -98,69 +86,74 @@ class Var
 public:
   Var() = default;
   ~Var() = default;
-  Var(std::unique_ptr<VarRep> var_rep);
+  Var(Eigen::Index index, std::string name);
+  Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector<std::string> names);
   Var(const Var& other) = default;
   Var& operator=(const Var&) = default;
   Var(Var&&) = default;
   Var& operator=(Var&&) = default;
 
-  Eigen::Index getIndex() const { return var_rep_->index; }
-  Eigen::Index getLength() const { return var_rep_->length; }
+  Eigen::Index getIndex() const { return index_; }
+  Eigen::Index size() const { return length_; }
 
-  template <typename T>
-  T value() const
+  void incrementIndex(Eigen::Index value) { index_ += value; }
+  void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x)
   {
-    throw std::runtime_error("This should never be used");
+    assert(index_ > -1 && index_ < x.size());
+    assert(length_ > 0 && (index_ + length_) < x.size());
+    values_ = x.segment(index_, length_);
   }
 
   template <typename T>
-  T name() const
+  const T& value() const
   {
     throw std::runtime_error("This should never be used");
   }
 
-  void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x)
+  template <typename T>
+  const T& name() const
   {
-    assert(var_rep_->index > -1 && var_rep_->index < x.size());
-    assert(var_rep_->length > 0 && (var_rep_->index + var_rep_->length) < x.size());
-    var_rep_->values = x.segment(var_rep_->index, var_rep_->length);
+    throw std::runtime_error("This should never be used");
   }
 
 private:
-  friend class Node;
-  std::shared_ptr<VarRep> var_rep_{ nullptr };
+  Eigen::Index index_{ -1 };
+  Eigen::Index length_{ -1 };
+  std::string identifier_;
+  std::vector<std::string> names_;
+  Eigen::VectorXd values_;
 };
 
 template <>
-inline double Var::value() const
+inline const double& Var::value() const
 {
-  assert(var_rep_->values.size() == 1);
-  assert(var_rep_->index > -1);
-  assert(var_rep_->length == 1);
-  return var_rep_->values[0];
+  assert(values_.size() == 1);
+  assert(index_ > -1);
+  assert(length_ == 1);
+  return values_[0];
 }
 
 template <>
-inline Eigen::VectorXd Var::value() const
+inline const Eigen::VectorXd& Var::value() const
 {
-  assert(var_rep_->index > -1);
-  assert(var_rep_->length > 1);
-  assert(var_rep_->names.size() > 1);
-  return var_rep_->values;
+  assert(index_ > -1);
+  assert(length_ > 1);
+  assert(names_.size() > 1);
+  return values_;
 }
 
 template <>
-inline std::string Var::name() const
+inline const std::string& Var::name() const
 {
-  assert(var_rep_->names.size() == 1);
-  return var_rep_->names[0];
+  assert(names_.size() == 1);
+  return names_[0];
 }
 
 template <>
-inline std::vector<std::string> Var::name() const
+inline const std::vector<std::string>& Var::name() const
 {
-  assert(var_rep_->names.size() > 1);
-  return var_rep_->names;
+  assert(names_.size() > 1);
+  return names_;
 }
 }  // namespace trajopt_ifopt
 

trajopt_ifopt/trajopt/src/variable_sets/node.cpp

@@ -1,32 +1,31 @@
 #include <trajopt_ifopt/trajopt/variable_sets/node.h>
 #include <trajopt_ifopt/trajopt/variable_sets/nodes_variables.h>
 #include <memory>
-#include <boost/uuid/uuid_generators.hpp>
 
 namespace trajopt_ifopt
 {
-Node::Node(std::string node_name) : uuid_(boost::uuids::random_generator()()), name_(std::move(node_name)) {}
+Node::Node(std::string node_name) : name_(std::move(node_name)) {}
 
-boost::uuids::uuid Node::getUUID() const { return uuid_; }
 const std::string& Node::getName() const { return name_; }
-NodesVariables* const Node::getParent() const { return parent_; }
-Var Node::addVar(const std::string& name, const std::vector<std::string>& child_names)
+trajopt_ifopt::NodesVariables* Node::getParent() const { return parent_; }
+std::shared_ptr<const trajopt_ifopt::Var> Node::addVar(const std::string& name,
+                                                       const std::vector<std::string>& child_names)
 {
-  Var var(std::make_unique<VarRep>(length_, child_names.size(), name, child_names));
+  auto var = std::make_shared<Var>(length_, child_names.size(), name, child_names);
   vars_[name] = var;
   length_ += static_cast<Eigen::Index>(child_names.size());
   return var;
 }
 
-Var Node::addVar(const std::string& name)
+std::shared_ptr<const trajopt_ifopt::Var> Node::addVar(const std::string& name)
 {
-  Var var(std::make_unique<VarRep>(length_, name));
+  auto var = std::make_shared<Var>(length_, name);
   vars_[name] = var;
   length_ += 1;
   return var;
 }
 
-Var Node::getVar(const std::string& name) const { return vars_.at(name); }
+std::shared_ptr<const trajopt_ifopt::Var> Node::getVar(const std::string& name) const { return vars_.at(name); }
 
 bool Node::hasVar(const std::string& name) const { return (vars_.find(name) != vars_.end()); }
 
@@ -35,11 +34,11 @@ Eigen::Index Node::size() const { return length_; }
 void Node::setVariables(const Eigen::Ref<const Eigen::VectorXd>& x)
 {
   for (auto& pair : vars_)
-    pair.second.setVariables(x);
+    pair.second->setVariables(x);
 }
 void Node::incrementIndex(Eigen::Index value)
 {
   for (auto& pair : vars_)
-    pair.second.var_rep_->index += value;
+    pair.second->incrementIndex(value);
 }
 }  // namespace trajopt_ifopt

trajopt_ifopt/trajopt/src/variable_sets/nodes_variables.cpp

@@ -34,26 +34,21 @@ namespace trajopt_ifopt
 {
 NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLater, name) {}
 
-void NodesVariables::AddNode(Node node)
+void NodesVariables::AddNode(std::unique_ptr<Node> node)
 {
-  auto it =
-      std::find_if(nodes_.begin(), nodes_.begin(), [&node](const Node& n) { return n.getUUID() == node.getUUID(); });
-  if (it != nodes_.end())
-    throw std::runtime_error("NodesVariables, Node must be unique!");
-
-  node.incrementIndex(n_dim_);
-  node.parent_ = this;
-  Eigen::Index length = node.size();
+  node->incrementIndex(n_dim_);
+  node->parent_ = this;
+  Eigen::Index length = node->size();
   nodes_.emplace_back(std::move(node));
   n_dim_ += length;
 }
 
-const Node& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); }
+const std::shared_ptr<const Node>& NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); }
 
 void NodesVariables::SetVariables(const VectorXd& x)
 {
   for (auto& node : nodes_)
-    node.setVariables(x);
+    node->setVariables(x);
 
   values_ = x;
 
@@ -74,6 +69,12 @@ Eigen::Index NodesVariables::GetDim() const { return n_dim_; }
 
 NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; }
 
-std::vector<Node> NodesVariables::GetNodes() const { return nodes_; }
+std::vector<std::shared_ptr<const Node> > NodesVariables::GetNodes() const
+{
+  std::vector<std::shared_ptr<const Node> > nodes;
+  nodes.reserve(nodes_.size());
+  std::copy(nodes_.begin(), nodes_.end(), std::back_inserter(nodes));
+  return nodes;
+}
 
 }  // namespace trajopt_ifopt

trajopt_ifopt/trajopt/src/variable_sets/var.cpp

@@ -9,19 +9,18 @@ namespace trajopt_ifopt
 
 // Var::Var(std::shared_ptr<const VarRep> var_rep) : var_rep_(std::move(var_rep)) {}
 
-VarRep::VarRep(Eigen::Index _index, std::string _name)
-  : index(_index), identifier(std::move(_name)), names({ identifier }), values(Eigen::VectorXd::Zero(1))
+Var::Var(Eigen::Index index, std::string name)
+  : index_(index), identifier_(std::move(name)), names_({ identifier_ }), values_(Eigen::VectorXd::Zero(1))
 {
 }
 
-VarRep::VarRep(Eigen::Index _index, Eigen::Index _length, std::string _identifier, std::vector<std::string> _names)
-  : index(_index)
-  , length(_length)
-  , identifier(std::move(_identifier))
-  , names(std::move(_names))
-  , values(Eigen::VectorXd::Zero(static_cast<Eigen::Index>(names.size())))
+Var::Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector<std::string> names)
+  : index_(index)
+  , length_(length)
+  , identifier_(std::move(identifier))
+  , names_(std::move(names))
+  , values_(Eigen::VectorXd::Zero(static_cast<Eigen::Index>(names.size())))
 {
 }
 
-Var::Var(std::unique_ptr<VarRep> var_rep) : var_rep_(std::move(var_rep)) {}
 }  // namespace trajopt_ifopt