47 std_msgs::ColorRGBA ret;
49 std::random_device rd_;
50 std::mt19937 gen(rd_());
51 std::uniform_real_distribution<> dis(0.0, 1.0);
52 ret.r =
static_cast<float>(dis(gen));
53 ret.g =
static_cast<float>(dis(gen));
54 ret.b =
static_cast<float>(dis(gen));
59 const Eigen::Ref<const Eigen::MatrixXd> mat)
62 myfile.open(file_name);
75 void LoadOBJ(
const std::string& data, Eigen::VectorXi& tri,
76 Eigen::VectorXd& vert)
78 std::stringstream ss(data);
85 while (std::getline(ss, line))
87 if (line.compare(0, 2,
"v ") == 0)
89 vert.conservativeResize((vn + 1) * 3);
90 std::stringstream sss(line.substr(2));
91 sss >> v[0] >> v[1] >> v[2];
93 vert(vn * 3 + 1) = v[1];
94 vert(vn * 3 + 2) = v[2];
97 else if (line.compare(0, 2,
"f ") == 0)
99 std::stringstream sss(line.substr(2));
101 for (i = 0; i < 9 && sss >> vv[i]; ++i)
103 while (sss.peek() ==
'/' || sss.peek() ==
' ')
110 tri.conservativeResize((tn + 1) * 3);
111 tri(tn * 3) = vv[0] - 1;
112 tri(tn * 3 + 1) = vv[3] - 1;
113 tri(tn * 3 + 2) = vv[6] - 1;
119 std::shared_ptr<octomap::OcTree>
LoadOctree(
const std::string& file_path)
121 std::shared_ptr<octomap::OcTree> octree(
new octomap::OcTree(file_path));
122 if (!octree)
ThrowPretty(
"Could not load OcTree!");
128 std::shared_ptr<octomap::OcTree> octree =
LoadOctree(file_path);
139 temp = abi::__cxa_demangle(type.name(), 0, 0, &status);
140 name = std::string(temp);
147 std::string ret = path;
149 std::regex_search(ret, matches, std::regex(
"\\{([^\\}]+){1,}\\}"));
150 for (
auto& match : matches)
152 std::string
package = match.str();
158 ret = std::regex_replace(ret, std::regex(
"\\{" +
package +
"\\}"), package_path, std::regex_constants::match_any);
160 catch (
const std::regex_error& e)
162 ThrowPretty(
"Package name resolution failed (regex error " << e.code() <<
")");
165 std::regex_search(ret, matches, std::regex(
"package://([^\\/]+){1,}"));
166 for (
auto& match : matches)
168 std::string
package = match.str();
169 if (
package.substr(0, 10) ==
"package://" ||
package ==
"")
continue;
174 ret = std::regex_replace(ret, std::regex(
"package://" +
package +
"/"), package_path +
"/", std::regex_constants::match_any);
176 catch (
const std::regex_error& e)
178 ThrowPretty(
"Package name resolution failed (regex error " << e.code() <<
")");
187 std::ifstream fstream(file_name);
188 if (!fstream)
ThrowPretty(
"File does not exist '" << file_name <<
"'");
191 return std::string((std::istreambuf_iterator<char>(fstream)), std::istreambuf_iterator<char>());
193 catch (
const std::ifstream::failure& e)
195 ThrowPretty(
"Can't read file '" << file_name <<
"'");