def _autoaim(self, robot: Robot, state: RobotState):
#detected = {}
if robot.robot_id == ID_R1:
self.o_dis = 8.0
scan_distance, scan_type = [], []
state.detect = False
type_ = None
for i in range(-135, 135, 2):
angle, pos = robot.get_angle_pos()
angle += i / 180 * math.pi
p1 = (pos[0] + 0.374 * math.cos(angle),
pos[1] + 0.374 * math.sin(angle))
p2 = (pos[0] + SCAN_RANGE * math.cos(angle),
pos[1] + SCAN_RANGE * math.sin(angle))
self.__world.RayCast(self.__callback_autoaim, p1, p2)
scan_distance.append(self.__callback_autoaim.fraction)
#print(f"ang {i}\tfraction:{self.__callback_autoaim.fraction}\t")
u = self.__callback_autoaim.userData
# print(f"u type {u.type}")
# if abs(i) == 45 and robot.robot_id == ID_R1:
# print(f"angle:{i}, frac:{self.__callback_autoaim.fraction}")
# FIXME 射线扫描距离检测
if robot.robot_id == ID_R1 and u is not None:
if u.type == 'wall':
if self.o_dis > self.__callback_autoaim.fraction * (
SCAN_RANGE - 0.374):
self.o_dis = self.__callback_autoaim.fraction * (
SCAN_RANGE - 0.374)
self.o_ang = i
#print("wall", self.o_dis)
if u.type == 'robot':
d = self.__callback_autoaim.fraction * (SCAN_RANGE -
0.374) - 0.10
if self.o_dis > d:
self.o_dis = d
#print("robot", self.o_dis)
self.o_ang = i
if u is not None and u.type == "robot":
if u.id % 2 == robot.robot_id % 2:
scan_type.append(0)
continue
else:
scan_type.append(1)
if -45 <= i <= 45:
#if u.id not in [ID_R1, ID_R2]:
if u.id % 2 != robot.robot_id % 2:
if not state.detect:
robot.set_gimbal(angle)
state.detect = True
else:
scan_type.append(0)
# FIXME 距离障碍物太近的时候惩罚, 但是由于扫面半径起点不是刚好是车的外表, 可能这个不加更好
# if min(scan_distance) * (SCAN_RANGE-0.374) <= 0.005:
# self.conflict = True
state.scan = [scan_distance, scan_type]
def _autoaim(self, robot: Robot, state: RobotState):
#detected = {}
scan_distance, scan_type = [], []
state.detect = False
for i in range(-135, 135, 2):
angle, pos = robot.get_angle_pos()
angle += i / 180 * math.pi
p1 = (pos[0] + 0.3 * math.cos(angle),
pos[1] + 0.3 * math.sin(angle))
p2 = (pos[0] + SCAN_RANGE * math.cos(angle),
pos[1] + SCAN_RANGE * math.sin(angle))
self.__world.RayCast(self.__callback_autoaim, p1, p2)
scan_distance.append(self.__callback_autoaim.fraction)
u = self.__callback_autoaim.userData
if u is not None and u.type == "robot":
scan_type.append(1)
if -45 <= i <= 45:
if not state.detect:
robot.set_gimbal(angle)
state.detect = True
else:
scan_type.append(0)
state.scan = [scan_distance, scan_type]
