diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp index 74ade24c0fa3f86d3ce4be975935ea88ef60cc13..5d8dadfa515831e981935d364cdf3ccc0a4f5937 100644 --- a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp +++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp @@ -37,13 +37,17 @@ namespace VirtualRobot RobotPtr result; boost::shared_ptr<urdf::ModelInterface> urdf_model = urdf::parseURDFFile(filename.c_str()); + boost::filesystem::path filenameBaseComplete(filename); + boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path(); + std::string basePath = filenameBasePath.string(); + if (!urdf_model) { VR_ERROR << "Error opening urdf file " << filename << endl; } else { - result = createRobot(urdf_model, useColModelsIfNoVisuModel); + result = createRobot(urdf_model, basePath, useColModelsIfNoVisuModel); } return result; @@ -85,7 +89,7 @@ namespace VirtualRobot return URDFFactory; } - VirtualRobot::RobotPtr SimoxURDFFactory::createRobot(boost::shared_ptr<urdf::ModelInterface> urdfModel, bool useColModelsIfNoVisuModel) + VirtualRobot::RobotPtr SimoxURDFFactory::createRobot(boost::shared_ptr<urdf::ModelInterface> urdfModel, const std::string &basePath, bool useColModelsIfNoVisuModel) { THROW_VR_EXCEPTION_IF(!urdfModel, "NULL data"); std::string robotType = urdfModel->getName(); @@ -103,7 +107,7 @@ namespace VirtualRobot while (itBodies != bodies.end()) { boost::shared_ptr<Link> body = itBodies->second; - RobotNodePtr nodeVisual = createBodyNode(robo, body, useColModelsIfNoVisuModel); + RobotNodePtr nodeVisual = createBodyNode(robo, body, basePath, useColModelsIfNoVisuModel); allNodes.push_back(nodeVisual); allNodesMap[body->name] = nodeVisual; std::vector<std::string> childrenlist; @@ -173,7 +177,7 @@ namespace VirtualRobot return result; } - VirtualRobot::VisualizationNodePtr SimoxURDFFactory::convertVisu(boost::shared_ptr<Geometry> g, urdf::Pose& pose) + VirtualRobot::VisualizationNodePtr SimoxURDFFactory::convertVisu(boost::shared_ptr<Geometry> g, urdf::Pose& pose, const std::string &basePath) { const float scale = 1000.0f; // mm VirtualRobot::VisualizationNodePtr res; @@ -203,7 +207,9 @@ namespace VirtualRobot case urdf::Geometry::MESH: { boost::shared_ptr<Mesh> m = boost::dynamic_pointer_cast<Mesh>(g); - std::string filename = getFilename(m->filename); +// std::string filename = getFilename(m->filename); + std::string filename = m->filename; + BaseIO::makeAbsolutePath(basePath, filename); res = factory->getVisualizationFromFile(filename); } break; @@ -221,7 +227,7 @@ namespace VirtualRobot return res; } - RobotNodePtr SimoxURDFFactory::createBodyNode(RobotPtr robo, boost::shared_ptr<Link> urdfBody, bool useColModelsIfNoVisuModel) + RobotNodePtr SimoxURDFFactory::createBodyNode(RobotPtr robo, boost::shared_ptr<Link> urdfBody, const std::string &basePath, bool useColModelsIfNoVisuModel) { const float scale = 1000.0f; // mm RobotNodePtr result; @@ -241,12 +247,12 @@ namespace VirtualRobot if (urdfBody->visual && urdfBody->visual) { - rnVisu = convertVisu(urdfBody->visual->geometry, urdfBody->visual->origin); + rnVisu = convertVisu(urdfBody->visual->geometry, urdfBody->visual->origin, basePath); } if (urdfBody->collision && urdfBody->collision) { - VisualizationNodePtr v = convertVisu(urdfBody->collision->geometry, urdfBody->collision->origin); + VisualizationNodePtr v = convertVisu(urdfBody->collision->geometry, urdfBody->collision->origin, basePath); if (v) { diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.h b/VirtualRobot/Import/URDF/SimoxURDFFactory.h index 36d822621a5b58a32a73afab5b3c0b9531dd5e22..79c1967589592c51ffc9fd7d4513a2958f025fb2 100644 --- a/VirtualRobot/Import/URDF/SimoxURDFFactory.h +++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.h @@ -59,7 +59,7 @@ namespace VirtualRobot \param urdfModel The model \param useColModelsIfNoVisuModel If set, a missing visualization is compensated by using the collision model (e.g. when the visu loading failed) */ - RobotPtr createRobot(boost::shared_ptr<urdf::ModelInterface> urdfModel, bool useColModelsIfNoVisuModel = true); + RobotPtr createRobot(boost::shared_ptr<urdf::ModelInterface> urdfModel, const std::string& basePath, bool useColModelsIfNoVisuModel = true); // AbstractFactoryMethod public: @@ -76,10 +76,10 @@ namespace VirtualRobot protected: - RobotNodePtr createBodyNode(RobotPtr robot, boost::shared_ptr<urdf::Link> urdfBody, bool useColModelsIfNoVisuModel = true); + RobotNodePtr createBodyNode(RobotPtr robot, boost::shared_ptr<urdf::Link> urdfBody, const std::string& basePath, bool useColModelsIfNoVisuModel = true); RobotNodePtr createJointNode(RobotPtr robot, boost::shared_ptr<urdf::Joint> urdfJoint); Eigen::Matrix4f convertPose(urdf::Pose& p); - VirtualRobot::VisualizationNodePtr convertVisu(boost::shared_ptr<urdf::Geometry> g, urdf::Pose& pose); + VirtualRobot::VisualizationNodePtr convertVisu(boost::shared_ptr<urdf::Geometry> g, urdf::Pose& pose, const std::string& basePath); std::string getFilename(const std::string& f); bool useColModelsIfNoVisuModel; diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp index 8d048ee192c5ce23738a1bb6fe4f1ae5a18e529f..cc9bb598e1c4685a4c3096fe50d7185fb779f08c 100644 --- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp +++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.cpp @@ -18,6 +18,7 @@ #include <Inventor/actions/SoWriteAction.h> #include <Inventor/actions/SoToVRML2Action.h> #include <Inventor/VRMLnodes/SoVRMLGroup.h> +#include <Inventor/nodes/SoRotation.h> namespace VirtualRobot { @@ -165,6 +166,15 @@ namespace VirtualRobot SoSeparator* root = new SoSeparator; root->ref(); + SoRotation* rotationX = new SoRotation(); + rotationX->ref(); + rotationX->rotation.setValue( SbVec3f(-1,0,0), M_PI/2); + SoRotation* rotationZ = new SoRotation(); + rotationZ->ref(); + rotationZ->rotation.setValue( SbVec3f(0,0,1), M_PI); + + root->addChild(rotationX); + root->addChild(rotationZ); root->addChild(this->getCoinVisualization()); printf("Converting...\n"); @@ -173,6 +183,8 @@ namespace VirtualRobot SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph(); newroot->ref(); root->unref(); + rotationZ->unref(); + rotationX->unref(); printf("Writing...\n"); diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp index 5f06e46d2127f7f610fbdc8615422a07f611653c..74af47212475fee50d28ad76b9ccd5a9a12947ef 100644 --- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp +++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp @@ -4,6 +4,7 @@ #include "VirtualRobot/Workspace/Reachability.h" #include <VirtualRobot/RuntimeEnvironment.h> #include <VirtualRobot/Import/RobotImporterFactory.h> +#include <boost/algorithm/string/predicate.hpp> #include <QFileDialog> #include <Eigen/Geometry> @@ -396,6 +397,8 @@ void showRobotWindow::exportVRML() if (!s.empty()) { + if (!boost::algorithm::ends_with(s, ".wrl")) + s += ".wrl"; SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full; visualization = robot->getVisualization<CoinVisualization>(colModel); visualization->exportToVRML2(s);