manager.py 文件源码

def __init__(self, robot, *robotargs):
        super(PickAndPlaceNode, self).__init__('pp_node')
    rospy.loginfo("PickAndPlaceNode")
        _post_perceive_trans = defaultdict(lambda: self._pick)
        _post_perceive_trans.update({'c': self._calibrate})
        _preplace = defaultdict(lambda: self._preplace)
        self.transition_table = {
            # If calibration has already happened, allow skipping it
            'initial': {'c': self._calibrate, 'q': self._perceive,
                        's': self._preplace},
            'calibrate': {'q': self._perceive, 'c': self._calibrate},
            'perceive': {'p': self._post_perceive, 'q': self._perceive, 's': self._stop_perceive, 'c': self._calibrate},
            'post_perceive': _post_perceive_trans,
            'postpick': {'1': self._level, '2': self._level, '9': self._level},
            'level': _preplace,
            'preplace': {'s': self._place},
            'place': {'q': self._perceive, 'c': self._calibrate}
            }
    rospy.loginfo("PickAndPlaceNode1")
        if callable(robot):
            self.robot = robot(*robotargs)
        else:
            self.robot = robot
        self.robot.level = 1
    rospy.loginfo("PickAndPlaceNode2")
        # Hardcoded place for now
        self.place_pose = PoseStamped(
            Header(0, rospy.Time(0), self.robot.base),
            Pose(Point(0.526025806, 0.4780144, -0.161326153),
                 Quaternion(1, 0, 0, 0)))
        self.tl = tf.TransformListener()
        self.num_objects = 0
        # Would this work too? Both tf and tf2 have (c) 2008...
        # self.tf2 = tf2_ros.TransformListener()
        self.n_objects_sub = rospy.Subscriber("/num_objects", Int64,
                                              self.update_num_objects,
                                              queue_size=1)
        self.perception_pub = rospy.Publisher("/perception/enabled",
                                              Bool,
                                              queue_size=1)
        self.alignment_pub = rospy.Publisher("/alignment/doit",
                                             Bool,
                                             queue_size=1)
        self.br = tf.TransformBroadcaster()
    rospy.loginfo("PickAndPlaceNode3")
        self.int_marker_server = InteractiveMarkerServer('int_markers')
        # Dict to map imarker names and their updated poses
        self.int_markers = {}

        # Ideally this call would be in a Factory/Metaclass/Parent
        self.show_options()
        self.perceive = False
        # self.robot.home()
        # self.move_calib_position()