-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Verbatim copy of model directives system from project anzu
- Loading branch information
1 parent
757b1bf
commit 4d1b6dc
Showing
11 changed files
with
1,146 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,239 @@ | ||
#pragma once | ||
/// @file | ||
/// Provides directives for building scenes (*not* scenarios). | ||
/// See `common/schema/README.md` for more info. | ||
|
||
#include <optional> | ||
#include <string> | ||
#include <vector> | ||
|
||
#include <Eigen/Dense> | ||
|
||
#include "drake/common/eigen_types.h" | ||
#include "drake/common/name_value.h" | ||
#include "drake/common/text_logging.h" | ||
#include "drake/math/rigid_transform.h" | ||
#include "drake/math/roll_pitch_yaw.h" | ||
#include "common/schema/transform.h" | ||
|
||
namespace anzu { | ||
namespace common { | ||
namespace schema { | ||
|
||
struct AddWeld { | ||
bool IsValid() const { | ||
if (parent.empty()) { | ||
drake::log()->error("add_weld: `parent` must be non-empty"); | ||
return false; | ||
} else if (child.empty()) { | ||
drake::log()->error("add_weld: `child` must be non-empty"); | ||
return false; | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(parent)); | ||
a->Visit(DRAKE_NVP(child)); | ||
} | ||
|
||
// Parent frame. Can specify scope. | ||
std::string parent; | ||
// Child frame. Can (and should) specify scope. | ||
std::string child; | ||
}; | ||
|
||
struct AddModel { | ||
bool IsValid() const { | ||
if (file.empty()) { | ||
drake::log()->error("add_model: `file` must be non-empty"); | ||
return false; | ||
} else if (name.empty()) { | ||
drake::log()->error("add_model: `name` must be non-empty"); | ||
return false; | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(file)); | ||
a->Visit(DRAKE_NVP(name)); | ||
} | ||
|
||
std::string file; | ||
// Model instance name. | ||
std::string name; | ||
}; | ||
|
||
struct AddModelInstance { | ||
bool IsValid() const { | ||
if (name.empty()) { | ||
drake::log()->error("add_model_instance: `name` must be non-empty"); | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(name)); | ||
} | ||
|
||
std::string name; | ||
}; | ||
|
||
struct AddPackagePath { | ||
bool IsValid() const { | ||
if (name.empty()) { | ||
drake::log()->error("add_package_path: `name` must be non-empty."); | ||
return false; | ||
} else if (path.empty()) { | ||
drake::log()->error("add_package_path: `path` must be non-empty."); | ||
return false; | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(name)); | ||
a->Visit(DRAKE_NVP(path)); | ||
} | ||
|
||
std::string name; | ||
std::string path; | ||
}; | ||
|
||
struct AddFrame { | ||
bool IsValid() const { | ||
if (name.empty()) { | ||
drake::log()->error("add_frame: `name` must be non-empty"); | ||
return false; | ||
} else if (!X_PF.base_frame || X_PF.base_frame->empty()) { | ||
drake::log()->error("add_frame: `X_PF.base_frame` must be defined"); | ||
return false; | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(name)); | ||
a->Visit(DRAKE_NVP(X_PF)); | ||
} | ||
|
||
// Name of frame to be added. If scope is specified, will override model | ||
// instance; otherwise, will use `X_PF.base_frame`s instance. | ||
std::string name; | ||
// Pose of frame to be added, `F`, w.r.t. parent frame `P` (as defined by | ||
// `X_PF.base_frame`). | ||
Transform X_PF; | ||
}; | ||
|
||
struct AddDirectives { | ||
bool IsValid() const { | ||
if (file.empty()) { | ||
drake::log()->error("add_directives: `file` must be non-empty"); | ||
return false; | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(file)); | ||
a->Visit(DRAKE_NVP(model_namespace)); | ||
} | ||
|
||
std::string file; | ||
// Namespaces base model instance for processing directive files. | ||
// Affects scoping (i.e. the following members): | ||
// - AddModel::name | ||
// - AddModelInstance::name | ||
// - AddFrame::name | ||
// - AddWeld::parent | ||
// - AddWeld::child | ||
// - AddFrame::X_PF::base_frame | ||
// - AddDirectives::model_namespace | ||
// See `README.md` for example references and namespacing. | ||
std::optional<std::string> model_namespace; | ||
}; | ||
|
||
struct ModelDirective { | ||
bool IsValid() const { | ||
const bool unique = | ||
(add_model.has_value() + add_model_instance.has_value() + | ||
add_frame.has_value() + add_weld.has_value() + | ||
add_package_path.has_value() + add_directives.has_value()) == 1; | ||
if (!unique) { | ||
drake::log()->error( | ||
"directive: Specify one of `add_model`, `add_model_instance`, " | ||
"`add_frame`, `add_package_path`, or `add_directives`"); | ||
return false; | ||
} else if (add_model) { | ||
return add_model->IsValid(); | ||
} else if (add_model_instance) { | ||
return add_model_instance->IsValid(); | ||
} else if (add_frame) { | ||
return add_frame->IsValid(); | ||
} else if (add_weld) { | ||
return add_weld->IsValid(); | ||
} else if (add_package_path) { | ||
return add_package_path->IsValid(); | ||
} else { | ||
return add_directives->IsValid(); | ||
} | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(add_model)); | ||
a->Visit(DRAKE_NVP(add_model_instance)); | ||
a->Visit(DRAKE_NVP(add_frame)); | ||
a->Visit(DRAKE_NVP(add_weld)); | ||
a->Visit(DRAKE_NVP(add_package_path)); | ||
a->Visit(DRAKE_NVP(add_directives)); | ||
} | ||
|
||
std::optional<AddModel> add_model; | ||
std::optional<AddModelInstance> add_model_instance; | ||
std::optional<AddFrame> add_frame; | ||
std::optional<AddWeld> add_weld; | ||
std::optional<AddPackagePath> add_package_path; | ||
std::optional<AddDirectives> add_directives; | ||
}; | ||
|
||
struct ModelDirectives { | ||
bool IsValid() const { | ||
for (auto& directive : directives) { | ||
if (!directive.IsValid()) return false; | ||
} | ||
return true; | ||
} | ||
|
||
template <typename Archive> | ||
void Serialize(Archive* a) { | ||
a->Visit(DRAKE_NVP(directives)); | ||
} | ||
|
||
std::vector<ModelDirective> directives; | ||
}; | ||
|
||
/// Syntactic sugar for a common idiom: Construct an add_package_path | ||
/// directive and insert it at the beginning of a ModelDirectives. | ||
inline void AddPackageToModelDirectives(const std::string& package_name, | ||
const std::string& package_path, | ||
ModelDirectives* directives) { | ||
common::schema::AddPackagePath add_package_path; | ||
add_package_path.name = package_name; | ||
add_package_path.path = package_path; | ||
common::schema::ModelDirective directive; | ||
directive.add_package_path = add_package_path; | ||
directives->directives.insert( | ||
directives->directives.begin(), directive); | ||
} | ||
|
||
} // namespace schema | ||
} // namespace common | ||
} // namespace anzu |
Oops, something went wrong.