robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

Multi-Robot Setup

Browse source
This commit is contained in:
Ilya Uraev 2024-04-18 13:29:36 +00:00 • committed by Igor Brylyov
parent bc48e0c35a
commit d5e7768d90
45 changed files with 4519 additions and 861 deletions
Download patch file Download diff file Expand all files Collapse all files

1
env_manager/env_manager/src/env_manager.cpp
View file

@ -1,5 +1,4 @@
#include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp"
#include <string>
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =

2
env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp
View file

@ -33,7 +33,7 @@ public:
protected:
void onGzPoseSub(const gz::msgs::Pose_V &pose_v);
bool doGzSpawn();
// bool doGzSpawn();
private:
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;

94
env_manager/gz_enviroment/src/gz_enviroment.cpp
View file

@ -153,54 +153,54 @@ std::string GzEnviroment::getGzWorldName() {
return worlds_msg.data(0);
}
bool GzEnviroment::doGzSpawn() {
gz::msgs::EntityFactory req;
gz::msgs::Boolean rep;
bool result;
unsigned int timeout = 5000;
bool executed;
// bool GzEnviroment::doGzSpawn() {
// gz::msgs::EntityFactory req;
// gz::msgs::Boolean rep;
// bool result;
// unsigned int timeout = 5000;
// bool executed;
//
// auto env_models = m_config_loader->getEnvModels();
// for (auto &model : *env_models) {
// std::string sdf_string =
// createGzModelString(model.model_pose, model.model_inertia, model.mass,
// model.mesh_path, model.model_name);
// req.set_sdf(sdf_string);
// executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
// }
// return executed;
// }
auto env_models = m_config_loader->getEnvModels();
for (auto &model : *env_models) {
std::string sdf_string =
createGzModelString(model.model_pose, model.model_inertia, model.mass,
model.mesh_path, model.model_name);
req.set_sdf(sdf_string);
executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
}
return executed;
}
std::string GzEnviroment::createGzModelString(
const std::vector<double> &pose, const std::vector<double> &inertia,
const double &mass, const std::string &mesh_filepath,
const std::string &name) {
std::string model_template =
std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
"<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
"</pose>"
// + "<inertial>"
// + "<inertia>"
// + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
// + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
// + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
// + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
// + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
// + "<izz>" + std::to_string(inertia[5]) + "</izz>"
// + "</inertia>"
// + "<mass>" + std::to_string(mass) + "</mass>"
// + "</inertial>"
+ "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
"<collision name='collision_" + name + "'>" +
"<geometry><mesh><uri>file://" + mesh_filepath +
"</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
"</sdf>";
return model_template;
}
// std::string GzEnviroment::createGzModelString(
// const std::vector<double> &pose, const std::vector<double> &inertia,
// const double &mass, const std::string &mesh_filepath,
// const std::string &name) {
// std::string model_template =
// std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
// "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
// std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
// std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
// std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
// "</pose>"
// // + "<inertial>"
// // + "<inertia>"
// // + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
// // + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
// // + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
// // + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
// // + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
// // + "<izz>" + std::to_string(inertia[5]) + "</izz>"
// // + "</inertia>"
// // + "<mass>" + std::to_string(mass) + "</mass>"
// // + "</inertial>"
// + "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
// mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
// "<collision name='collision_" + name + "'>" +
// "<geometry><mesh><uri>file://" + mesh_filepath +
// "</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
// "</sdf>";
// return model_template;
// }
} // namespace gz_enviroment