YAML::Node MergeYaml( const YAML::Node& a, const YAML::Node& b )
{
// Short circuit cases
if( a.IsNull() ) { return b; }
else if( b.IsNull() ) { return a; }
if( !a.IsMap() || !b.IsMap() )
{
throw std::runtime_error( "Cannot merge non-map nodes." );
}
YAML::Node node;
CopyYaml( a, node );
YAML::Node::const_iterator iter;
// Cycle through b and add all fields to node
for( iter = b.begin(); iter != b.end(); iter++ )
{
std::string key = iter->first.as<std::string>();
// If both a and b have a key we have to merge them
if( node[key] )
{
node[key] = MergeYaml( node[key], iter->second );
}
// Otherwise we just add it
else
{
node[key] = iter->second;
}
}
return node;
}
virtual Node* getNode(const char* key) {
std::string path = key;
YAML::Node result;
for(size_t i = 0; i < entries.size(); i++) {
YAML::Node node = entries[i];
size_t pos = 0;
while(pos < path.size()) {
std::string subpath;
size_t end = path.find('.', pos);
if(end == std::string::npos) {
subpath = path.substr(pos);
pos = path.size();
} else {
subpath = path.substr(pos, end - pos);
pos = end + 1;
}
if(!node.IsNull()) {
node.reset(node[subpath]);
}
}
if(!node.IsNull() && node.IsDefined()) {
return new NodeImpl(node);
}
}
return NULL;
}
TEST ResetNode()
{
YAML::Node node = YAML::Load("[1, 2, 3]");
YAML_ASSERT(!node.IsNull());
YAML::Node other = node;
node.reset();
YAML_ASSERT(node.IsNull());
YAML_ASSERT(!other.IsNull());
node.reset(other);
YAML_ASSERT(!node.IsNull());
YAML_ASSERT(other == node);
return true;
}
void
Module::loadConfigurationFile( const QString& configFileName ) //throws YAML::Exception
{
foreach ( const QString& path, moduleConfigurationCandidates( Settings::instance()->debugMode(), m_name, configFileName ) )
{
QFile configFile( path );
if ( configFile.exists() && configFile.open( QFile::ReadOnly | QFile::Text ) )
{
QByteArray ba = configFile.readAll();
YAML::Node doc = YAML::Load( ba.constData() );
if ( doc.IsNull() )
{
cDebug() << "Found empty module configuration" << path;
// Special case: empty config files are valid,
// but aren't a map.
return;
}
if ( !doc.IsMap() )
{
cWarning() << "Bad module configuration format" << path;
return;
}
cDebug() << "Loaded module configuration" << path;
m_configurationMap = CalamaresUtils::yamlMapToVariant( doc ).toMap();
m_emergency = m_maybe_emergency
&& m_configurationMap.contains( EMERGENCY )
&& m_configurationMap[ EMERGENCY ].toBool();
return;
}
}
bool get_field(doid_t do_id, const Field* field, val_t &value)
{
m_log->trace() << "Getting field on obj-" << do_id << endl;
YAML::Node document;
if(!load(do_id, document))
{
return false;
}
// Get the fields from the file that are not being updated
YAML::Node node = document["fields"][field->get_name()];
if(!node.IsDefined() || node.IsNull())
{
return false;
}
m_log->trace() << "Found requested field: " + field->get_name() << endl;
value = read_yaml_field(field, node, do_id);
if(value.size() > 0)
{
return true;
}
return false;
}
inline bool load(doid_t do_id, YAML::Node &document)
{
ifstream stream(filename(do_id));
document = YAML::Load(stream);
if(!document.IsDefined() || document.IsNull())
{
m_log->error() << "obj-" << do_id << " does not exist in database." << endl;
return false;
}
if(!document["class"].IsDefined() || document["class"].IsNull())
{
m_log->error() << filename(do_id) << " does not contain the 'class' key." << endl;
return false;
}
if(!document["fields"].IsDefined() || document["fields"].IsNull())
{
m_log->error() << filename(do_id) << " does not contain the 'fields' key." << endl;
return false;
}
// Read object's DistributedClass
string dc_name = document["class"].as<string>();
if(!g_dcf->get_class_by_name(dc_name))
{
m_log->error() << "Class '" << dc_name << "', loaded from '" << filename(do_id)
<< "', does not exist." << endl;
return false;
}
return true;
}
bool BandwidthGui::groupFromYaml(const std::string& yaml, GroupMap* groupMap)
{
YAML::Node grpInfo = YAML::Load(yaml);
if (grpInfo.IsNull())
{
return true;
}
if (grpInfo.Type() != YAML::NodeType::Map)
{
return false;
}
for (const auto& pair : grpInfo)
{
if(pair.first.Type() != YAML::NodeType::Scalar)
{
return false;
}
std::string key = pair.first.as<std::string>();
for (const auto& element : pair.second)
{
if(element.Type() != YAML::NodeType::Scalar)
{
return false;
}
(*groupMap)[key].push_back(element.as<std::string>());
}
}
return true;
}
boost::optional<T> from_yaml(type_t<boost::optional<T>>,
const YAML::Node& value)
{
if (!value || value.IsNull())
return {};
return yaml::deserialize<T>(value);
}
bool loadPluginsConfig (YAML::Node & y_root, PluginsConfig & config)
{
try
{
if (y_root.IsNull())
return false;
YAML::Node y_cfg = y_root["Plugins"]; // @TODO: unicode? utf8?
if (y_cfg)
{
int const n = y_cfg.size();
//YAML::NodeType::value cst = y_tasks.Type();
for (int i = 0; i < n; ++i)
{
YAML::Node y_cfg_i = y_cfg[i];
bb::PluginConfig cfg = y_cfg[i].as<bb::PluginConfig>();
config.m_plugins.push_back(cfg);
}
return true;
}
}
catch (std::exception & e)
{
return false;
}
return false;
}
// Incrementally load YAML
static NEVER_INLINE void operator +=(YAML::Node& left, const YAML::Node& node)
{
if (node && !node.IsNull())
{
if (node.IsMap())
{
for (const auto& pair : node)
{
if (pair.first.IsScalar())
{
auto&& lhs = left[pair.first.Scalar()];
lhs += pair.second;
}
else
{
// Exotic case (TODO: probably doesn't work)
auto&& lhs = left[YAML::Clone(pair.first)];
lhs += pair.second;
}
}
}
else if (node.IsScalar() || node.IsSequence())
{
// Scalars and sequences are replaced completely, but this may change in future.
// This logic may be overwritten by custom demands of every specific cfg:: node.
left = node;
}
}
}
virtual void parseFile(const char* path, const char* key) {
try {
YAML::Node part = YAML::LoadFile(path)[key];
if(!part.IsNull() && part.IsDefined())
entries.push_back(part);
} catch(std::exception& e) {
throw exception(e.what());
}
}
param_tree param_tree::generate_from_yaml(YAML::Node const& yaml)
{
if(yaml.IsNull())
{
return param_tree{};
}
param_tree pt;
auto root = pt.m_tree.create_root().first;
pt.generate_from_yaml(yaml, root);
return pt;
}
T get(const std::string& path, const T& fallback = T()) {
for(size_t i = 0; i < entries.size(); i++) {
YAML::Node node = entries[i];
size_t pos = 0;
while(pos < path.size()) {
std::string subpath;
size_t end = path.find('.', pos);
if(end == std::string::npos) {
subpath = path.substr(pos);
pos = path.size();
} else {
subpath = path.substr(pos, end - pos);
pos = end + 1;
}
if(!node.IsNull()) {
node.reset(node[subpath]);
}
}
if(!node.IsNull() && node.IsDefined()) {
return node.as<T>();
}
}
return fallback;
}
void CopyYaml( const YAML::Node& src, YAML::Node& dst )
{
if( src.IsNull() ) { return; }
if( src.IsScalar() )
{
dst = src;
return;
};
YAML::Node::const_iterator iter;
for( iter = src.begin(); iter != src.end(); iter++ )
{
dst[ iter->first.as<std::string>() ] = iter->second;
}
}
ClientAgent::ClientAgent(RoleConfig roleconfig) : Role(roleconfig), m_acceptor(NULL),
m_client_type(client_type.get_rval(roleconfig)),
m_server_version(server_version.get_rval(roleconfig)),
m_ct(min_channel.get_rval(roleconfig), max_channel.get_rval(roleconfig))
{
std::stringstream ss;
ss << "Client Agent (" << bind_addr.get_rval(roleconfig) << ")";
m_log = new LogCategory("clientagent", ss.str());
//Initialize the network
std::string str_ip = bind_addr.get_rval(m_roleconfig);
std::string str_port = str_ip.substr(str_ip.find(':', 0) + 1, std::string::npos);
str_ip = str_ip.substr(0, str_ip.find(':', 0));
tcp::resolver resolver(io_service);
tcp::resolver::query query(str_ip, str_port);
tcp::resolver::iterator it = resolver.resolve(query);
m_acceptor = new tcp::acceptor(io_service, *it, true);
if(g_uberdogs.empty())
{
YAML::Node udnodes = g_config->copy_node()["uberdogs"];
if(!udnodes.IsNull())
{
for(auto it = udnodes.begin(); it != udnodes.end(); ++it)
{
YAML::Node udnode = *it;
Uberdog ud;
ud.dcc = g_dcf->get_class_by_name(udnode["class"].as<std::string>());
if(!ud.dcc)
{
m_log->fatal() << "DCClass " << udnode["class"].as<std::string>()
<< "Does not exist!" << std::endl;
exit(1);
}
ud.anonymous = udnode["anonymous"].as<bool>();
g_uberdogs[udnode["id"].as<uint32_t>()] = ud;
}
}
}
start_accept();
}
TableRow::TableRow( YAML::Node config )
{
this->readRequiredEntry<std::string>( table, config["table"], "rows::table" );
this->readEntry<bool>( recycleRows, config["recycleRows"], false );
this->readEntry<callback>( callbackPreUpdate, config["callbackPreUpdate"], "~" );
this->readEntry<callback>( callbackPostUpdate, config["callbackPostUpdate"], "~" );
this->readEntry<callback>( callbackPreInsert, config["callbackPreInsert"], "~" );
this->readEntry<callback>( callbackPostInsert, config["callbackPostInsert"], "~" );
this->readEntry<callback>( callbackPreDelete, config["callbackPreDelete"], "~" );
this->readEntry<callback>( callbackPostDelete, config["callbackPostDelete"], "~" );
Logger::debug( std::string( "* Table : " ) + table + "\n" +
"- Recyclage de rows : " + ( recycleRows ? "Oui" : "Non" ) + "\n" +
"- Callback update : " + callbackPreUpdate + " / " + callbackPostUpdate + "\n" +
"- Callback insert : " + callbackPreInsert + " / " + callbackPostInsert + "\n" +
"- Callback delete : " + callbackPreDelete + " / " + callbackPostDelete + "\n" +
"- Champs : \n"
);
YAML::Node fields = config["fields"];
for( uint i = 0; i < fields.size(); i++ )
{
this->fields.push_back( TableRow_Col( fields[i] ) );
}
Logger::debug( "\n" );
YAML::Node subrows = config["subRows"];
if( subrows.IsDefined() && !subrows.IsNull() )
{
for( uint i = 0; i < subrows.size(); i++ )
{
this->subRows.push_back( TableRow( subrows[i] ) );
}
}
}
YAMLDatabase(ConfigNode dbeconfig, doid_t min_id, doid_t max_id) :
DatabaseBackend(dbeconfig, min_id, max_id),
m_next_id(min_id),
m_free_ids(),
m_foldername(foldername.get_rval(m_config))
{
stringstream log_name;
log_name << "Database-YAML" << "(Range: [" << min_id << ", " << max_id << "])";
m_log = new LogCategory("yamldb", log_name.str());
// Open database info file
ifstream infostream(m_foldername + "/info.yaml");
YAML::Node document = YAML::Load(infostream);
if(document.IsDefined() && !document.IsNull())
{
// Read next available id
YAML::Node key_next = document["next"];
if(key_next.IsDefined() && !key_next.IsNull())
{
m_next_id = document["next"].as<doid_t>();
}
// Read available freed ids
YAML::Node key_free = document["free"];
if(key_free.IsDefined() && !key_free.IsNull())
{
for(doid_t i = 0; i < key_free.size(); i++)
{
m_free_ids.push_back(key_free[i].as<doid_t>());
}
}
}
// Close database info file
infostream.close();
}
void Motion::parseRule(const YAML::Node& node)
{
rules.clear();
YAML::Node rulesNode = node["rules"];
YAML::Node partsNode;
if(rulesNode.IsNull())
return;
// Parse rules
for (std::size_t i = 0; i < rulesNode.size(); i++)
{
rules.push_back(Rule());
partsNode = rulesNode[i];
rules.back().name = partsNode["name"].as<std::string>();
//printf("Rule %d \n\n", (int)i);
// Parse rule parts
partsNode = partsNode["parts"];
for (std::size_t j = 0; j < partsNode.size(); j++)
{
const YAML::Node& parsedRulePart = partsNode[j];
RulePart rulePart;
rulePart.type = parsedRulePart["type"].as<std::string>();
//printf("type %s \n", rulePart.type.c_str());
rulePart.frameName = parsedRulePart["frameName"].as<std::string>();
rulePart.jointName = parsedRulePart["jointName"].as<std::string>();
rulePart.scale = parsedRulePart["scale"].as<double>();
rules.back().parts.push_back(rulePart);
}
}
}
bool loadTrayConfig (YAML::Node & y_root, TrayConfig & config)
{
try
{
if (y_root.IsNull())
return false;
YAML::Node y_tasks = y_root["Tray"]; // @TODO: unicode? utf8?
if (y_tasks)
{
int const n = y_tasks.size();
for (int i = 0; i < n; ++i)
{
}
return true;
}
}
catch (std::exception & e)
{
TRACE_MSG(LL_ERROR, CTX_CONFIG, "YAML exception in source %s: %s", __FILE__, e.what());
return false;
}
return false;
}
XmlRpc::XmlRpcValue YamlToXml( const YAML::Node& node )
{
XmlRpc::XmlRpcValue xml;
if( node.IsNull() )
{
return xml;
}
else if( node.IsSequence() )
{
std::vector< std::string > contents = node.as< std::vector< std::string > >();
xml.setSize( contents.size() );
for( unsigned int i = 0; i < contents.size(); i++ )
{
xml[i] = contents[i];
}
}
else if( node.IsScalar() )
{
xml = node.as< std::string >();
}
else if( node.IsMap() )
{
YAML::Node::const_iterator iter;
for( iter = node.begin(); iter != node.end(); iter++ )
{
std::string name = iter->first.as<std::string>();
xml[ name ] = YamlToXml( iter->second );
}
}
else
{
std::cerr << "Invalid YAML node type." << std::endl;
}
return xml;
}
int main(int argc, char** argv)
{
bool verbose = false;
bool bytes = false;
int opt;
while ((opt = getopt(argc, argv, "vb")) != -1) {
switch (opt) {
case 'v':
verbose = true;
break;
case 'b':
bytes = true;
break;
default: /* '?' */
cerr << usage;
return 1;
}
}
if ( optind >= argc )
{
cerr << usage;
return 1;
}
const char* filename = argv[optind];
try
{
YAML::Node doc;
if ( bytes )
{
QFile f( filename );
if ( f.open( QFile::ReadOnly | QFile::Text ) )
doc = YAML::Load( f.readAll().constData() );
}
else
doc = YAML::LoadFile( filename );
if ( doc.IsNull() )
{
// Special case: empty config files are valid,
// but aren't a map. For the example configs,
// this is still an error.
cerr << "WARNING:" << filename << '\n';
cerr << "WARNING: empty YAML\n";
return 1;
}
if ( !doc.IsMap() )
{
cerr << "WARNING:" << filename << '\n';
cerr << "WARNING: not-a-YAML-map (type=" << doc.Type() << ")\n";
return 1;
}
if ( verbose )
{
cerr << "Keys:\n";
for ( auto i = doc.begin(); i != doc.end(); ++i )
cerr << i->first.as<std::string>() << '\n';
}
}
catch ( YAML::Exception& e )
{
cerr << "WARNING:" << filename << '\n';
cerr << "WARNING: YAML parser error " << e.what() << '\n';
return 1;
}
return 0;
}
static bool
hasValue( const YAML::Node& v )
{
return v.IsDefined() && !v.IsNull();
}
//########## CONSTRUCTOR ###############################################################################################
exercise04_gripper_test::exercise04_gripper_test(): ros::NodeHandle()
{
// === PARAMETERS ===
turn_angle_ = M_PI / 4;
search_pose_1_ = "SEARCH_LOW";
search_pose_2_ = "SEARCH_HIGH";
grip_pose_ = "GRIP_POSE";
forward_distance_ = 0.2;
angle_align_tolerance_ = 0.1; // in relation to image height
angle_align_velocity_factor_ = 1.0;
max_lost_ball_count_ = 3;
target_point_x_ = 0.0;
target_point_y_ = 0.0;
position_align_tolerance_ = 0.02;
position_align_velocity_factor_ = 1.0;
grip_width_ = 0.027;
pose_file_name_ = ros::package::getPath("oberstufenlabor_mechatronik_robot");
pose_file_name_.append("/poses/poses.yaml");
above_pose_height_diff_ = 0.03;
above_pose_radius_diff_ = 0.01;
max_turn_angle_ = M_PI / 2;
hold_duration_ = 1.0;
double interpolation_velocity_factor = 0.3;
bool all_params_loaded =
ros::param::get("oberstufenlabor_mechatronik_robot/turn_angle", turn_angle_) &&
ros::param::get("oberstufenlabor_mechatronik_robot/search_pose_1", search_pose_1_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/search_pose_2", search_pose_2_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/grip_pose", grip_pose_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/forward_distance", forward_distance_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/angle_align_tolerance", angle_align_tolerance_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/angle_align_velocity_factor", angle_align_velocity_factor_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/max_lost_ball_count", max_lost_ball_count_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/position_align_tolerance", position_align_tolerance_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/position_align_velocity_factor", position_align_velocity_factor_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/poses_file", pose_file_name_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/above_pose_height_diff", above_pose_height_diff_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/above_pose_radius_diff", above_pose_radius_diff_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/max_turn_angle", max_turn_angle_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/hold_duration", hold_duration_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/image_ratio", image_ratio_)&&
ros::param::get("oberstufenlabor_mechatronik_robot/interpolation_velocity_factor", interpolation_velocity_factor);
if(!all_params_loaded)
ROS_WARN("Not all parameters could be loaded.");
std::string target_point_file = ros::package::getPath("oberstufenlabor_mechatronik_robot");
target_point_file.append("/cfg/calibration_params.yaml");
YAML::Node doc = YAML::LoadFile(target_point_file);
if(doc.IsNull())
{
ROS_ERROR("target_point.yaml could not be opened.");
}
else
{
target_point_x_ = doc["x"].as<double>();
target_point_y_ = doc["y"].as<double>();
grip_width_ = doc["grip_width"].as<double>();
}
// === LOAD POSES ===
poses_ = youbot_poses::read(pose_file_name_);
cargo_.resize(9);
for(uint i=0; i<cargo_.size(); i++)
{
std::stringstream ss;
ss << "CARGO_" << i+1;
ykin::JointPosition cargo_jnt_pos = poses_[ss.str()];
ykin::CylindricPosition cargo_cyl_pos = cargo_jnt_pos.toCylindric();
ykin::CylindricPosition rel_pos;
rel_pos.setZ(above_pose_height_diff_);
rel_pos.setR(above_pose_radius_diff_);
ykin::CylindricPosition above_cyl_pos = cargo_cyl_pos + rel_pos;
ykin::JointPosition above_jnt_pos = above_cyl_pos.toJointspace(cargo_jnt_pos);
cargo_[i].is_free = true;
cargo_[i].pose = cargo_cyl_pos;
cargo_[i].above_pose = above_jnt_pos;
cargo_[i].ball_color = "";
}
current_cargo_slot_ = &cargo_[0];
// === ROS COMMUNICATION ===
cmd_vel_publisher_ = advertise<geometry_msgs::Twist>("cmd_vel", 1);
// === INITIALISE YOUBOT ===
arm_.init(*this);
base_.init(*this);
gripper_.init(*this);
// === OTHER INITIALISATION ===
low_velocity_.setQ1(ykin::MAX_JNT_VELOCITIES[0]);
low_velocity_.setQ2(ykin::MAX_JNT_VELOCITIES[1]);
low_velocity_.setQ3(ykin::MAX_JNT_VELOCITIES[2]);
low_velocity_.setQ4(ykin::MAX_JNT_VELOCITIES[3]);
low_velocity_.setQ5(ykin::MAX_JNT_VELOCITIES[4]);
high_velocity_ = low_velocity_;
low_velocity_ *= interpolation_velocity_factor;
arm_.moveToPose("HOME");
gripper_.open();
gripper_.waitForCurrentAction();
substate_ = GRIP_START;
ROS_INFO("Gripper Test is initialized.");
}
