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

no matter where is mesh files!

Browse source
This commit is contained in:
Splinter1984 2023-05-20 16:33:23 +02:00
parent 23489d7b7a
commit 23b01f4657
2 changed files with 63 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

32
rbs_simulation/worlds/mir.sdf
View file

@ -103,7 +103,7 @@
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>package://rbs_simulation/rack/model/rack.dae</uri>
<uri>model://rack/model/rack.dae</uri>
</mesh>
</geometry>
</visual>
@ -111,7 +111,7 @@
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>package://rbs_simulation/rack/model/rack.stl</uri>
<uri>model://rack/model/rack.stl</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
@ -141,7 +141,7 @@
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>package://rbs_simulation/table/model/table.dae</uri>
<uri>model://table/model/table.dae</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
@ -157,7 +157,7 @@
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>package://rbs_simulation/table/model/table.stl</uri>
<uri>model://table/model/table.stl</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
@ -183,62 +183,62 @@
</link>
</model>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>box1</name>
<pose>0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>box2</name>
<pose>0 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>box4</name>
<pose>0.25 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>box5</name>
<pose>0 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>box6</name>
<pose>-0.25 0.65 0.3015 0 0 0</pose>
</include>
<!-- <include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>pyramid1</name>
<pose>-0.9000 -0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>pyramid2</name>
<pose>-0.9000 0.0 0.5250 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>pyramid3</name>
<pose>-0.9000 0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>pyramid4</name>
<pose>-0.9 -0.0243 0.5750 0 -0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>pyramid5</name>
<pose>-0.9 0.0260 0.5750 0 -0 0</pose>
</include>
<include>
<uri>package://rbs_simulation/box1</uri>
<uri>model://box1</uri>
<name>pyramid6</name>
<pose>-0.9 0 0.6250 0 0 0</pose>
</include> -->

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;
}