environment_util.cpp

//
// Created by sebastian on 19.05.20.
//
#include <ros/ros.h>

#include "environment_util.h"

void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {

    moveit_msgs::CollisionObject plate;

    plate.header.frame_id = "panda_link0";
    plate.id = "plate";

    plate.primitives.resize(1);
    plate.primitives[0].type = 1u;
    plate.primitives[0].dimensions.resize(3);
    plate.primitives[0].dimensions[0] = x_dimension;
    plate.primitives[0].dimensions[1] = y_dimension;

    plate.primitives[0].dimensions[2] = 0.1;
    plate.primitive_poses.resize(1);
    plate.primitive_poses[0].position.x = 0;
    plate.primitive_poses[0].position.y = 0;
    plate.primitive_poses[0].position.z = -0.1;

    plate.operation = plate.ADD;

    collision_objects.push_back(plate);
}

void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
                      double object_x_dimension, double object_y_dimension, double object_z_dimension,
                      geometry_msgs::Pose &object_to_pick_pose) {

    moveit_msgs::CollisionObject pick_support;

    pick_support.header.frame_id = "panda_link0";
    pick_support.id = id;

    pick_support.primitives.resize(1);
    pick_support.primitives[0].type = collision_objects[1].primitives[0].BOX;
    pick_support.primitives[0].dimensions.resize(3);
    pick_support.primitives[0].dimensions[0] = object_x_dimension;
    pick_support.primitives[0].dimensions[1] = object_y_dimension;
    pick_support.primitives[0].dimensions[2] = 0.04;

    pick_support.primitive_poses.resize(1);
    pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x;
    pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y;
    pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02);

    pick_support.operation = pick_support.ADD;

    collision_objects.push_back(pick_support);
}

void EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects,
        std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions){

    moveit_msgs::CollisionObject object_to_pick;

    object_to_pick.header.frame_id = "panda_link0";
    object_to_pick.id = id;

    object_to_pick.primitives.resize(1);
    object_to_pick.primitives[0].type = object_to_pick.primitives[0].BOX;
    object_to_pick.primitives[0].dimensions.resize(3);
    object_to_pick.primitives[0].dimensions[0] = dimensions.x;
    object_to_pick.primitives[0].dimensions[1] = dimensions.y;
    object_to_pick.primitives[0].dimensions[2] = dimensions.z;

    object_to_pick.primitive_poses.resize(1);
    object_to_pick.primitive_poses[0].position = pose.position;
    object_to_pick.primitive_poses[0].orientation = pose.orientation;

    object_to_pick.operation = object_to_pick.ADD;

    collision_objects.push_back(object_to_pick);
}