def test_get_robot_pitch_rpy2(self):
     robot, pose, internal_angle = self.init_robot_and_pose()
     # robot.set_initial_angles(0, internal_angle, 0)
     self.assertEquals(internal_angle, -13)
     cam_angles = get_robot_horizon_r(robot, PyDataVector(0, -internal_angle, 0)) * rtd()
     self.assertTrue(abs(cam_angles[1]) < 1e-10)
     self.assertTrue(abs(cam_angles[0] + 2 * internal_angle) < 1e-10)
    def update(self, data):
        """ This Method checks if there is new data to process and if so let the
            robot vision process the image and extract data for further behaviour """
        if not self.check_is_new_frame(data):
            data[DATA_KEY_IS_NEW_FRAME] = False
            return
        else:
            data[DATA_KEY_IS_NEW_FRAME] = True
            need_to_recalibrate_ball_config = data[DATA_KEY_RECALIBRATE_BALL]
            # Process the Image
            if self.use_kinematic_horizon:
                robo_horizon_k = get_robot_horizon_p(self.transformer.robot, data[DATA_KEY_CAMERA_FOOT_PHASE])
                robo_horizon_imu = get_robot_horizon_r(self.transformer.robot , data[DATA_KEY_IPC].get_robot_angle()) \
                                    if data.get(DATA_KEY_IPC, False) else robo_horizon_k
                #print robo_horizon_k - robo_horizon_imu
                robo_horizon = (robo_horizon_k + robo_horizon_imu) / 2
                #robo_horizon = robo_horizon_k
                robo_horizon[1] = robo_horizon[1] / self.transformer.get_camera_angle()
                self.vision.set_robot_horizon(robo_horizon / self.transformer.get_camera_angle())
            self.vision.process(data[DATA_KEY_RAW_IMAGE], need_to_recalibrate_ball_config)
            if need_to_recalibrate_ball_config is True:
                self.save_color_config("ball")

            # Get the Informations from the Cython Class
            data[DATA_KEY_RAW_BALL] = self.vision.ball_info
            goal_info = self.vision.goal_info
            data[DATA_KEY_RAW_GOAL_DATA] = goal_info
            obstacles = self.vision.obstacle

            # Pass the Information to the extractor
            #self.extract_goal_infos(data, goal_info)
            self.extract_horizon_obstacles(data, obstacles)
            self.extract_horizon_line_properties(data)

            data[DATA_KEY_IGNORE_MASQ_HITS] = self.vision.ignore_masq_hits
            #debug_m(5, "IgnoreMasq", self.vision.ignore_masq_hits)
            data[DATA_KEY_LINE_POINTS] = self.vision.line_points
            #debug_m(5, "NumberLinePoints", data["LinePoints"].size)

            if self.export_frame_skip > 0 and data[DATA_KEY_CAMERA_FRAME_VERSION] % self.export_frame_skip is 0:
                # Dateiname für den nächsten Frame
                name = "frame-%04d.yuv" % self.idx
                self.idx += 1
                with Timer("Speichere " + name, self.debug):
                    yuyv = data[DATA_KEY_RAW_IMAGE]

                    with open("/home/darwin/" + name + ".json", "wb") as fp:
                        angle = data[DATA_KEY_IPC].get_robot_angle()
                        json.dump({"positions": data[DATA_KEY_IMAGE_POSE].positions, "robot_angle": [angle.x, angle.y, angle.z]}, fp)

                    with gzip.GzipFile("/home/darwin/" + name + ".gz", "wb", compresslevel=1) as fp:
                        fp.write(yuyv.tostring())
 def test_get_robot_roll_rpy2(self):
     robot, pose, internal_angle = self.init_robot_and_pose()
     cam_angles = get_robot_horizon_r(robot, PyDataVector(-internal_angle, 0, 0)) * rtd()
     self.assertEquals(internal_angle, -13)
     self.assertTrue(abs(cam_angles[1] + internal_angle) < 1e-10)