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

fix launch errors after merge

Browse source
This commit is contained in:
Ilya Uraev 2023-07-31 12:05:05 +03:00
parent 0bcfa40750
commit b02e3db31f
3 changed files with 64 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

56
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

@ -37,7 +37,26 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p
return result;
}
static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
static std::string get_correct_mesh_path(
const std::string& uri, const std::vector<std::string> &resources)
{
std::string result = "";
std::regex reg(R"((?:model|package)(?:\:\/)(.*))");
std::smatch m;
if (std::regex_match(uri, m, reg))
{
std::string rel_path = m[1];
std::for_each(resources.begin(), resources.end(),
[&result, &rel_path](const std::string& res){
if (result.empty())
result = std::filesystem::exists(res + rel_path)? std::string(res + rel_path): result;
});
}
return "file://" + result;
}
static moveit_msgs::msg::CollisionObject
get_object(const sdf::Model *model, const std::vector<std::string> &resources)
{
moveit_msgs::msg::CollisionObject obj;
obj.header.frame_id = "world";
@ -47,14 +66,16 @@ static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
for (size_t i = 0; i < link_count; ++i)
{
auto link = model->LinkByIndex(i);
auto collision = link->CollisionByIndex(0); // NOTE: ok for right now.
auto collision = link->CollisionByIndex(0);
auto link_pose = pose_from_pose3d(link->RawPose());
auto geometry = collision->Geom();
switch(geometry->Type())
{
case sdf::GeometryType::MESH:
{
shapes::Mesh *m = shapes::createMeshFromResource(geometry->MeshShape()->Uri());
auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources);
shapes::Mesh *m = shapes::createMeshFromResource(
!path.empty()? path: geometry->MeshShape()->Uri());
auto scale = geometry->MeshShape()->Scale().X();
m->scale(scale);
@ -96,23 +117,40 @@ static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
obj.primitive_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::PLANE:
{
auto sdf_plane = geometry->PlaneShape();
shape_msgs::msg::Plane plane;
auto normal = sdf_plane->Normal();
plane.coef[0] = normal.X();
plane.coef[1] = normal.Y();
plane.coef[2] = normal.Z();
obj.planes.push_back(plane);
obj.plane_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::EMPTY:
case sdf::GeometryType::CYLINDER:
case sdf::GeometryType::PLANE:
case sdf::GeometryType::SPHERE:
case sdf::GeometryType::HEIGHTMAP:
case sdf::GeometryType::CAPSULE:
case sdf::GeometryType::ELLIPSOID:
case sdf::GeometryType::POLYLINE:
break; //NOTE: sorry not now
break;
}
}
return obj;
}
static std::vector<moveit_msgs::msg::CollisionObject> get_objects(const sdf::World *world)
static std::vector<moveit_msgs::msg::CollisionObject>
get_objects(const sdf::World *world, const std::string &model_resources)
{
std::vector<std::string> resources;
std::regex reg("\\:+");
std::sregex_token_iterator begin(
model_resources.begin(), model_resources.end(), reg, -1), end;
std::copy(++begin, end, std::back_inserter(resources));
std::vector<moveit_msgs::msg::CollisionObject> result;
auto models_count = world->ModelCount();
@ -120,7 +158,7 @@ static std::vector<moveit_msgs::msg::CollisionObject> get_objects(const sdf::Wor
{
try{
auto model = world->ModelByIndex(i);
result.push_back(get_object(model));
result.push_back(get_object(model, resources));
} catch (std::exception &ex){
std::cerr << ex.what() << std::endl;
}
@ -153,6 +191,7 @@ public:
sdf::Root root;
sdf::ParserConfig config;
config.AddURIPath("package://", model_resources);
config.AddURIPath("model://", model_resources);
sdf::Errors errors = root.Load(init_scene, config);
if (!errors.empty())
{
@ -161,8 +200,7 @@ public:
return false;
}
auto world = root.WorldByIndex(0);
auto objects = get_objects(world);
//NOTE: as simple as possible!
auto objects = get_objects(world, model_resources);
planning_scene_.applyCollisionObjects(objects);
return true;
}