grasp_service.cpp

//
// Created by sebastian on 13.05.20.
//
#include "ros/ros.h"
#include <geometry_msgs/Pose.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include "panda_grasping/PickObject.h"
#include "panda_grasping/PlaceObject.h"
#include "environment_util.h"


bool pickObject(panda_grasping::PickObject::Request &req,
                       panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
                       moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
                       moveit::planning_interface::MoveGroupInterface &group) {

    std::cout << "received new pick-request: " << req << std::endl;

    group.setPlanningTime(45.0);

    EnvironmentUtil env_util;
    std::vector<moveit_msgs::CollisionObject> collision_objects;

    ROS_INFO("Adding plate.");
    env_util.constructPlate(collision_objects, 3.0, 3.0);

    ROS_INFO("Adding support.");
    env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z, req.pose);

    ROS_INFO("Applying to scene.");
    planning_scene_interface.applyCollisionObjects(collision_objects);

    res.success = true;
    return true;
}

bool placeObject(panda_grasping::PlaceObject::Request &req,
                       panda_grasping::PlaceObject::Response &res) {

    std::cout << "received: " << req << std::endl;
    return true;
}


int main(int argc, char **argv) {

    ros::init(argc, argv, "grasp_service");
    ros::NodeHandle n("panda_grasping");
   // ros::AsyncSpinner spinner(2);
   // spinner.start();

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>("PickObjectService",
            boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));

    //ros::ServiceServer place_object_service = n.advertiseService("PlaceObjectService", placeObject);

    ROS_INFO("Ready to grasp.");
    ros::spin();

    return 0;
}