robowflex::openrave Namespace Reference

Functions
bool	fromXMLFile (moveit_msgs::PlanningScene &planning_scene, const std::string &file, const std::string &model_dir)
	Loads a planning_scene from an OpenRAVE Environment XML. More...

Function Documentation

fromXMLFile()

bool robowflex::openrave::fromXMLFile	(	moveit_msgs::PlanningScene &	planning_scene,
		const std::string &	file,
		const std::string &	model_dir
	)

Loads a planning_scene from an OpenRAVE Environment XML.

Parameters

[out]	planning_scene	The output MoveIt message that will be filled with the planning scene contents.
[in]	file	The path to the OpenRAVE environment XML.
[in]	model_dir	The path to the models directory, which should contain files referenced by the passed in file. In OpenRAVE, "the root directory for all models files is the folder openrave is launched at."

Returns

True on success, false on failure.

Definition at line 208 of file openrave.cpp.

{
    SceneParsingContext load_struct;
    load_struct.directory_stack.push(model_dir);
 
    // Hardcoded offset on WAM (see wam7.kinbody.xml)
    RobotPose tf;
    tf.translation() = Eigen::Vector3d(0.0, 0.0, -0.346);
    tf.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();
    load_struct.robot_offset = tf;
 
    tinyxml2::XMLDocument doc;
    if (!doc.LoadFile(IO::resolvePath(file).c_str()))
    {
        RBX_ERROR("Cannot load file %s", file);
        return false;
    }
 
    auto *env = getFirstChild(&doc, "Environment");
    auto *robot = getFirstChild(env, "Robot");
    if (robot)
        load_struct.robot_offset =
            load_struct.robot_offset * TFfromXML(getFirstChild(robot, "Translation"),  //
                                                 getFirstChild(robot, "RotationAxis"), nullptr);
 
    auto *elem = getFirstChild(env);
    if (not elem)
    {
        RBX_ERROR("There is no/an empty environment element in this openrave scene.");
        return false;
    }
 
    for (; elem; elem = elem->NextSiblingElement())
    {
        const std::string p_key = std::string(elem->Value());
        if (p_key == "KinBody")
        {
            if (!parseKinbody(load_struct, elem, load_struct.robot_offset.inverse(), planning_scene))
                return false;
        }
        else
            RBX_INFO("Ignoring elements of value %s", p_key);
    }
 
    if (not load_struct.coll_objects.empty())
        planning_scene.is_diff = true;
 
    return true;
}