def generate_grasps(self, pose, width):
#send request to block grasp generator service
if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)):
return []
rospy.loginfo("Successfully connected.")
goal = GenerateBlockGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
t_start = rospy.Time.now()
self.grasps_ac.wait_for_result()
t_end = rospy.Time.now()
t_total = t_end - t_start
rospy.loginfo("Result received in " + str(t_total.to_sec()))
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def retrieveGrasps(pose, width=0.04):
rospy.loginfo("Connecting to grasp generator AS")
grasps_ac = actionlib.SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
grasps_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
goal = GenerateBlockGraspsGoal()
goal.pose = pose
goal.width = width
grasps_ac.send_goal(goal)
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
t_start = rospy.Time.now()
grasps_ac.wait_for_result()
t_end = rospy.Time.now()
t_total = t_end - t_start
rospy.loginfo("Result received in " + str(t_total.to_sec()))
grasp_list = grasps_ac.get_result().grasps
return grasp_list
