def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_grasps(self, pose, width):
"""
使用抓握生成器生成动作来生成抓握； 基于moveit_simple_grasps pkg上的server_test.py示例。
生成抓取姿势阵列数据以进行可视化，
然后将抓取目标发送到抓取服务器.
"""
self._grasps_ac.wait_for_server()
rospy.loginfo("Successfully connected!")
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
# Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary.
#
rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal))
#self._grasps_ac.wait_for_result()
#发送目标并等待结果：
# ＃将目标发送到ActionServer，等待目标完成，然后抢占目标是必要的。
t_start = rospy.Time.now()
state = self._grasps_ac.send_goal_and_wait(goal)
t_end = rospy.Time.now()
t_toal = t_end - t_start
rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec()))
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' %
self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
"""
print("---------grasps--start------------")
print(grasps)
print("-----------test-grasps-----end-----------------")
rospy.sleep(2)
print("-grasps---sleep----end-----")
"""
return grasps
def _generate_grasps(self, pose, width): goal = GenerateGraspsGoal() goal.pose = pose #Generated from inputs goal.width = width #Generated from inputs impl = GraspGeneratorOptions() impl.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP impl.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL state = self._grasps_ac.send_goal_and_wait(goal) if state = GoalStatus.SUCCEEDED: grasps = self.
def generate_grasps(self, pose, width):
"""Send request to block grasp generator service"""
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
grasp_options = GraspGeneratorOptions()
grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y
grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN
grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF
goal.options.append(grasp_options)
self.grasps_ac.send_goal(goal)
if DEBUG_MODE:
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
self.grasps_ac.wait_for_result()
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def generate_grasps(pose, width):
#send request to block grasp generator service
grasps_ac.wait_for_server()
rospy.loginfo("Successfully connected.")
goal = GenerateGraspsGoal()
goal.pose = 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
def generate_grasps(pose, width):
#send request to block grasp generator service
grasps_ac.wait_for_server()
rospy.loginfo("Successfully connected.")
goal = GenerateGraspsGoal()
goal.pose = 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
def generate_grasps(self, pose, width):
self.grasps_ac.wait_for_server()
if self.DEBUG is 1:
print "[DEBUG] Successfully connected."
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
if self.DEBUG is 1:
print "[DEBUG] 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
if self.DEBUG is 1:
print "[DEBUG] Result received in " + str(t_total.to_sec())
grasps = self.grasps_ac.get_result().grasps
return grasps
def retrieveGrasps(pose, width=0.04):
rospy.loginfo("Connecting to grasp generator AS")
grasps_ac = actionlib.SimpleActionClient('/grasp_generator_server/generate', GenerateGraspsAction)
grasps_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
goal = GenerateGraspsGoal()
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
def generate_grasps(self, pose, width):
self.grasps_ac.wait_for_server()
if self.DEBUG is 1:
print "[DEBUG] Successfully connected."
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
if self.DEBUG is 1:
print "[DEBUG] 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
if self.DEBUG is 1:
print "[DEBUG] Result received in " + str(t_total.to_sec())
grasps = self.grasps_ac.get_result().grasps
return grasps
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
使用抓握生成器生成动作来生成抓握； 基于moveit_simple_grasps pkg上的server_test.py示例
Generate the grasp Pose Array data for visualization,
and then send the grasp goal to the grasp server.
生成抓取姿势阵列数据以进行可视化，然后将抓取目标发送到抓取服务器。
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# 发送目标并等待结果:
# 将目标发送到ActionServer，等待目标完成，并且必须抢占目标.
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# 发布掌握（用于调试/可视化目的）:
self._publish_grasps(grasps)
print "-------------test7------------------"
return grasps