コード例 #1
0
ファイル: YamlUtils.cpp プロジェクト: Humhu/argus_utils
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;
}
コード例 #2
0
ファイル: config.cpp プロジェクト: guozanhua/Allofw
 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;
 }
コード例 #3
0
ファイル: nodetests.cpp プロジェクト: egocarib/yaml-cpp
 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;
 }
コード例 #4
0
ファイル: Module.cpp プロジェクト: tsimonq2/calamares
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;
        }
    }
コード例 #5
0
ファイル: YAMLDatabase.cpp プロジェクト: Echocage/Astron
		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;
		}
コード例 #6
0
ファイル: YAMLDatabase.cpp プロジェクト: Echocage/Astron
		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;
		}
コード例 #7
0
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;
}
コード例 #8
0
ファイル: yaml.hpp プロジェクト: k0zmo/kl
boost::optional<T> from_yaml(type_t<boost::optional<T>>,
                             const YAML::Node& value)
{
    if (!value || value.IsNull())
        return {};
    return yaml::deserialize<T>(value);
}
コード例 #9
0
	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;
	}
コード例 #10
0
ファイル: SettingsDialog.cpp プロジェクト: Zangetsu38/rpcs3
// 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;
		}
	}
}
コード例 #11
0
ファイル: config.cpp プロジェクト: guozanhua/Allofw
 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());
     }
 }
コード例 #12
0
ファイル: param_tree.cpp プロジェクト: kamrann/workbase
	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;
	}
コード例 #13
0
ファイル: config.cpp プロジェクト: guozanhua/Allofw
 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;
 }
コード例 #14
0
ファイル: YamlUtils.cpp プロジェクト: Humhu/argus_utils
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;
	}
}
コード例 #15
0
ファイル: ClientAgent.cpp プロジェクト: shadowcoder/Astron
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();
}
コード例 #16
0
ファイル: TableRow.cpp プロジェクト: Juluan/phpimport
    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] ) );
            }
        }
    }
コード例 #17
0
ファイル: YAMLDatabase.cpp プロジェクト: Echocage/Astron
		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();
		}
コード例 #18
0
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);
		}
	}
}
コード例 #19
0
	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;
	}
コード例 #20
0
ファイル: YamlUtils.cpp プロジェクト: Humhu/argus_utils
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;
}
コード例 #21
0
ファイル: test_conf.cpp プロジェクト: KaOSx/calamares
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;
}
コード例 #22
0
ファイル: Settings.cpp プロジェクト: calamares/calamares
static bool
hasValue( const YAML::Node& v )
{
    return v.IsDefined() && !v.IsNull();
}
コード例 #23
0
//########## 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.");
}