fix launch errors after merge

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",
         )
     )

rbs_simulation/worlds/mir.sdf

@@ -124,7 +124,7 @@
           <geometry>
             <mesh>
               <scale>0.35 0.35 0.35</scale>
-              <uri>package://rbs_simulation/rack/model/rack.dae</uri>
+              <uri>model://rack/model/rack.dae</uri>
             </mesh>
           </geometry>
         </visual>
@@ -132,7 +132,7 @@
           <geometry>
             <mesh>
               <scale>0.35 0.35 0.35</scale>
-              <uri>package://rbs_simulation/rack/model/rack.stl</uri>
+              <uri>model://rack/model/rack.stl</uri>
             </mesh>
           </geometry>
           <max_contacts>10</max_contacts>
@@ -162,7 +162,7 @@
           <geometry>
             <!-- <mesh>
               <scale>1 1 1</scale>
-              <uri>package://rbs_simulation/table/model/table.dae</uri>
+              <uri>model://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>package://rbs_simulation/table/model/table.stl</uri>
+              <uri>model://table/model/table.stl</uri>
             </mesh> -->
             <box>
                 <size>0.8 0.8 0.4</size>
@@ -204,62 +204,62 @@
       </link>
     </model>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>box1</name>
       <pose>0.25 0.65 0.6515 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>box2</name>
       <pose>0 0.65 0.6515 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>box3</name>
       <pose>-0.25 0.65 0.6515 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>box4</name>
       <pose>0.25 0.65 0.3015 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>box5</name>
       <pose>0 0.65 0.3015 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>box6</name>
       <pose>-0.25 0.65 0.3015 0 0 0</pose>
     </include> 
     <!-- <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>pyramid1</name>
       <pose>-0.9000 -0.05 0.5250 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>pyramid2</name>
       <pose>-0.9000 0.0 0.5250 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>pyramid3</name>
       <pose>-0.9000 0.05 0.5250 0 0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>pyramid4</name>
       <pose>-0.9 -0.0243 0.5750 0 -0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://box1</uri>
       <name>pyramid5</name>
       <pose>-0.9 0.0260 0.5750 0 -0 0</pose>
     </include>
     <include>
-      <uri>package://rbs_simulation/box1</uri>
+      <uri>model://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,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<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";
@@ -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<moveit_msgs::msg::CollisionObject> get_objects(const sdf::World *world)
+static std::vector<moveit_msgs::msg::CollisionObject> 
+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();
 
@@ -120,7 +158,7 @@ static std::vector<moveit_msgs::msg::CollisionObject> 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);
+      auto objects = get_objects(world, model_resources);
-      //NOTE: as simple as possible!
       planning_scene_.applyCollisionObjects(objects);
       return true;
     }