action_database.py 文件源码

def execute(self,userdata):
        time.sleep(1)
        self.perception_pub.publish(Bool(True))
        time.sleep(self.perception_time)
        self.perception_pub.publish(Bool(False))
        frames = self.listen.getFrameStrings()
        object_frames = [of for of in frames if of in self.object_list]
        if object_frames[0] and object_frames[1]:
            translation1, quaternion1 = self.listen.lookupTransform("/root", object_frames[0], rospy.Time(0))
            translation2, quaternion2 = self.listen.lookupTransform("/root", object_frames[1], rospy.Time(0))
            if translation1[2] > translation2[2]:
                userdata.place_obj_name_out = object_frames[0]
                userdata.pick_obj_name_out = object_frames[1]
            else:
                userdata.place_obj_name_out = object_frames[1]
                userdata.pick_obj_name_out = object_frames[0]
            return 'objects_found'
        else:
            return 'objects_not_found'