//==============================================================================
dynamics::SkeletonPtr readSkeleton(
const common::Uri& fileUri,
const common::ResourceRetrieverPtr& nullOrRetriever)
{
const auto retriever = getRetriever(nullOrRetriever);
//--------------------------------------------------------------------------
// Load xml and create Document
tinyxml2::XMLDocument _dartFile;
try
{
openXMLFile(_dartFile, fileUri, retriever);
}
catch(std::exception const& e)
{
dtwarn << "[SdfParser::readSkeleton] Loading file [" << fileUri.toString()
<< "] failed: " << e.what() << "\n";
return nullptr;
}
//--------------------------------------------------------------------------
// Load sdf
tinyxml2::XMLElement* sdfElement = nullptr;
sdfElement = _dartFile.FirstChildElement("sdf");
if (sdfElement == nullptr)
return nullptr;
//--------------------------------------------------------------------------
// version attribute
std::string version = getAttributeString(sdfElement, "version");
// TODO: We need version aware SDF parser (see #264)
// We support 1.4 only for now.
if (version != "1.4" && version != "1.5")
{
dtwarn << "[SdfParser::readSdfFile] The file format of ["
<< fileUri.toString() << "] was found to be [" << version
<< "], but we only support SDF 1.4 and 1.5!\n";
return nullptr;
}
//--------------------------------------------------------------------------
// Load skeleton
tinyxml2::XMLElement* skelElement = nullptr;
skelElement = sdfElement->FirstChildElement("model");
if (skelElement == nullptr)
return nullptr;
std::string fileName = fileUri.getFilesystemPath(); // Uri's path is unix-style path
std::string skelPath = fileName.substr(0, fileName.rfind("/") + 1);
dynamics::SkeletonPtr newSkeleton = readSkeleton(skelElement, skelPath,
retriever);
return newSkeleton;
}
virtual void StartEndpoints(const std::vector<EndpointDescription>& endpoints, OpcUa::Services::SharedPtr server) override
{
for (const EndpointDescription endpoint : endpoints)
{
const Common::Uri uri(endpoint.EndpointURL);
if (uri.Scheme() == "opc.tcp")
{
std::shared_ptr<IncomingConnectionProcessor> processor(new OpcTcp(server, Debug));
TcpParameters tcpParams;
tcpParams.Port = uri.Port();
if (Debug) std::clog << "opc_tcp_processor| Starting listen port " << tcpParams.Port << std::endl;
TcpAddon.Listen(tcpParams, processor);
Ports.push_back(tcpParams);
}
}
}
void OpcUaProtocol::StartEndpoints(std::vector<EndpointDescription> endpoints, Common::AddonsManager& addons)
{
InternalServer = addons.GetAddon<OpcUa::UaServer::ServicesRegistryAddon>(OpcUa::UaServer::ServicesRegistryAddonID);
TcpAddon = addons.GetAddon<OpcUa::UaServer::TcpServerAddon>(OpcUa::UaServer::TcpServerAddonID);
for (const EndpointDescription endpoint : endpoints)
{
const Common::Uri uri(endpoint.EndpointURL);
if (uri.Scheme() == "opc.tcp")
{
std::shared_ptr<IncomingConnectionProcessor> processor = OpcUa::Internal::CreateOpcTcpProcessor(InternalServer->GetServer(), Debug);
TcpParameters tcpParams;
tcpParams.Port = uri.Port();
if (Debug) std::clog << "Starting listen port " << tcpParams.Port << std::endl;
TcpAddon->Listen(tcpParams, processor);
Ports.push_back(tcpParams.Port);
}
}
}
dynamics::SkeletonPtr DartLoader::parseSkeleton(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
std::string content;
if (!readFileToString(resourceRetriever, _uri, content))
return nullptr;
// Use urdfdom to load the URDF file.
const ModelInterfacePtr urdfInterface = urdf::parseURDF(content);
if(!urdfInterface)
{
dtwarn << "[DartLoader::readSkeleton] Failed loading URDF file '"
<< _uri.toString() << "'.\n";
return nullptr;
}
return modelInterfaceToSkeleton(urdfInterface.get(), _uri, resourceRetriever);
}
dynamics::ShapePtr DartLoader::createShape(
const VisualOrCollision* _vizOrCol,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
dynamics::ShapePtr shape;
// Sphere
if(urdf::Sphere* sphere = dynamic_cast<urdf::Sphere*>(_vizOrCol->geometry.get()))
{
shape = dynamics::ShapePtr(new dynamics::EllipsoidShape(
2.0 * sphere->radius * Eigen::Vector3d::Ones()));
}
// Box
else if(urdf::Box* box = dynamic_cast<urdf::Box*>(_vizOrCol->geometry.get()))
{
shape = dynamics::ShapePtr(new dynamics::BoxShape(
Eigen::Vector3d(box->dim.x, box->dim.y, box->dim.z)));
}
// Cylinder
else if(urdf::Cylinder* cylinder = dynamic_cast<urdf::Cylinder*>(_vizOrCol->geometry.get()))
{
shape = dynamics::ShapePtr(new dynamics::CylinderShape(
cylinder->radius, cylinder->length));
}
// Mesh
else if(urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(_vizOrCol->geometry.get()))
{
// Resolve relative URIs.
common::Uri relativeUri, absoluteUri;
if(!absoluteUri.fromRelativeUri(_baseUri, mesh->filename))
{
dtwarn << "[DartLoader::createShape] Failed resolving mesh URI '"
<< mesh->filename << "' relative to '" << _baseUri.toString()
<< "'.\n";
return nullptr;
}
// Load the mesh.
const std::string resolvedUri = absoluteUri.toString();
const aiScene* scene = dynamics::MeshShape::loadMesh(
resolvedUri, _resourceRetriever);
if (!scene)
return nullptr;
const Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
shape = Eigen::make_aligned_shared<dynamics::MeshShape>(
scale, scene, resolvedUri, _resourceRetriever);
}
// Unknown geometry type
else
{
dtwarn << "[DartLoader::createShape] Unknown URDF Shape type "
<< "(we only know of Sphere, Box, Cylinder, and Mesh). "
<< "We are returning a nullptr." << std::endl;
return nullptr;
}
shape->setLocalTransform(toEigen(_vizOrCol->origin));
setMaterial(shape, _vizOrCol);
return shape;
}
