From b02e3db31f747f59e0b8fcf0d2acde20024078dc Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 31 Jul 2023 12:05:05 +0300 Subject: [PATCH] fix launch errors after merge --- rbs_bringup/launch/bringup.launch.py | 2 +- rbs_simulation/worlds/mir.sdf | 32 +++++------ .../src/moveit_update_planning_scene.cpp | 56 ++++++++++++++++--- 3 files changed, 64 insertions(+), 26 deletions(-) diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py index b4fb4bb..32caa7f 100644 --- a/rbs_bringup/launch/bringup.launch.py +++ b/rbs_bringup/launch/bringup.launch.py @@ -83,7 +83,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "start_joint_controller", - default_value="false", + default_value="true", description="Enable headless mode for robot control", ) ) diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index 61a74df..d658131 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -124,7 +124,7 @@ 0.35 0.35 0.35 - package://rbs_simulation/rack/model/rack.dae + model://rack/model/rack.dae @@ -132,7 +132,7 @@ 0.35 0.35 0.35 - package://rbs_simulation/rack/model/rack.stl + model://rack/model/rack.stl 10 @@ -162,7 +162,7 @@ 0.8 0.8 0.4 @@ -178,7 +178,7 @@ 0.8 0.8 0.4 @@ -204,62 +204,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; }