void WindowsManager::addUrdfObjects (const char* urdfNameCorba,
const char* urdfPathCorba,
const char* urdfPackagePathCorba,
bool visual)
{
const std::string urdfName (urdfNameCorba);
const std::string urdfPath (urdfPathCorba);
const std::string urdfPackagePath (urdfPackagePathCorba);
if (urdfName == "") {
throw std::runtime_error ("Parameter nodeName cannot be empty in "
"idl request addUrdfObjects.");
}
if (nodes_.find (urdfName) != nodes_.end ()) {
std::ostringstream oss;
oss << "You need to chose an other name, \"" << urdfName
<< "\" already exist.";
throw std::runtime_error (oss.str ());
}
mtx_.lock();
GroupNodePtr_t urdf = urdfParser::parse
(urdfName, urdfPath, urdfPackagePath,
visual ? "visual" : "collision", "object");
NodePtr_t link;
for (int i=0; i< urdf->getNumOfChildren (); i++) {
link = urdf->getChild (i);
nodes_[link->getID ()] = link;
}
WindowsManager::initParent (urdfName, urdf);
addGroup (urdfName, urdf);
mtx_.unlock();
}
bool WindowsManager::addUrdfCollision (const char* urdfNameCorba,
const char* urdfPathCorba, const char* urdfPackagePathCorba)
{
const std::string urdfName (urdfNameCorba);
const std::string urdfPath (urdfPathCorba);
const std::string urdfPackagePath (urdfPackagePathCorba);
if (nodes_.find (urdfName) != nodes_.end ()) {
std::cout << "You need to chose an other name, \"" << urdfName
<< "\" already exist." << std::endl;
return false;
}
else {
mtx_.lock();
GroupNodePtr_t urdf = urdfParser::parse
(urdfName, urdfPath, urdfPackagePath, "collision");
NodePtr_t link;
for (int i=0; i< urdf->getNumOfChildren (); i++) {
link = urdf->getChild (i);
nodes_[link->getID ()] = link;
}
WindowsManager::initParent (urdfName, urdf);
addGroup (urdfName, urdf);
mtx_.unlock();
return true;
}
}
void WindowsManager::initParent (const std::string& nodeName,
NodePtr_t node)
{
GroupNodePtr_t groupNode = groupNodes_
[WindowsManager::parentName (nodeName)];
if (groupNode) {
groupNode->addChild (node);
}
}
void WindowsManager::createSceneWithFloor (const char* sceneNameCorba)
{
std::string sceneName (sceneNameCorba);
if (nodes_.find (sceneName) != nodes_.end ()) {
std::ostringstream oss;
oss << "A scene with name, \"" << sceneName << "\" already exists.";
throw std::runtime_error (oss.str ());
}
else {
mtx_.lock();
GroupNodePtr_t mainNode = GroupNode::create (sceneName);
addGroup (sceneName, mainNode);
std::string floorName = sceneName + "/floor";
LeafNodeGroundPtr_t floor = LeafNodeGround::create (floorName);
addNode (floorName, floor);
mainNode->addChild (floor);
mtx_.unlock();
}
}
void addMesh (const std::string &robotName,
const std::string &meshDataRootDir,
boost::shared_ptr < urdf::Link >& urdfLink,
std::size_t j,
GroupNodePtr_t &linkNode, bool visual, bool linkFrame)
{
std::string link_name;
std::string mesh_path;
::boost::shared_ptr< ::urdf::Mesh > mesh_shared_ptr;
if (visual) {
mesh_shared_ptr = boost::static_pointer_cast< ::urdf::Mesh >
( urdfLink->visual_array [j]->geometry );
} else {
mesh_shared_ptr = boost::static_pointer_cast< ::urdf::Mesh >
( urdfLink->collision_array [j]->geometry );
}
link_name = urdfLink->name;
if ( mesh_shared_ptr != 0 )
{
mesh_path = getFilename (mesh_shared_ptr->filename, meshDataRootDir);
std::ostringstream oss;
oss << robotName << "/" << link_name << "_" << j;
LeafNodeColladaPtr_t meshNode = LeafNodeCollada::create
(oss.str (), mesh_path);
osgVector3 static_pos; osgQuat static_quat;
if (linkFrame) {
getStaticTransform (urdfLink, static_pos, static_quat, visual,j);
}
meshNode->setStaticTransform(static_pos,static_quat);
meshNode->setScale(osgVector3((float)mesh_shared_ptr->scale.x,
(float)mesh_shared_ptr->scale.y,
(float)mesh_shared_ptr->scale.z));
linkNode->addChild (meshNode);
// Set Color if specified
if (visual && urdfLink->visual_array [j]->material != NULL) {
osgVector4 color(urdfLink->visual_array [j]->material->color.r,
urdfLink->visual_array [j]->material->color.g,
urdfLink->visual_array [j]->material->color.b,
urdfLink->visual_array [j]->material->color.a);
meshNode->setColor(color);
if (urdfLink->visual_array [j]->material->texture_filename != "") {
std::string textureFilename = getFilename
(urdfLink->visual_array [j]->material->texture_filename, meshDataRootDir);
meshNode->setTexture(textureFilename);
}
}
}
}
void addCylinder (const std::string &robotName,
const std::string& meshDataRootDir,
boost::shared_ptr < urdf::Link >& urdfLink,
std::size_t j,
GroupNodePtr_t &linkNode, bool visual, bool linkFrame)
{
std::string link_name;
::boost::shared_ptr< ::urdf::Cylinder > cylinder_shared_ptr;
if (visual) {
cylinder_shared_ptr = boost::static_pointer_cast < ::urdf::Cylinder >
( urdfLink->visual_array [j]->geometry );
} else {
cylinder_shared_ptr = boost::static_pointer_cast < ::urdf::Cylinder >
( urdfLink->collision_array [j]->geometry );
}
link_name = urdfLink->name;
std::cout << "Cylinder" << std::endl;
if ( cylinder_shared_ptr != 0 )
{
std::ostringstream oss;
oss << robotName << "/" << link_name << "_" << j;
LeafNodeCylinderPtr_t cylinderNode
(LeafNodeCylinder::create
(oss.str (), (float)cylinder_shared_ptr.get()->radius,
(float)cylinder_shared_ptr.get()->length));
osgVector3 static_pos; osgQuat static_quat;
if (linkFrame) {
getStaticTransform (urdfLink, static_pos, static_quat, visual,j);
}
cylinderNode->setStaticTransform(static_pos,static_quat);
// Set Color if specified
if (visual && urdfLink->visual_array [j]->material != NULL) {
osgVector4 color(urdfLink->visual_array [j]->material->color.r, urdfLink->visual_array [j]->material->color.g, urdfLink->visual_array [j]->material->color.b, urdfLink->visual_array [j]->material->color.a);
cylinderNode->setColor(color);
if (urdfLink->visual_array [j]->material->texture_filename != "") {
std::string textureFilename = getFilename
(urdfLink->visual_array [j]->material->texture_filename, meshDataRootDir);
cylinderNode->setTexture(textureFilename);
}
}
// add object to link node
linkNode->addChild(cylinderNode);
}
}
void addBox (const std::string &robotName,
const std::string& meshDataRootDir,
boost::shared_ptr < urdf::Link >& urdfLink,
std::size_t j,
GroupNodePtr_t &linkNode, bool visual, bool linkFrame)
{
std::string link_name;
::boost::shared_ptr< ::urdf::Box > box_shared_ptr;
if (visual) {
box_shared_ptr = boost::static_pointer_cast< ::urdf::Box >
( urdfLink->visual_array [j]->geometry);
} else {
box_shared_ptr = boost::static_pointer_cast< ::urdf::Box >
( urdfLink->collision_array [j]->geometry);
}
link_name = urdfLink->name;
std::cout << "Box" << std::endl;
if ( box_shared_ptr != 0 ) {
std::ostringstream oss;
oss << robotName << "/" << link_name << "_" << j;
LeafNodeBoxPtr_t boxNode = LeafNodeBox::create
(oss.str (),
osgVector3((float)(.5*box_shared_ptr->dim.x),
(float)(.5*box_shared_ptr->dim.y),
(float)(.5*box_shared_ptr->dim.z)));
osgVector3 static_pos; osgQuat static_quat;
if (linkFrame) {
getStaticTransform (urdfLink, static_pos, static_quat, visual,j);
}
boxNode->setStaticTransform(static_pos,static_quat);
// Set Color if specified
if (visual && urdfLink->visual_array [j]->material != NULL) {
osgVector4 color(urdfLink->visual_array [j]->material->color.r, urdfLink->visual_array [j]->material->color.g, urdfLink->visual_array [j]->material->color.b, urdfLink->visual_array [j]->material->color.a);
boxNode->setColor(color);
if (urdfLink->visual_array [j]->material->texture_filename != "") {
std::string textureFilename = getFilename
(urdfLink->visual_array [j]->material->texture_filename, meshDataRootDir);
boxNode->setTexture(textureFilename);
}
}
// add object to link node
linkNode->addChild(boxNode);
}
}
int main(int, const char**)
{
using namespace graphics;
LeafNodeBoxPtr_t box = LeafNodeBox::create("box1", osgVector3(1.,1.,1.));
/*LeafNodeCapsulePtr_t capsule = LeafNodeCapsule::create("capsule1", 1,1);
LeafNodeConePtr_t cone = LeafNodeCone::create("cone", 1,1);
LeafNodeCylinderPtr_t cylindre = LeafNodeCylinder::create("cylindre", 1,1);
LeafNodeSpherePtr_t sphere = LeafNodeSphere::create("sphere", 1);
LeafNodeGroundPtr_t ground = LeafNodeGround::create("ground");*/
//LeafNodeColladaPtr_t collada = LeafNodeCollada::create("collada","/home/simeval/AluminumChair.dae");
box->addLandmark(1.);
//LeafNodeLinePtr_t line = LeafNodeLine::create(std::string("line"), osgVector3(1.0,1.0,1.0), osgVector3(0.0,0.0,0.0));
//LeafNodeFacePtr_t face = LeafNodeFace::create(std::string("face"), osgVector3(0.0,0.0,0.0), osgVector3(-2.0,0.0,0.0), osgVector3(-2.0,-2.0,0.0), osgVector3(0.0,-2.0,0.0));
//face->addVertex(osgVector3(0.,0.,2.));
GroupNodePtr_t world = GroupNode::create(std::string("world"));
//GroupNodePtr_t robot = GroupNode::create(std::string("robot"));
// GroupNodePtr_t robot = urdfParser::parse(std::string("hrp2"),
//std::string("/home/ostasse/devel/ros-indigo-1/install/share/hrp2_14_description/urdf/hrp2_14.urdf"),
//std::string("/home/ostasse/devel/ros-indigo-1/install/share/"));
world->addChild(box);
/*world->addChild(obstacle);
DefVector3 position1(2.,0.,0.);
DefQuat attitude1(1.,0.,0.,0.);
Tools::ConfigurationPtr_t config1 = Tools::Configuration::create(position1, attitude1);
DefVector3 position2(0.,2.,0.);
DefQuat attitude2(1.,0.,0.,0.);
Tools::ConfigurationPtr_t config2 = Tools::Configuration::create(position2, attitude2);
robot->addChild(box);
robot->applyConfiguration(config1);
box->applyConfiguration(config1);
robot->addChild(capsule);
capsule->setVisibilityMode(VISIBILITY_OFF);
robot->addChild(cone);
cone->applyConfiguration(config2);
cone->setColor(osgVector4(1.,1.,0.5,1.));
cone->setVisibilityMode(VISIBILITY_ON);
DefVector3 position3(0.,0.,8.);
DefQuat attitude3(1.,0.,0.,0.);
Tools::ConfigurationPtr_t config3 = Tools::Configuration::create(position3, attitude3);
obstacle->applyConfiguration(config3);
obstacle->addChild(cylindre);
sphere->applyConfiguration(config2);
obstacle->addChild(sphere);
sphere->setAlpha(0.1f);
world->addChild(ground);
world->addChild(collada);
collada->applyConfiguration(config2);
std::string name("world/robot/genou");
std::cout << (parseName(name)) << std::endl;*/
//world->addChild(ground);
//world->addChild(line);
world->addChild(box);
//world->addChild(robot);
WindowManagerPtr_t gm = WindowManager::create();
gm->addNode(world);
//osgViewer::Viewer viewer;
//viewer.setSceneData( world->asGroup() );
box->deleteLandmark();
box->addLandmark(2.);
box->applyConfiguration(osgVector3(0.,0.,0.), osgQuat(0.884,0.306,-0.177,0.306));
world->addLandmark(1.);
return gm->run();
}
GroupNodePtr_t urdfParser::parse (const std::string& robotName,
const std::string& urdf_file_path,
const std::string& meshDataRootDir,
const std::string& collisionOrVisual,
const std::string& linkOrObjectFrame)
{
if (collisionOrVisual != "visual" && collisionOrVisual != "collision") {
throw std::runtime_error ("parameter collisionOrVisual should be either "
"\"collision\" or \"visual\"");
}
if (linkOrObjectFrame != "link" && linkOrObjectFrame != "object") {
throw std::runtime_error ("parameter linkOrObjectFrame should be either "
"\"link\" or \"object\"");
}
bool linkFrame = true;
if (linkOrObjectFrame == "object") linkFrame = false;
boost::shared_ptr< urdf::ModelInterface > model =
urdf::parseURDFFile( urdf_file_path );
// Test that file has correctly been parsed
if (!model) {
throw std::runtime_error (std::string ("Failed to parse ") +
urdf_file_path);
}
GroupNodePtr_t robot = GroupNode::create(robotName);
std::vector< boost::shared_ptr < urdf::Link > > links;
model->getLinks(links);
std::string link_name;
for (unsigned int i = 0 ; i < links.size() ; i++) {
link_name = links[i]->name;
std::cout << link_name << std::endl;
GroupNodePtr_t linkNode (GroupNode::create (robotName + "/" + link_name));
// add link to robot node
robot->addChild (linkNode);
if (collisionOrVisual == "visual") {
for (std::size_t j = 0; j < links[i]->visual_array.size (); ++j) {
switch (links[i]->visual_array [j]->geometry->type) {
case urdf::Geometry::MESH:
internal_urdf_parser::addMesh (robotName, meshDataRootDir,
links [i], j, linkNode, true,
linkFrame);
break;
case urdf::Geometry::CYLINDER:
internal_urdf_parser::addCylinder (robotName, meshDataRootDir,
links [i], j, linkNode, true,
linkFrame);
break;
case urdf::Geometry::BOX:
internal_urdf_parser::addBox (robotName, meshDataRootDir,
links [i], j, linkNode,
true, linkFrame);
break;
case urdf::Geometry::SPHERE:
internal_urdf_parser::addSphere (robotName, meshDataRootDir,
links [i], j, linkNode, true,
linkFrame);
break;
}
}
} else {
for (std::size_t j=0; j < links[i]->collision_array.size (); ++j) {
switch (links[i]->collision_array [j]->geometry->type) {
case urdf::Geometry::MESH:
internal_urdf_parser::addMesh (robotName, meshDataRootDir,
links [i], j, linkNode, false,
linkFrame);
break;
case urdf::Geometry::CYLINDER:
internal_urdf_parser::addCylinder (robotName, meshDataRootDir,
links [i], j, linkNode, false,
linkFrame);
break;
case urdf::Geometry::BOX:
internal_urdf_parser::addBox (robotName, meshDataRootDir,
links [i], j, linkNode, false,
linkFrame);
break;
case urdf::Geometry::SPHERE:
internal_urdf_parser::addSphere (robotName, meshDataRootDir,
links [i], j, linkNode, false,
linkFrame);
break;
}
}
}
}
return robot;
}
