Python MoveGroupCommander.stop方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import stop [as 别名]

#.........这里部分代码省略.........
                rospy.sleep(1)
                continue
            
            # Transform the pose to the base reference frame
            target_base = self.listener.transformPose(self.reference_frame, target)
            
            # Compute the distance between the current target and the last target
            p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
            p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
                    
            dist_last_target = euclidean(p1, p2)
            
            # Move the arm only if we are far enough away from the previous target
            if dist_last_target < self.last_target_threshold:
                rospy.loginfo("Still close to last target")
                rospy.sleep(0.5)
                continue
            
            # Get the pose of the end effector in the base reference frame
            ee_pose = self.right_arm.get_current_pose(self.ee_link)
            
            # Convert the position values to a Python list
            p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
            
            # Compute the distance between the target and the end-effector
            dist_target = euclidean(p1, p3)
            
            # Only move the arm if we are far enough away from the target
            if dist_target < self.target_ee_threshold:
                rospy.loginfo("Already close enough to target")
                rospy.sleep(1)
                continue
            
            # We want the gripper somewhere on the line connecting the shoulder and the target.
            # Using a parametric form of the line, the parameter ranges from 0 to the
            # minimum of the arm length and the distance to the target.
            t_max = min(self.arm_length, dist_target_shoulder)
            
            # Bring it back 10% so we don't collide with the target
            t = 0.9 * t_max
            
            # Now compute the target positions from the parameter
            try:
                target_arm.pose.position.x *= (t / dist_target_shoulder)
                target_arm.pose.position.y *= (t / dist_target_shoulder)
                target_arm.pose.position.z *= (t / dist_target_shoulder)
            except:
                rospy.sleep(1)
                rospy.loginfo("Exception!")
                continue
            
            # Transform to the base_footprint frame
            target_ee = self.listener.transformPose(self.reference_frame, target_arm)
            
            # Set the target gripper orientation to be horizontal
            target_ee.pose.orientation.x = 0
            target_ee.pose.orientation.y = 0
            target_ee.pose.orientation.z = 0
            target_ee.pose.orientation.w = 1
            
            # Update the current start state
            self.right_arm.set_start_state_to_current_state()
            
            # Set the target pose for the end-effector
            self.right_arm.set_pose_target(target_ee, self.ee_link)
            
            # Plan and execute the trajectory
            success = self.right_arm.go()
            
            if success:
                # Store the current target as the last target
                self.last_target_pose = target
            
            # Pause a bit between motions to keep from locking up
            rospy.sleep(0.5)
            
                    
    def update_target_pose(self, target):
        self.target = target

    def relax_all_servos(self):
        command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
        args = shlex.split(command)
        subprocess.Popen(args)
           
    def shutdown(self):
        # Stop any further target messages from being processed
        self.target_subscriber.unregister()
        
        # Stop any current arm movement
        self.right_arm.stop()
        
        # Move to the resting position
        self.right_arm.set_named_target('resting')
        self.right_arm.go()
        
        # Relax the servos
        self.relax_all_servos()
        
        os._exit(0)