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