diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index bbc6341..bdfed36 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/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 ] diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index 3a24e5a..4b399ec 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -103,7 +103,7 @@ 0.35 0.35 0.35 - model://rack/model/rack.dae + package://rbs_simulation/rack/model/rack.dae @@ -111,7 +111,7 @@ 0.35 0.35 0.35 - model://rack/model/rack.stl + package://rbs_simulation/rack/model/rack.stl 10 @@ -141,7 +141,7 @@ 0.8 0.8 0.4 @@ -157,7 +157,7 @@ 0.8 0.8 0.4 @@ -183,62 +183,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_interfaces/CMakeLists.txt b/rbs_skill_interfaces/CMakeLists.txt index aeb90e8..e4fd403 100644 --- a/rbs_skill_interfaces/CMakeLists.txt +++ b/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( 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 ) diff --git a/rbs_skill_interfaces/package.xml b/rbs_skill_interfaces/package.xml index c3a746a..74fb6eb 100644 --- a/rbs_skill_interfaces/package.xml +++ b/rbs_skill_interfaces/package.xml @@ -14,6 +14,7 @@ std_msgs geometry_msgs + shape_msgs ament_lint_auto ament_lint_common diff --git a/rbs_skill_interfaces/srv/PlanningSceneObjectState.srv b/rbs_skill_interfaces/srv/PlanningSceneObjectState.srv new file mode 100644 index 0000000..be617e2 --- /dev/null +++ b/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 diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index 3f442ae..7163af5 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/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} diff --git a/rbs_skill_servers/package.xml b/rbs_skill_servers/package.xml index 3eb215d..d69f792 100644 --- a/rbs_skill_servers/package.xml +++ b/rbs_skill_servers/package.xml @@ -17,6 +17,7 @@ tf2_ros rclcpp_action geometry_msgs + geometric_shapes action_msgs rbs_skill_interfaces tf2_eigen diff --git a/rbs_skill_servers/src/moveit_update_planning_scene.cpp b/rbs_skill_servers/src/moveit_update_planning_scene.cpp new file mode 100644 index 0000000..116ba1e --- /dev/null +++ b/rbs_skill_servers/src/moveit_update_planning_scene.cpp @@ -0,0 +1,211 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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(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 get_objects(const sdf::World *world) +{ + std::vector 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( + 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 request, + std::shared_ptr response) + { + auto state = static_cast(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::SharedPtr service_; + moveit::planning_interface::PlanningSceneInterface planning_scene_; +}; + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(UpdatePlanningSceneServer);