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
Exemple #3
0
    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
Exemple #4
0
	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.
Exemple #5
0
 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(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