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)
