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 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 convert_laser_position(self, sensor_device, laser_x, laser_y):
t_world2robot = almath.transformFromPose2D(almath.Pose2D(self.robot_pose))
t_device2point = almath.transformFromPosition3D(almath.Position3D(laser_x, laser_y, 0))
t_robot2device = almath.transformFromPosition6D(self.device_position_list[sensor_device])
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
return (p2d_world2point.x, p2d_world2point.y)
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 convert_laser_position(self, sensor_device, laser_x, laser_y):
# ワールド座標系におけるロボット位置の変換行列
t_world2robot = almath.transformFromPose2D(
almath.Pose2D(self.robot_pose))
# デバイス→レーザー点群
t_device2point = almath.transformFromPosition3D(
almath.Position3D(laser_x, laser_y, 0))
# ロボット→デバイス
t_robot2device = almath.transformFromPosition6D(
self.device_position_list[sensor_device])
# ワールド座標系におけるレーザー点群
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
return (p2d_world2point.x, p2d_world2point.y)
def convert_laser_position(self, laser_values):
try:
# ロボットの位置
t_world2robot = almath.transformFromPose2D(almath.Pose2D(self.robot_pose))
for sensor_device in range(1):
for point_num in range(15):
laser = laser_values[sensor_device][point_num]
# センサデバイスから点群までの距離
t_device2point = almath.transformFromPosition3D(almath.Position3D(laser[0], laser[1], 0))
# センサデバイスの位置・姿勢
t_robot2device = almath.transformFromPosition6D(self.device_position_list[sensor_device])
# ワールド座標系におけるレーザー点群の位置
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
laser_values[sensor_device][point_num] = (p2d_world2point.x, p2d_world2point.y)
except Exception as errorMsg:
failure(errorMsg)
return laser_values