71 lines
No EOL
2.6 KiB
C++
71 lines
No EOL
2.6 KiB
C++
#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;
|
|
} |