fix building errors

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

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
     ]
 

rbs_simulation/worlds/mir.sdf

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

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(
+static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
-  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";
@@ -66,16 +47,14 @@ get_object(const sdf::Model *model, const std::vector<std::string> &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(geometry->MeshShape()->Uri());
-        shapes::Mesh *m = shapes::createMeshFromResource(
-          !path.empty()? path: 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<std::string> &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<moveit_msgs::msg::CollisionObject> 
+static std::vector<moveit_msgs::msg::CollisionObject> get_objects(const sdf::World *world)
-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();
 
@@ -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;
     }