Select Git revision
grasp_service.cpp

Johannes Mey authored
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;
}