runtime/rbs_skill_servers/src/add_planning_scene_objects_service.cpp

#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include <algorithm>
#include <cinttypes>
#include <memory>
#include <moveit/planning_scene_interface/planning_scene_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;
}