Skip to content
Snippets Groups Projects
Select Git revision
  • d82db6b778187e7a85b71f0449a7ff83b936ecbb
  • master default protected
  • noetic/main
  • feature/chem-feature-integration
  • feature/tests
5 results

grasp_service.cpp

Blame
  • Johannes Mey's avatar
    Johannes Mey authored
    d82db6b7
    History
    grasp_service.cpp 5.36 KiB
    //
    // 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 "panda_grasping/PickObject.h"
    #include "panda_grasping/PlaceObject.h"
    
    #include "environment_util.h"
    #include "grasp_util.h"
    
    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    #include <tf2_ros/transform_listener.h>
    
    namespace grasping_state
    {
    bool object_picked = false;
    }
    
    /**
     * tf2 buffer to receive current robot state
     */
    tf2_ros::Buffer tfBuffer;
    
    /**
     * Picks a specified object
     * @param req a message containing the object specification
     * @param res reference to the returned data
     * @param planning_scene_interface a valid PlanningSceneInterface instance
     * @param group a initialized MoveGroupInterface instance
     * @param n a valid nodehandle
     */
    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)
    {
      if (!grasping_state::object_picked)
      {
        grasping_state::object_picked = true;
    
        EnvironmentUtil env_util;
        GraspUtil grasp_util;
    
        std::vector<moveit_msgs::CollisionObject> collision_objects;
    
        ROS_INFO("Adding plate.");
        env_util.constructPlate(collision_objects, 3.0, 3.0);
    
        ROS_INFO("Adding pick support.");
        env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
                                  req.pose);
    
        ROS_INFO("Adding object.");
        moveit_msgs::CollisionObject po =
            env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
    
        ROS_INFO("Applying to scene.");
        if (!planning_scene_interface.applyCollisionObjects(collision_objects))
        {
          res.success = false;
          return false;
        }
    
        ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
        geometry_msgs::Transform gripper_transform =
            tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform;
    
        ROS_INFO("Computing pre-grasp-pose.");
        geometry_msgs::Pose pick_pose =
            grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
    
        ROS_INFO("Picking up ...");
        // 0.08 = 8cm = maximum gripper opening
        if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only))
        {
          res.success = false;
          return false;
        }
    
        res.success = true;
        return true;
      }
    
      ROS_ERROR("Error: Cannot pick object. An other object is already picked.");
      res.success = false;
      return false;
    }
    
    /**
     * Places a specified object
     * @param req a message containing the objects target pose specification
     * @param res reference to the returned data
     * @param planning_scene_interface a valid PlanningSceneInterface instance
     * @param group a initialized MoveGroupInterface instance
     * @param n a valid nodehandle
     */
    bool placeObject(panda_grasping::PlaceObject::Request& req, panda_grasping::PlaceObject::Response& res,
                     ros::NodeHandle& n, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
                     moveit::planning_interface::MoveGroupInterface& group)
    {
      ROS_INFO_STREAM("received new place-request: " << req);
    
      if (grasping_state::object_picked)
      {
        EnvironmentUtil env_util;
        GraspUtil grasp_util;
    
        std::vector<moveit_msgs::CollisionObject> collision_objects;
    
        ROS_INFO("Adding place support.");
        env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
                                  req.pose);
    
        ROS_INFO("Applying to scene.");
        if (!planning_scene_interface.applyCollisionObjects(collision_objects))
        {
          res.success = false;
          return false;
        }
    
        ROS_INFO("Placing object.");
        if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only))
        {
          res.success = false;
          return false;
        }
    
        grasping_state::object_picked = false;
        res.success = true;
        return true;
      }
    
      ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper.");
      res.success = false;
      return false;
    }
    
    int main(int argc, char** argv)
    {
      ros::init(argc, argv, "grasp_service");
      ros::NodeHandle n("panda_grasping");
      ros::AsyncSpinner spinner(2);
      spinner.start();
    
      tf2_ros::TransformListener tfListener(tfBuffer);
    
      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<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
              "PlaceObjectService",
              boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
    
      ros::waitForShutdown();
    
      return 0;
    }