def get_closest_landmark_non_move(self, robot_pose, landmark):
side_range = [-math.radians(70), math.radians(70)]
min_distance = 100
min_landmark_id = -1
for id in landmark:
# calculate angle robot to landmark
# マップ原点座標から見たランドマークの位置・姿勢
map2landmark = almath.Position6D(landmark[id])
t_map2landmark = almath.transformFromPosition6D(map2landmark)
# map原点座標から見たロボットの位置・姿勢
map2robot = almath.position6DFromPose2D(robot_pose)
t_map2robot = almath.transformFromPosition6D(map2robot)
# ロボットから見たランドマークの位置・姿勢
t_robot2landmark = t_map2robot.inverse() * t_map2landmark
robot2landmark = almath.position6DFromTransform(t_robot2landmark)
#print(id, robot2landmark, robot2landmark.norm())
local_angle_xy, local_angle_xz = self._get_angle_vector(
robot2landmark)
if (side_range[0] < local_angle_xy < side_range[1]) and (
side_range[0] < local_angle_xz < side_range[1]):
if robot2landmark.norm() < min_distance:
min_distance = robot2landmark.norm()
min_landmark_id = id
#print(math.degrees(local_angle_xy))
return min_landmark_id
def approach_home(self):
x_coord, y_coord, theta = self.services.PositionManager.get_home_pos()
coord_pod = [x_coord, y_coord, theta]
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
p2D_world2target = almath.Pose2D(coord_pod)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(t_robot2target)
coord_pod = list(p6D_robot2target.toVector())
coord_home = [
coord_pod[0] + DISTANCE_FIRST_APPROACH * math.cos(coord_pod[-1]),
coord_pod[1] + DISTANCE_FIRST_APPROACH * math.sin(coord_pod[-1]),
coord_pod[-1] + math.pi
]
is_success = False
if self.services.PositionManager.turn_toward_position(
[DISTANCE_FIRST_APPROACH, 0]):
x, y, theta = coord_home
r, r_theta = self.get_polar_coord([x, y, theta])
if self.services.ALMotion.moveTo(r, 0, 0, MOVE_CONFIG_HIGH_SPEED):
is_success = True
yield self.services.PositionManager.turn_toward_position(
[0, 0], _async=True)
yield stk.coroutines.Return(is_success)
def get_closest_landmark(self, robot_pose, landmark):
distance_info = dict()
side_range = [-math.radians(70), math.radians(70)]
for id in landmark:
# calculate angle robot to landmark
# マップ原点座標から見たランドマークの位置・姿勢
map2landmark = almath.Position6D(landmark[id])
t_map2landmark = almath.transformFromPosition6D(map2landmark)
# map原点座標から見たロボットの位置・姿勢
map2robot = almath.position6DFromPose2D(robot_pose)
t_map2robot = almath.transformFromPosition6D(map2robot)
# ロボットから見たランドマークの位置・姿勢
t_robot2landmark = t_map2robot.inverse() * t_map2landmark
robot2landmark = almath.position6DFromTransform(t_robot2landmark)
local_angle_xy, local_angle_xz = self._get_angle_vector(
robot2landmark)
# 1. xz面において任意の角度内に収まっている
# TODO この時点でnullが帰ってくる可能性がある
print(math.degrees(local_angle_xz))
#if (side_range[0] < local_angle_xz < side_range[1]):
# ベクトルのノルムを格納する
distance_info[id] = local_angle_xy #robot2landmark.norm()
# 2. xy面においてベクトル同士のなす角がすべてのランドマークに対して最小か
# 1.でフィルタしたランドマークリストを元に角度が小さい順にソートする
#min_k = min(d, key=d.get)
#min_landmark_id = min(distance_info, key=distance_info.get)
distance_info = sorted(distance_info.items(), key=lambda x: x[1])
print(distance_info)
return distance_info
def get_home_position(self, params):
p6Ds = self.detect_with_try_params_list(params)
p6Ds_list = {}
params_list = []
if not isinstance(params, list):
params_list.append(dict(DEFAULT_PARAMS))
else:
params_list = params
id_list = []
for tmp_params in params_list:
id_list += tmp_params["ids"]
for ids in id_list:
try:
p6D_world2target = almath.Position6D(p6Ds[ids]["world2target"])
t_world2target = almath.transformFromPosition6D(
p6D_world2target)
self.logger.info("@@@ TARGET IN WORLD %s" % p6D_world2target)
p2D_world2robot = almath.Pose2D(
self.ALMotion.getRobotPosition(True))
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
self.logger.info("@@@ ROBOT POSITION %s" % p2D_world2robot)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
self.logger.info("@@@ TARGET IN ROBOT %s" % p6D_robot2target)
p6Ds_list[ids] = list(p6D_robot2target.toVector())
except Exception as e:
pass
return p6Ds_list
def find_home(self, research_360_activated=False):
"returns (coords) of any code it finds, or (None)."
distance_pod = None
mode_search = False
if not self.services.PositionManager:
yield stk.coroutines.Return(None)
if self.pod_pos_in_world and not research_360_activated:
pod_future = self.services.PositionManager.turn_toward_position(
[-1.0, 0], _async=True)
yield pod_future
distance_pod = self.services.PositionManager.get_dist_from_robot(
-0.70, 0)
pod_future = self.services.ALTracker._lookAtWithEffector(
[distance_pod, 0, 0], 2, 2, 0.1, 0, _async=True)
yield pod_future
else:
mode_search = True
pod_future = None
if mode_search:
pod_future = self.search_home()
yield pod_future
self.logger.info(repr(pod_future.value()))
else:
yield self.services.ALRecharge.setUseTrackerSearcher(False,
_async=True)
pod_future = qi. async (self.services.ALRecharge.lookForStation)
self.future_sleep = stk.coroutines.sleep(5)
yield self.future_sleep
if pod_future.isRunning():
self.services.ALRecharge.stopAll()
yield stk.coroutines.Return(None)
if pod_future:
coord_pod = pod_future.value()
if coord_pod and coord_pod[1] and len(coord_pod[1]) == 3:
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
coord_pod = coord_pod[1]
self.services.PositionManager.init_position_with_coord(
coord_pod)
self.pod_pos_in_world = [0.0, 0.0]
p2D_world2target = almath.Pose2D(coord_pod)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
coord_pod = list(p6D_robot2target.toVector())
coord_home = [
coord_pod[0] + DISTANCE_FROM_POD * math.cos(coord_pod[-1]),
coord_pod[1] + DISTANCE_FROM_POD * math.sin(coord_pod[-1]),
coord_pod[-1]
]
yield stk.coroutines.Return(coord_home)
yield stk.coroutines.Return(None)
def arm_dist(motionProxy,effectorName,x,y,z):
# arm transformation in robot space
arm_transform = motionProxy.getTransform(effectorName,2,True)
robot2Arm = almath.Transform(almath.vectorFloat(arm_transform))
rot=almath.position6DFromTransform(robot2Arm)
print effectorName,"position",robot2Arm.r1_c4,robot2Arm.r2_c4,robot2Arm.r3_c4
mx,my,mz=rot.wx,rot.wy,rot.wz
x_d,y_d,z_d=x-robot2Arm.r1_c4,y-robot2Arm.r2_c4,z-robot2Arm.r3_c4
print "distances update:"
print x_d,y_d,z_d
return [x_d,y_d,z_d],mx
def CalculateDistanceNew(Weight,Alpha,Beta,IP,PORT,landmarkTheoreticalSize):
currentCamera = "CameraBottom"
memoryProxy = ALProxy("ALMemory", IP,PORT)
motionProxy=ALProxy("ALMotion",IP,PORT)
angularSize = (Weight/ 640.0) * 60.97 * almath.PI / 180
distanceFromCameraToLandmark = landmarkTheoreticalSize / (2 * math.tan(angularSize / 2))
# Get current camera position in NAO space.
transform = motionProxy.getTransform(currentCamera, 2, True)
print "transform=",transform
transformList = almath.vectorFloat(transform)
robotToCamera = almath.Transform(transformList)
# Compute the rotation to point towards the landmark.
cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, Beta, Alpha)
# Compute the translation to reach the landmark.
cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)
# Combine all transformations to get the landmark position in NAO space.
robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform * cameraToLandmarkTranslationTransform
print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
print "z " + str(robotToLandmark.r3_c4) + " (in meters)"
print "robotToLandmark=",robotToLandmark
# 从机器人端读取相关传感器的角度和位置数据
headAgl = motionProxy.getAngles(["HeadYaw", "HeadPitch"], True)
# ColumnAglY = Alpha + headAgl[0] #水平方向角度差
# print "ColumnAglY=",ColumnAglY
# Position3D=almath.Position3D(robotToLandmark.r1_c4,robotToLandmark.r2_c4,robotToLandmark.r3_c4)
# Position6D=almath.Position6D(0,0,0,0,0,0)
# almath.position6DFromPosition3DInPlace(Position3D,Position6D)
Position6D=almath.position6DFromTransform(robotToLandmark)
# position6D=almath.vectorFloat(Position6D)
# ColumnAglY=position6D
print "Position6D.wz=",Position6D.wz
print "Position6D",Position6D
# print "type of Position6D=",type(Position6D)
ColumnAglY=Position6D.wz
# print "ColumnAglY=",ColumnAglY
return robotToLandmark.r1_c4*1000,robotToLandmark.r2_c4*1000,robotToLandmark.r3_c4*1000,ColumnAglY #传出Torso_X和Torso_Y的毫米值
def calc_marker(self):
print("calculation marker information")
start_time = time.time()
try:
self.image_remote = self.camera.getImageRemote(self.subscriber_id)
except Exception as message:
self.unsubscribe()
print(str(message))
if not self.image_remote:
self.unsubscribe()
raise Exception("No data in image")
camera = self.params["camera"]
camera_name = CAMERAS[camera]
seconds = self.image_remote[4]
micro_seconds = self.image_remote[5]
t_world2camera = almath.Transform(
self.motion._getSensorTransformAtTime(
camera_name, seconds * 10e8 + micro_seconds * 10e2))
t_robot2camera = almath.Transform(
self.motion.getTransform(camera_name, 2, True))
resolution = self.params["resolution"]
x, y = CAMERA_DATAS_AT_RESOLUTION[resolution]["image_size"]
color_space, channels = self.params["color_space_and_channels"]
image = numpy.frombuffer(self.image_remote[6],
dtype=numpy.uint8).reshape(y, x, channels)
if self.params["color"] and color_space == vd.kBGRColorSpace:
print("Thresholding image...")
lower_b = tuple([int(val) for val in self.params["color"]])
upper_b = (255, 255, 255)
image = cv2.inRange(image, lower_b, upper_b)
print("Thresholding image done")
p6Ds = dict()
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
image, self.params["dictionary"])
result = False
if ids is not None:
try:
if [328] in ids:
count = 0
for _id in ids:
print(_id)
if _id == [328]:
break
count = count + 1
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, \
self.params["size"], \
CAMERA_DATAS_AT_RESOLUTION[resolution]["matrix"], \
CAMERA_DISTORTION_COEFF)
tvec = tvecs[count][0]
x, y, z = tvec[2], -tvec[0], -tvec[1]
p3d_camera2target = almath.Position3D(x, y, z)
rvec = rvecs[count][0]
wx, wy, wz = rvec[2], -rvec[0], -rvec[1]
proj_rvec, _ = cv2.Rodrigues(numpy.array([wx, wy, wz]))
r_camera2target = almath.Rotation(proj_rvec.flatten())
t_camera2target = almath.transformFromRotationPosition3D(
r_camera2target, p3d_camera2target)
r3d_correction = almath.Rotation3D(0., 3 * math.pi / 2, 0)
t_corretion = almath.transformFromRotation3D(
r3d_correction)
t_world2target = t_world2camera * t_camera2target * t_corretion
t_robot2target = t_robot2camera * t_camera2target * t_corretion
p6D_world2target = almath.position6DFromTransform(
t_world2target)
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
print("[x,y,theta] = [{},{},{}]".format(
p6D_robot2target.x, p6D_robot2target.y,
math.degrees(p6D_robot2target.wz)))
#p6Ds[ids] = {
# "robot2target": list(p6D_robot2target.toVector())
#"world2target": list(p6D_world2target.toVector())
#}
result = True
print("ID:" + str(ids))
except Exception as message:
print("failed: {}".format(str(message)))
self.unsubscribe()
else:
result = False
print("No Marker")
delta_time = time.time() - start_time
return result
def _detect_markers(self, params, image, t_world2camera, timestamp):
t = time.time()
# self.logger.info("_detect_markers...")
p6Ds = dict()
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
image, params["dictionary"])
try:
ids = [int(_id) for _id in ids.flatten()]
except Exception as e:
raise Exception("No markers found")
# self.logger.info("@@@@@@@@@ IDs: %s" % ids)
if ids:
if params["ids"]:
id_indices = [
indice for (indice, _id) in enumerate(ids)
if _id in set(params["ids"]).intersection(set(ids))
]
else:
id_indices = [indice for (indice, _id) in enumerate(ids)]
# self.logger.info("@@@@@@@@@ IDs indices: %s" % id_indices)
if id_indices:
resolution = params["resolution"]
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, \
params["size"], \
CAMERA_DATAS_AT_RESOLUTION[resolution]["matrix"], \
CAMERA_DISTORTION_COEFF)
for indice in id_indices:
# switch from opencv coordinates to NAOqi coordinates
tvec = tvecs[indice][0] # translation vectors
x, y, z = tvec[2], -tvec[0], -tvec[1]
p3d_camera2target = almath.Position3D(x, y, z)
rvec = rvecs[indice][0] # rotation vectors
wx, wy, wz = rvec[2], -rvec[0], -rvec[1]
proj_rvec, _ = cv2.Rodrigues(numpy.array([wx, wy, wz]))
r_camera2target = almath.Rotation(proj_rvec.flatten())
t_camera2target = almath.transformFromRotationPosition3D(
r_camera2target, p3d_camera2target)
if params["position"] == "floor":
r3d_correction = almath.Rotation3D(
0., math.pi / 2,
0.) # we have 90° on y axis that is invalid
elif params["position"] == "wall":
r3d_correction = almath.Rotation3D(
0., math.pi,
0.) # we have 180° on y axis that is invalid
t_correction = almath.transformFromRotation3D(
r3d_correction)
t_world2target = t_world2camera * t_camera2target * t_correction
p6D_world2target = almath.position6DFromTransform(
t_world2target)
self.logger.info(
"@@@@@@@@@ ID: %s - P6D_WORLD2TARGET: %s" %
(ids[indice], p6D_world2target))
p6Ds[ids[indice]] = {
"world2target": list(p6D_world2target.toVector()),
"timestamp": timestamp,
}
else:
raise Exception("No markers with IDs %s found" % params["ids"])
d = time.time() - t
self.logger.info("_detect_markers done %s" % d)
return p6Ds
def find_home(self, research_360_activated=False):
"returns (coords) of any code it finds, or (None)."
distance_big_aruco = None
distance_small_aruco = None
id_aruco = 128
mode_search = False
if not self.services.PositionManager:
self.logger.info("no PositionManager")
yield stk.coroutines.Return(None)
if self.aruco_pos_in_world and not research_360_activated:
distance_big_aruco = self.services.PositionManager.get_dist_from_robot(
0, 0)
distance_small_aruco = self.services.PositionManager.get_dist_from_robot(
0.56, 0)
if distance_big_aruco > 0.60:
aruco_future = self.services.PositionManager.turn_toward_position(\
[0, 0],
_async=True)
yield aruco_future
aruco_future = self.services.ALTracker._lookAtWithEffector(
[distance_big_aruco, 0, 0], 2, 2, 0.1, 0, _async=True)
yield aruco_future
id_aruco = 128
elif distance_small_aruco > 0.40:
aruco_future = self.services.PositionManager.turn_toward_position(\
[1.0, 0],
_async=True)
yield aruco_future
aruco_future = self.services.ALTracker._lookAtWithEffector(
[distance_small_aruco, 0, 0], 2, 2, 0.1, 0, _async=True)
yield aruco_future
id_aruco = 448
else:
yield stk.coroutines.Return(None)
else:
self.logger.info("Lost mode")
mode_search = True
try:
aruco_future = None
if mode_search:
aruco_future = yield self.search_home()
else:
print "DBG done moving, now scanning"
aruco_future = yield self.services.DXAruco.get_home_position_in_world(
DEFAULT_PARAMS[id_aruco], _async=True)
if aruco_future:
self.logger.info(aruco_future)
if id_aruco in aruco_future:
self.logger.info("id_aruco")
self.logger.info(id_aruco)
else:
self.logger.info("id_aruco")
[id_aruco] = [tmp for tmp in [128, 448] if tmp != id_aruco]
self.logger.info(id_aruco)
if id_aruco in aruco_future:
self.logger.info(aruco_future)
coord_aruco = aruco_future[id_aruco]
coord_home_pos = None
if len(coord_aruco) == 6:
coord_aruco = [
coord_aruco[0], coord_aruco[1], coord_aruco[-1]
]
coord_home_pos = coord_aruco
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
p2D_world2target = almath.Pose2D(coord_aruco)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(
p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
coord_aruco = list(p6D_robot2target.toVector())
x, y, theta = [
coord_aruco[0], coord_aruco[1], coord_aruco[-1]
]
coord_home = None
if id_aruco == 128:
theta += math.radians(-135)
coord_home = [x, y, theta]
coord_home_pos = [
coord_home_pos[0], coord_home_pos[1],
coord_home_pos[-1] + math.radians(-135)
]
else:
theta += math.radians(135)
coord_home = [
x - DISTANCE_FROM_SMALL_ARUCO * math.cos(theta),
y - DISTANCE_FROM_SMALL_ARUCO * math.sin(theta),
theta
]
coord_home_pos = [
coord_home_pos[0] - DISTANCE_FROM_SMALL_ARUCO *
math.cos(coord_home_pos[-1] + math.radians(135)),
coord_home_pos[1] - DISTANCE_FROM_SMALL_ARUCO *
math.sin(coord_home_pos[-1] + math.radians(135)),
coord_home_pos[-1] + math.radians(135)
]
if coord_home_pos != None:
self.services.PositionManager.init_position_with_coord(
coord_home_pos)
self.aruco_pos_in_world = [0.0, 0.0]
self.logger.info("coord " + repr(coord_home))
yield stk.coroutines.Return([coord_home, id_aruco])
return
except Exception as error_msg:
self.logger.warning("No aruco with the id " + repr(id_aruco) +
" found.")
# Now continue at the next head angle
yield stk.coroutines.Return(None)