From 99452210b83485b0d5d2c6cbf6a8d7419da31a92 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 31 Jul 2023 10:25:09 +0300 Subject: [PATCH] fix building errors --- rbs.repos | 2 +- .../launch/rbs_simulation.launch.py | 15 +++-- rbs_simulation/worlds/mir.sdf | 32 +++++------ .../src/moveit_update_planning_scene.cpp | 56 +++---------------- 4 files changed, 36 insertions(+), 69 deletions(-) diff --git a/rbs.repos b/rbs.repos index b00f7de..ebb7409 100644 --- a/rbs.repos +++ b/rbs.repos @@ -20,6 +20,6 @@ repositories: url: https://github.com/solid-sinusoid/ros2_robotiq_gripper.git version: dev cartesian_controllers: - type: gut + type: git url: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git version: ros2 \ No newline at end of file diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index cc9f23e..910d581 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -465,6 +465,16 @@ def generate_launch_description(): ] ) + moveit_planning_scene_init = Node( + package="rbs_skill_servers", + executable="moveit_update_planning_scene_service_server", + output="screen", + parameters=[ + {'init_scene': world_config_file}, + {'models_paths': os.environ['IGN_GAZEBO_RESOURCE_PATH']} + ] + ) + moveit_planning_scene_init = Node( package="rbs_skill_servers", executable="moveit_update_planning_scene_service_server", @@ -535,11 +545,6 @@ def generate_launch_description(): grasp_pose_loader, assemble_state, moveit_planning_scene_init, - #bridge, - cartesian_motion_controller_spawner, - motion_control_handle_spawner, - cartesian_compliance_controller_spawner, - #pc_filter #add_planning_scene_object ] diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index d658131..61a74df 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -124,7 +124,7 @@ 0.35 0.35 0.35 - model://rack/model/rack.dae + package://rbs_simulation/rack/model/rack.dae @@ -132,7 +132,7 @@ 0.35 0.35 0.35 - model://rack/model/rack.stl + package://rbs_simulation/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 @@ - model://box1 + package://rbs_simulation/box1 box1 0.25 0.65 0.6515 0 0 0 - model://box1 + package://rbs_simulation/box1 box2 0 0.65 0.6515 0 0 0 - model://box1 + package://rbs_simulation/box1 box3 -0.25 0.65 0.6515 0 0 0 - model://box1 + package://rbs_simulation/box1 box4 0.25 0.65 0.3015 0 0 0 - model://box1 + package://rbs_simulation/box1 box5 0 0.65 0.3015 0 0 0 - model://box1 + package://rbs_simulation/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 366bf35..49b8f2b 100644 --- a/rbs_skill_servers/src/moveit_update_planning_scene.cpp +++ b/rbs_skill_servers/src/moveit_update_planning_scene.cpp @@ -37,26 +37,7 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p return result; } -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) +static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model) { moveit_msgs::msg::CollisionObject obj; obj.header.frame_id = "world"; @@ -66,16 +47,14 @@ get_object(const sdf::Model *model, const std::vector &resources) for (size_t i = 0; i < link_count; ++i) { auto link = model->LinkByIndex(i); - auto collision = link->CollisionByIndex(0); + auto collision = link->CollisionByIndex(0); // NOTE: ok for right now. auto link_pose = pose_from_pose3d(link->RawPose()); auto geometry = collision->Geom(); switch(geometry->Type()) { case sdf::GeometryType::MESH: { - auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); - shapes::Mesh *m = shapes::createMeshFromResource( - !path.empty()? path: geometry->MeshShape()->Uri()); + shapes::Mesh *m = shapes::createMeshFromResource(geometry->MeshShape()->Uri()); auto scale = geometry->MeshShape()->Scale().X(); m->scale(scale); @@ -117,40 +96,23 @@ get_object(const sdf::Model *model, const std::vector &resources) 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; + break; //NOTE: sorry not now } } return obj; } -static std::vector -get_objects(const sdf::World *world, const std::string &model_resources) +static std::vector get_objects(const sdf::World *world) { - 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(); @@ -158,7 +120,7 @@ get_objects(const sdf::World *world, const std::string &model_resources) { try{ auto model = world->ModelByIndex(i); - result.push_back(get_object(model, resources)); + result.push_back(get_object(model)); } catch (std::exception &ex){ std::cerr << ex.what() << std::endl; } @@ -191,7 +153,6 @@ 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()) { @@ -200,7 +161,8 @@ public: return false; } auto world = root.WorldByIndex(0); - auto objects = get_objects(world, model_resources); + auto objects = get_objects(world); + //NOTE: as simple as possible! planning_scene_.applyCollisionObjects(objects); return true; }