visual_mpc_client.py 文件源码

def query_action(self):

        if self.use_robot:
            if self.use_aux:
                self.recorder.get_aux_img()
                imageaux1 = self.recorder.ltob_aux1.img_msg
            else:
                imageaux1 = np.zeros((64, 64, 3), dtype=np.uint8)
                imageaux1 = self.bridge.cv2_to_imgmsg(imageaux1)

            imagemain = self.bridge.cv2_to_imgmsg(self.recorder.ltob.img_cropped)
            state = self.get_endeffector_pos()
        else:
            imagemain = np.zeros((64,64,3))
            imagemain = self.bridge.cv2_to_imgmsg(imagemain)
            imageaux1 = self.bridge.cv2_to_imgmsg(self.test_img)
            state = np.zeros(3)

        try:
            rospy.wait_for_service('get_action', timeout=240)
            get_action_resp = self.get_action_func(imagemain, imageaux1,
                                              tuple(state),
                                              tuple(self.desig_pos_main.flatten()),
                                              tuple(self.goal_pos_main.flatten()))

            action_vec = get_action_resp.action

        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            raise ValueError('get action service call failed')
        return action_vec