Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit 430233a

Browse files
markarenmarkaren
committedJun 25, 2024·
update
1 parent d7c87b3 commit 430233a

File tree

2 files changed

+49
-34
lines changed

2 files changed

+49
-34
lines changed
 

‎include/threepp/objects/Robot.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -51,9 +51,9 @@ namespace threepp {
5151
}
5252

5353
void finalize() {
54-
for (int i = 0; i < joints_.size(); i++) {
55-
auto info = jointInfos_[i];
56-
auto joint = joints_[i];
54+
for (auto i = 0; i < joints_.size(); i++) {
55+
const auto info = jointInfos_[i];
56+
const auto joint = joints_[i];
5757

5858
auto parent = std::ranges::find_if(links_, [&](auto link) {
5959
return link->name == info.parent;
@@ -105,7 +105,7 @@ namespace threepp {
105105
std::vector<JointInfo> jointInfos_;
106106
std::vector<std::shared_ptr<Object3D>> links_;
107107
std::vector<std::shared_ptr<Object3D>> joints_;
108-
std::unordered_map<int, std::pair<Object3D*, JointInfo>> articulatedJoints_;
108+
std::unordered_map<size_t, std::pair<Object3D*, JointInfo>> articulatedJoints_;
109109
};
110110

111111
}// namespace threepp

‎src/threepp/loaders/URDFLoader.cpp

+45-30
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,8 @@
22
#include "threepp/loaders/URDFLoader.hpp"
33

44
#include "pugixml.hpp"
5-
#include "threepp/materials/MeshBasicMaterial.hpp"
5+
#include "threepp/materials/MeshStandardMaterial.hpp"
6+
#include "threepp/math/MathUtils.hpp"
67
#include "threepp/objects/Group.hpp"
78
#include "threepp/objects/Mesh.hpp"
89
#include "threepp/utils/StringUtils.hpp"
@@ -38,6 +39,25 @@ namespace {
3839
-utils::parseFloat(xyz[2]));
3940
}
4041

42+
std::shared_ptr<Material> getMaterial(const pugi::xml_node& node) {
43+
const auto color = node.child("color");
44+
const auto diffuse = color.attribute("rgba").value();
45+
const auto diffuseArray = utils::split(diffuse, ' ');
46+
47+
const auto mtl = MeshStandardMaterial::create();
48+
mtl->color.setRGB(
49+
utils::parseFloat(diffuseArray[0]),
50+
utils::parseFloat(diffuseArray[1]),
51+
utils::parseFloat(diffuseArray[2]));
52+
53+
if (float alpha = utils::parseFloat(diffuseArray[3]) < 1) {
54+
mtl->transparent = true;
55+
mtl->opacity = alpha;
56+
}
57+
58+
return mtl;
59+
}
60+
4161
JointType getType(const std::string& type) {
4262
if (type == "revolute") {
4363
return JointType::Revolute;
@@ -78,7 +98,7 @@ namespace {
7898

7999
//find parent path with package.xml
80100
bool packageFound = false;
81-
auto packagePath = basePath.parent_path();
101+
auto packagePath = basePath;
82102
for (int i = 0; i < 10; ++i) {
83103
if (exists(packagePath / "package.xml")) {
84104
packageFound = true;
@@ -93,7 +113,7 @@ namespace {
93113
return packagePath.parent_path().string() + "/" + fileName;
94114
}
95115

96-
return fileName;
116+
return basePath / fileName;
97117
}
98118

99119
}// namespace
@@ -110,55 +130,50 @@ struct URDFLoader::Impl {
110130
const auto root = doc.child("robot");
111131
if (!root) return nullptr;
112132

113-
114133
auto robot = std::make_shared<Robot>();
115134
robot->name = root.attribute("name").as_string("robot");
116135

117136
for (const auto link : root.children("link")) {
118137

119-
auto linkObject = std::make_shared<Object3D>();
138+
const auto linkObject = std::make_shared<Object3D>();
120139
linkObject->name = link.attribute("name").value();
121140

122141
for (const auto visual : link.children("visual")) {
123142

124-
auto visualObject = std::make_shared<Object3D>();
125-
visualObject->name = visual.attribute("name").value();
143+
if (const auto geometry = visual.child("geometry")) {
144+
145+
const auto mesh = geometry.child("mesh");
146+
const auto fileName = getModelPath(path.parent_path(), mesh.attribute("filename").value());
147+
148+
if (auto visualObject = loader.load(fileName)) {
149+
visualObject->name = visual.attribute("name").value();
150+
visualObject->position.copy(getXYZ(visual));
151+
visualObject->rotation.copy(getRPY(visual));
126152

127-
visualObject->position.copy(getXYZ(visual));
128-
visualObject->rotation.copy(getRPY(visual));
153+
if (const auto material = visual.child("material")) {
129154

130-
for (const auto geometry : visual.children("geometry")) {
155+
const auto mtl = getMaterial(material);
131156

132-
auto mesh = geometry.child("mesh");
133-
auto fileName = getModelPath(path, mesh.attribute("filename").value());
157+
visualObject->traverseType<Mesh>([&](Mesh& mesh) {
158+
mesh.setMaterial(mtl);
159+
});
160+
}
134161

135-
if (auto load = loader.load(fileName)) {
136-
visualObject->add(load);
162+
if (fileName.extension().string() == ".stl" || fileName.extension().string() == ".STL") {
163+
visualObject->rotateX(-math::PI / 2);
164+
}
165+
166+
linkObject->add(visualObject);
137167
}
138168
}
139-
140-
// for (auto material : visual.children("material")) {
141-
//
142-
// auto color = material.child("color");
143-
// auto diffuse = color.attribute("rgba").value();
144-
// auto diffuseArray = utils::split(diffuse, ' ');
145-
//
146-
// auto mtl = MeshBasicMaterial::create();
147-
// mtl->color.setRGB(
148-
// utils::parseFloat(diffuseArray[0]),
149-
// utils::parseFloat(diffuseArray[1]),
150-
// utils::parseFloat(diffuseArray[2]));
151-
// }
152-
153-
linkObject->add(visualObject);
154169
}
155170

156171
robot->addLink(linkObject);
157172
}
158173

159174
for (const auto joint : root.children("joint")) {
160175

161-
auto jointObject = std::make_shared<Object3D>();
176+
const auto jointObject = std::make_shared<Object3D>();
162177
jointObject->name = joint.attribute("name").value();
163178

164179
jointObject->position.copy(getXYZ(joint));

0 commit comments

Comments
 (0)
Please sign in to comment.