void WindowsManager::initParent (const std::string& nodeName,
         NodePtr_t node)
 {
     GroupNodePtr_t groupNode = groupNodes_
         [WindowsManager::parentName (nodeName)];
     if (groupNode) {
         groupNode->addChild (node);
     }
 }
예제 #2
0
    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);
            }
          }
        }
    }
예제 #3
0
    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);
        }
    }
예제 #4
0
    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);
        }
    }
 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();
     }
 }
예제 #6
0
  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();
  }
예제 #7
0
  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;
  }