From 23b01f46575cc0f2a875128b45f737ba23594bdf Mon Sep 17 00:00:00 2001 From: Splinter1984 Date: Sat, 20 May 2023 16:33:23 +0200 Subject: [PATCH] no matter where is mesh files! --- rbs_simulation/worlds/mir.sdf | 32 +++++------ .../src/moveit_update_planning_scene.cpp | 56 ++++++++++++++++--- 2 files changed, 63 insertions(+), 25 deletions(-) diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index 4b399ec..6adc35d 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -103,7 +103,7 @@ 0.35 0.35 0.35 - package://rbs_simulation/rack/model/rack.dae + model://rack/model/rack.dae @@ -111,7 +111,7 @@ 0.35 0.35 0.35 - package://rbs_simulation/rack/model/rack.stl + model://rack/model/rack.stl 10 @@ -141,7 +141,7 @@ 0.8 0.8 0.4 @@ -157,7 +157,7 @@ 0.8 0.8 0.4 @@ -183,62 +183,62 @@ - package://rbs_simulation/box1 + model://box1 box1 0.25 0.65 0.6515 0 0 0 - package://rbs_simulation/box1 + model://box1 box2 0 0.65 0.6515 0 0 0 - package://rbs_simulation/box1 + model://box1 box3 -0.25 0.65 0.6515 0 0 0 - package://rbs_simulation/box1 + model://box1 box4 0.25 0.65 0.3015 0 0 0 - package://rbs_simulation/box1 + model://box1 box5 0 0.65 0.3015 0 0 0 - package://rbs_simulation/box1 + model://box1 box6 -0.25 0.65 0.3015 0 0 0 diff --git a/rbs_skill_servers/src/moveit_update_planning_scene.cpp b/rbs_skill_servers/src/moveit_update_planning_scene.cpp index 49b8f2b..366bf35 100644 --- a/rbs_skill_servers/src/moveit_update_planning_scene.cpp +++ b/rbs_skill_servers/src/moveit_update_planning_scene.cpp @@ -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 &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 &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 get_objects(const sdf::World *world) +static std::vector +get_objects(const sdf::World *world, const std::string &model_resources) { + std::vector 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 result; auto models_count = world->ModelCount(); @@ -120,7 +158,7 @@ static std::vector 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; }