From a8c8a9288989dd4f046248147da469345f24be54 Mon Sep 17 00:00:00 2001 From: Jivko Sinapov Date: Fri, 28 Apr 2017 20:43:16 -0500 Subject: [PATCH] minor changes --- src/agile_grasp/grasp_localizer_server.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/agile_grasp/grasp_localizer_server.cpp b/src/agile_grasp/grasp_localizer_server.cpp index b4e61a3..a34ebfa 100644 --- a/src/agile_grasp/grasp_localizer_server.cpp +++ b/src/agile_grasp/grasp_localizer_server.cpp @@ -161,6 +161,9 @@ bool GraspLocalizerServer::service_callback(agile_grasp::FindGrasps::Request &re sensor_msgs::PointCloud2 cloud = req.object_cloud; pcl::fromROSMsg(cloud, *cloud_left_); + //hand offset approach + //double hand_offset_approach = req.approach_offset; + //compute grasp points std::vector indices(0); hands_ = localization_->localizeHands(cloud_left_, cloud_left_->size(), indices, false, false); @@ -173,7 +176,7 @@ bool GraspLocalizerServer::service_callback(agile_grasp::FindGrasps::Request &re for (unsigned int i = 0; i < grasps.grasps.size(); i++){ geometry_msgs::PoseStamped p_grasp_i = graspToPose(grasps.grasps.at(i),HAND_OFFSET_GRASP,req.object_cloud.header.frame_id); - geometry_msgs::PoseStamped p_approach_i = graspToPose(grasps.grasps.at(i),HAND_OFFSET_APPROACH,req.object_cloud.header.frame_id); + geometry_msgs::PoseStamped p_approach_i = graspToPose(grasps.grasps.at(i),HAND_OFFSET_GRASP,req.object_cloud.header.frame_id); res.grasp_poses.push_back(p_grasp_i); res.approach_poses.push_back(p_approach_i);