visual_mpc_server.py 文件源码

def init_traj_visualmpc_handler(self, req):
        self.igrp = req.igrp
        self.i_traj = req.itr

        self.t = 0
        if 'use_goalimage' in self.policyparams:
            goal_main = self.bridge.imgmsg_to_cv2(req.goalmain)
            goal_main = cv2.cvtColor(goal_main, cv2.COLOR_BGR2RGB)
            # goal_aux1 = self.bridge.imgmsg_to_cv2(req.goalaux1)
            # goal_aux1 = cv2.cvtColor(goal_aux1, cv2.COLOR_BGR2RGB)
            Image.fromarray(goal_main).show()
            goal_main = goal_main.astype(np.float32) / 255.
            self.cem_controller.goal_image = goal_main

        print 'init traj{} group{}'.format(self.i_traj, self.igrp)

        if 'ndesig' in self.policyparams:
            self.initial_pix_distrib = []
        else:
            self.initial_pix_distrib1 = []
            self.initial_pix_distrib2 = []

        self.cem_controller = CEM_controller(self.agentparams, self.policyparams, self.predictor, save_subdir=req.save_subdir)
        self.save_subdir = req.save_subdir
        return init_traj_visualmpcResponse()