v0.1

rbs_simulation/launch/rbs_simulation.launch.py

@@ -434,6 +434,16 @@ def generate_launch_description():
             {'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'}
         ]
     )
+
+    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']}
+        ]
+    )
     
     # add_planning_scene_object = Node(
     #     package="rbs_skill_servers",
@@ -463,6 +473,7 @@ def generate_launch_description():
         move_joint_state_action_server,
         grasp_pose_loader,
         assemble_state,
+        moveit_planning_scene_init,
         #add_planning_scene_object
     ]
 

rbs_simulation/worlds/mir.sdf

@@ -103,7 +103,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>
@@ -111,7 +111,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>
@@ -141,7 +141,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>
@@ -157,7 +157,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>
@@ -183,62 +183,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> 
     <!-- <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_interfaces/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(moveit_msgs REQUIRED)
+find_package(shape_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -26,7 +27,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "srv/AssembleState.srv"
     "srv/GetPickPlacePoses.srv"
     "srv/AddPlanningSceneObject.srv"
-    DEPENDENCIES std_msgs geometry_msgs moveit_msgs
+    "srv/PlanningSceneObjectState.srv"
+    DEPENDENCIES std_msgs geometry_msgs moveit_msgs shape_msgs
 )
 
 

rbs_skill_interfaces/package.xml

@@ -14,6 +14,7 @@
 
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>shape_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rbs_skill_interfaces/srv/PlanningSceneObjectState.srv

@@ -0,0 +1,15 @@
+# ADD - add new object to planning scene
+uint8 ADD=0
+# REMOVE - remove object from planning scene
+uint8 REMOVE=1
+# UPDATE - update object positions or geometry
+uint8 UPDATE=2
+
+string frame_name
+shape_msgs/Mesh mesh
+geometry_msgs/PoseStamped pose
+uint8  req_kind
+
+---
+bool call_status
+string error_msg

rbs_skill_servers/CMakeLists.txt

@@ -33,6 +33,38 @@ find_package(tf2_msgs REQUIRED)
 find_package(tinyxml2_vendor REQUIRED)
 find_package(TinyXML2 REQUIRED)
 
+# Default to Fortress
+set(SDF_VER 12)
+
+# If the user didn't specify a GZ distribution, pick the one matching the ROS distribution according to REP 2000
+if(NOT DEFINED ENV{GZ_VERSION} AND DEFINED ENV{ROS_DISTRO})
+  if("$ENV{ROS_DISTRO}" STREQUAL "humble")
+    set(ENV{GZ_VERSION} "fortress")
+  endif()
+endif()
+
+# Find libsdformat matching the picked GZ distribution
+if("$ENV{GZ_VERSION}" STREQUAL "fortress")
+  find_package(sdformat12 REQUIRED)
+  set(SDF_VER ${sdformat12_VERSION_MAJOR})
+  message(STATUS "Compiling against Gazebo Fortress (libSDFormat 12)")
+elseif("$ENV{GZ_VERSION}" STREQUAL "garden")
+  find_package(sdformat13 REQUIRED)
+  set(SDF_VER ${sdformat13_VERSION_MAJOR})
+  message(STATUS "Compiling against Gazebo Garden (libSDFormat 13)")
+# No GZ distribution specified, find any version of libsdformat we can
+else()
+  foreach(major RANGE 13 9)
+    find_package(sdformat${major} QUIET)
+    if(sdformat${major}_FOUND)
+      # Next `find_package` call will be a noop
+      set(SDF_VER ${major})
+      message(STATUS "Compiling against libSDFormat ${major}")
+      break()
+    endif()
+  endforeach()
+endif()
+
 include_directories(SYSTEM
   ${TINYXML2_INCLUDE_DIR}
 )
@@ -55,6 +87,8 @@ set(deps
   tf2_eigen
   tf2_msgs
   tinyxml2_vendor
+  geometric_shapes
+  sdformat${SDF_VER}
 )
 
 if(BUILD_TESTING)
@@ -107,6 +141,15 @@ ament_target_dependencies(assemble_state_server ${deps})
 target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY})
 rclcpp_components_register_node(assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE assemble_state_service_server)
 
+add_library(moveit_update_planning_scene_server SHARED src/moveit_update_planning_scene.cpp)
+target_compile_definitions(moveit_update_planning_scene_server
+  PRIVATE "MOVEIT_UPDATE_PLANNING_SCENE_SERVER_CPP_BUILDING_DLL")
+ament_target_dependencies(moveit_update_planning_scene_server ${deps})
+target_link_libraries(moveit_update_planning_scene_server ${TINYXML2_LIBRARY})
+rclcpp_components_register_node(moveit_update_planning_scene_server PLUGIN "UpdatePlanningSceneServer" EXECUTABLE moveit_update_planning_scene_service_server)
+
+
+
 install(
   DIRECTORY include/
   DESTINATION include
@@ -121,6 +164,7 @@ install(
   move_cartesian_path_action_server
   add_planning_scene_object_service
   assemble_state_server
+  moveit_update_planning_scene_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

rbs_skill_servers/package.xml

@@ -17,6 +17,7 @@
   <depend>tf2_ros</depend>
   <depend>rclcpp_action</depend>
   <depend>geometry_msgs</depend>
+  <depend>geometric_shapes</depend>
   <depend>action_msgs</depend>
   <depend>rbs_skill_interfaces</depend>
   <depend>tf2_eigen</depend>

rbs_skill_servers/src/moveit_update_planning_scene.cpp

@@ -0,0 +1,211 @@
+#include <iostream>
+#include <vector>
+#include <filesystem>
+
+#include <tinyxml2.h>
+#include <geometric_shapes/mesh_operations.h>
+#include <geometric_shapes/shape_operations.h>
+#include <sdf/sdf.hh>
+#include <sdf/World.hh>
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/buffer.h>
+
+#include <moveit_msgs/msg/collision_object.hpp>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp"
+
+#define SERVICE_NAME "update_planning_scene"
+#define INIT_SCENE_PARAM_NAME "init_scene"
+#define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths"
+
+static geometry_msgs::msg::Pose pose_from_pose3d(const gz::math::Pose3d &pose)
+{
+  geometry_msgs::msg::Pose result;
+  auto position = pose.Pos();
+  result.position.set__x(position.X());
+  result.position.set__y(position.Y());
+  result.position.set__z(position.Z());
+  auto orientation = pose.Rot();
+  result.orientation.set__x(orientation.X());
+  result.orientation.set__y(orientation.Y());
+  result.orientation.set__z(orientation.Z());
+  result.orientation.set__w(orientation.W());
+  return result;
+}
+
+static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
+{
+  moveit_msgs::msg::CollisionObject obj;
+  obj.header.frame_id = "world";
+  obj.id = model->Name();
+  obj.pose = pose_from_pose3d(model->RawPose());
+  size_t link_count = model->LinkCount();
+  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 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 scale = geometry->MeshShape()->Scale().X();
+        m->scale(scale);
+
+        Eigen::Vector3d centroid = {0, 0, 0};
+        for (size_t i = 0; i < 3 * m->vertex_count; i += 3)
+        {
+          const auto x = m->vertices[i];
+          const auto y = m->vertices[i+1];
+          const auto z = m->vertices[i+2];
+
+          centroid.x() += x*(1-scale);
+          centroid.y() += y*(1-scale);
+          centroid.z() += z*(1-scale);
+        }
+
+        centroid = centroid / m->vertex_count;
+        for (size_t i = 0; i < 3 * m->vertex_count; i += 3) {
+          m->vertices[i] -= centroid.x();
+          m->vertices[i + 1] -= centroid.y();
+          m->vertices[i + 2] -= centroid.z();
+        }
+
+        shape_msgs::msg::Mesh mesh;
+        shapes::ShapeMsg m_msg;
+        shapes::constructMsgFromShape(m, m_msg);
+        mesh = boost::get<shape_msgs::msg::Mesh>(m_msg);
+        obj.meshes.push_back(mesh);
+        obj.mesh_poses.push_back(link_pose);
+        break;
+      } case sdf::GeometryType::BOX: {
+        auto sdf_box = geometry->BoxShape();
+        shape_msgs::msg::SolidPrimitive box;
+        box.type = shape_msgs::msg::SolidPrimitive::BOX;
+        auto sdf_box_size = sdf_box->Size();
+        box.dimensions.push_back(sdf_box_size.X());
+        box.dimensions.push_back(sdf_box_size.Y());
+        box.dimensions.push_back(sdf_box_size.Z());
+        obj.primitives.push_back(box);
+        obj.primitive_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
+    }
+  }
+
+  return obj;
+}
+
+static std::vector<moveit_msgs::msg::CollisionObject> get_objects(const sdf::World *world)
+{
+  std::vector<moveit_msgs::msg::CollisionObject> result;
+  auto models_count = world->ModelCount();
+
+  for (size_t i = 0; i < models_count; ++i)
+  {
+    try{
+      auto model = world->ModelByIndex(i);
+      result.push_back(get_object(model));
+    } catch (std::exception &ex){
+      std::cerr << ex.what() << std::endl;
+    }
+  }
+
+  return result;
+}
+
+class UpdatePlanningSceneServer: public rclcpp::Node
+{
+public:
+    UpdatePlanningSceneServer(rclcpp::NodeOptions options) 
+    : Node("update_planning_scene_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
+    {
+      std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); 
+      std::string model_resources = get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string();
+
+      if (!scene.empty())
+      {
+        RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str());
+        init_scene(scene, model_resources);
+      }
+      service_ = create_service<rbs_skill_interfaces::srv::PlanningSceneObjectState>(
+        SERVICE_NAME, std::bind(&UpdatePlanningSceneServer::callback, this, 
+          std::placeholders::_1, std::placeholders::_2));
+    }
+
+    bool init_scene(const std::string &init_scene, const std::string &model_resources)
+    {
+      sdf::Root root;
+      sdf::ParserConfig config;
+      config.AddURIPath("package://", model_resources);
+      sdf::Errors errors = root.Load(init_scene, config);
+      if (!errors.empty())
+      {
+        for (auto const &e: errors)
+          RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str());
+        return false;
+      }
+      auto world = root.WorldByIndex(0);
+      auto objects = get_objects(world);
+      //NOTE: as simple as possible!
+      planning_scene_.applyCollisionObjects(objects);
+      return true;
+    }
+
+    void callback(
+      std::shared_ptr<rbs_skill_interfaces::srv::PlanningSceneObjectState::Request> request,
+      std::shared_ptr<rbs_skill_interfaces::srv::PlanningSceneObjectState::Response> response)
+    {
+      auto state = static_cast<UpdatePlanningSceneRequestState>(request->req_kind);
+      moveit_msgs::msg::CollisionObject obj;
+      obj.id = request->frame_name;
+      switch(state)
+      {
+        case ADD:
+        case UPDATE:
+          obj.meshes.resize(1);
+          obj.mesh_poses.resize(1);
+          obj.operation = state == ADD? 
+            moveit_msgs::msg::CollisionObject::ADD: 
+            moveit_msgs::msg::CollisionObject::MOVE;
+          obj.meshes.at(0) = std::move(request->mesh);
+          obj.mesh_poses.at(0) = std::move(request->pose.pose);
+          break;
+        case REMOVE:
+          obj.operation = moveit_msgs::msg::CollisionObject::REMOVE;
+          break;
+      }
+      
+      response->call_status = planning_scene_.applyCollisionObject(obj); 
+    }
+
+private:
+    enum UpdatePlanningSceneRequestState
+    {
+      ADD=0,
+      REMOVE=1,
+      UPDATE=2
+    };
+
+    rclcpp::Service<rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_;  
+    moveit::planning_interface::PlanningSceneInterface planning_scene_;
+};
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(UpdatePlanningSceneServer);