Add minimal pick-and-place case with world embedded

rbs_skill_servers/src/add_planning_scene_objects_service.cpp

@@ -0,0 +1,71 @@
+#include <cinttypes>
+#include <memory>
+#include <algorithm>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
+#include "moveit/move_group_interface/move_group_interface.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject;
+rclcpp::Node::SharedPtr g_node = nullptr;
+
+void handle_service(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<AddPlanningSceneObject::Request> request,
+    const std::shared_ptr<AddPlanningSceneObject::Response> response)
+{
+    (void)request_header;
+    // Create collision object for the robot to avoid
+    auto const collision_object = [frame_id =
+                                    "world", object_name=request->object_id, object_pose=request->object_pose] {
+    moveit_msgs::msg::CollisionObject collision_object;
+    collision_object.header.frame_id = frame_id;
+    collision_object.id = object_name;
+    shape_msgs::msg::SolidPrimitive primitive;
+
+    // Define the size of the box in meters
+    primitive.type = primitive.BOX;
+    primitive.dimensions.resize(3);
+    primitive.dimensions[primitive.BOX_X] = 0.05;
+    primitive.dimensions[primitive.BOX_Y] = 0.05;
+    primitive.dimensions[primitive.BOX_Z] = 0.05;
+
+    // Define the pose of the box (relative to the frame_id)
+    geometry_msgs::msg::Pose box_pose;
+    box_pose.position.x = object_pose[0];
+    box_pose.position.y = object_pose[1];
+    box_pose.position.z = object_pose[2];
+    box_pose.orientation.x = object_pose[3];
+    box_pose.orientation.y = object_pose[4];
+    box_pose.orientation.z = object_pose[5];
+    box_pose.orientation.w = object_pose[6];
+    
+
+
+    collision_object.primitives.push_back(primitive);
+    collision_object.primitive_poses.push_back(box_pose);
+    collision_object.operation = collision_object.ADD;
+
+    return collision_object;
+    }();
+
+    // Add the collision object to the scene
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    planning_scene_interface.applyCollisionObject(collision_object);
+
+    response->success = true;
+}
+
+int main(int argc, char ** argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::NodeOptions node_options;
+    node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
+    g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
+    auto server = g_node->create_service<AddPlanningSceneObject>("add_planing_scene_object_service", handle_service);
+    rclcpp::spin(g_node);
+    rclcpp::shutdown();
+    g_node = nullptr;
+    return 0;
+}