def collision_detection(self, goal_pos_x, goal_pos_y):
#返り値
# 衝突するか(bool)
# 障害物位置から機体位置と目標地の直線に下ろした垂線の交点x(float)
# 障害物位置から機体位置と目標地の直線に下ろした垂線の交点y(float)
# 障害物位置x(float)
# 障害物位置y(float)
# 機体位置と目標地の直線と障害物の距離(float)
a, b, c = functions.line_parameters(self.ctrld_robot.get_current_position()[0], self.ctrld_robot.get_current_position()[1], goal_pos_x, goal_pos_y)
if a != 0 and b != 0:
#自分以外の全robotに対して衝突判定
for i in self.objects.get_active_robot_ids():
if i != self.robot_id:
robot = self.objects.get_robot_by_id(i)
if robot is None:
continue
distance = functions.distance_of_a_point_and_a_straight_line(robot.get_current_position()[0], robot.get_current_position()[1], a, b, c)
if distance < self.ctrld_robot.size_r * 2:
x = (-robot.get_current_position()[1] * b + (b**2 / a) * robot.get_current_position()[0] - c) / (a + b**2 / a)
y = (-a * x -c) / b
if (self.ctrld_robot.get_current_position()[0] < x < goal_pos_x \
or self.ctrld_robot.get_current_position()[0] > x > goal_pos_x) \
and (self.ctrld_robot.get_current_position()[1] < y < goal_pos_y \
or self.ctrld_robot.get_current_position()[1] > y > goal_pos_y):
return True, x, y, robot.get_current_position()[0] , robot.get_current_position()[1], distance
#全enemyに対して衝突判定
for j in self.objects.get_active_enemy_ids():
enemy = self.objects.get_enemy_by_id(j)
if enemy is None:
continue
distance = functions.distance_of_a_point_and_a_straight_line(enemy.get_current_position()[0], enemy.get_current_position()[1], a, b, c)
if distance < self.ctrld_robot.size_r * 2:
x = (-enemy.get_current_position()[1] * b + (b**2 / a) * enemy.get_current_position()[0] - c) / (a + b**2 / a)
y = (-a * x -c) / b
if (self.ctrld_robot.get_current_position()[0] < x < goal_pos_x \
or self.ctrld_robot.get_current_position()[0] > x > goal_pos_x) \
and (self.ctrld_robot.get_current_position()[1] < y < goal_pos_y \
or self.ctrld_robot.get_current_position()[1] > y > goal_pos_y):
return True, x, y, enemy.get_current_position()[0] , enemy.get_current_position()[1], distance
distance = functions.distance_of_a_point_and_a_straight_line(self.ball_params.get_current_position()[0], self.ball_params.get_current_position()[1], a, b, c)
#ballに対して衝突判定
if distance < self.ctrld_robot.size_r * 1:
x = (-self.ball_params.get_current_position()[1] * b + (b**2 / a) * self.ball_params.get_current_position()[0] - c) / (a + b**2 / a)
y = (-a * x -c) / b
if (self.ctrld_robot.get_current_position()[0] < x < goal_pos_x \
or self.ctrld_robot.get_current_position()[0] > x > goal_pos_x) \
and (self.ctrld_robot.get_current_position()[1] < y < goal_pos_y \
or self.ctrld_robot.get_current_position()[1] > y > goal_pos_y):
return True, x, y, self.ball_params.get_current_position()[0] , self.ball_params.get_current_position()[1], distance
return False, 0, 0, 0, 0, 0
def dribble_ball(self, target_x, target_y, ignore_penalty_area=False):
distance = math.sqrt(
(target_x - self.ball_params.get_current_position()[0])**2 +
(target_y - self.ball_params.get_current_position()[1])**2)
if distance != 0:
#ボール後方へ移動
if self.dribble_stage == 0:
prepare_offset = 0.4
pose_x = (
-prepare_offset * target_x + (prepare_offset + distance) *
self.ball_params.get_current_position()[0]) / distance
pose_y = (
-prepare_offset * target_y + (prepare_offset + distance) *
self.ball_params.get_current_position()[1]) / distance
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
a, b, c = functions.line_parameters(
self.ball_params.get_current_position()[0],
self.ball_params.get_current_position()[1], target_x,
target_y)
self.dispersion1.append(
functions.distance_of_a_point_and_a_straight_line(
self.ctrld_robot.get_current_position()[0],
self.ctrld_robot.get_current_position()[1], a, b, c))
#(pose_x - self.ctrld_robot.get_current_position()[0])**2 \
#+ (pose_y - self.ctrld_robot.get_current_position()[1])**2)
del self.dispersion1[0]
self.dispersion2.append(
(pose_x - self.ctrld_robot.get_current_position()[0])**2 \
+ (pose_y - self.ctrld_robot.get_current_position()[1])**2)
del self.dispersion2[0]
dispersion_average1 = sum(self.dispersion1) / len(
self.dispersion1)
dispersion_average2 = sum(self.dispersion2) / len(
self.dispersion2)
self.rot_dispersion.append(
(pose_theta -
self.ctrld_robot.get_current_orientation())**2)
del self.rot_dispersion[0]
rot_dispersion_average = sum(self.rot_dispersion) / len(
self.rot_dispersion)
# print("{} {}".format(dispersion_average, rot_dispersion_average))
if dispersion_average1 < self.dribble_access_threshold1 and dispersion_average2 < self.dribble_access_threshold2 and rot_dispersion_average < self.dribble_rot_access_threshold:
self.dribble_stage = 1
self.pid.pid_linear(pose_x,
pose_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area)
#目標地付近までドリブル
elif self.dribble_stage == 1:
self.ctrld_robot.set_max_velocity(
config.ROBOT_DRIBBLE_VELOCITY)
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
area = 0.5
if functions.distance_btw_two_points(self.ball_params.get_current_position(),
self.ctrld_robot.get_current_position()) \
> self.ctrld_robot.size_r + area:
self.cmd.vel_surge = 0
self.cmd.vel_sway = 0
self.cmd.omega = 0
self.cmd.dribble_power = 0
self.status.robot_status = "None"
self.dribble_stage = 0
return
if functions.distance_btw_two_points(
self.ball_params.get_current_position(),
[target_x, target_y]) < 0.3:
self.dribble_stage = 2
self.cmd.dribble_power = self.dribble_power
self.pid.pid_straight(target_x,
target_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area,
avoid=False)
#目標地付近になったらドリブラーを止めて引きずる(バックスピン防止)
elif self.dribble_stage == 2:
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
area = 0.5
if functions.distance_btw_two_points(self.ball_params.get_current_position(),
self.ctrld_robot.get_current_position()) \
> self.ctrld_robot.size_r + area \
or functions.distance_btw_two_points(self.ball_params.get_current_position(), [target_x, target_y]) < 0.1:
self.cmd.vel_surge = 0
self.cmd.vel_sway = 0
self.cmd.omega = 0
self.cmd.dribble_power = 0
self.status.robot_status = "None"
self.dribble_stage = 0
return
self.cmd.dribble_power = 0
self.pid.pid_straight(target_x,
target_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area,
avoid=False)
def pass_ball(self,
target_x,
target_y,
should_wait=False,
is_shoot=False,
is_tip_kick=False,
ignore_penalty_area=False,
place=False):
"""
Parameters
----------
should_wait: boolean TrueならKick準備完了後、Kick直前でKickせず待機
is_shoot: boolean Trueならshootの威力でKick、Falseならpassの威力でKick
is_tip_kick boolean Trueならtip kick
place boolean Trueなら目標位置でballが静止するようにKick
"""
point = Point()
point.x = target_x
point.y = target_y
self._pass_target_pos_publisher.publish(point)
distance = math.sqrt(
(target_x - self.ball_params.get_current_position()[0])**2 +
(target_y - self.ball_params.get_current_position()[1])**2)
if distance != 0:
#print self.pass_stage
if self.pass_stage == 0:
prepare_offset = 0.4
pose_x = (
-prepare_offset * target_x + (prepare_offset + distance) *
self.ball_params.get_current_position()[0]) / distance
pose_y = (
-prepare_offset * target_y + (prepare_offset + distance) *
self.ball_params.get_current_position()[1]) / distance
pose_theta = math.atan2(
(target_y - self.ctrld_robot.get_current_position()[1]),
(target_x - self.ctrld_robot.get_current_position()[0]))
a, b, c = functions.line_parameters(
self.ball_params.get_current_position()[0],
self.ball_params.get_current_position()[1], target_x,
target_y)
self.dispersion1.append(
functions.distance_of_a_point_and_a_straight_line(
self.ctrld_robot.get_current_position()[0],
self.ctrld_robot.get_current_position()[1], a, b, c))
#(pose_x - self.ctrld_robot.get_current_position()[0])**2 \
#+ (pose_y - self.ctrld_robot.get_current_position()[1])**2)
del self.dispersion1[0]
self.dispersion2.append(
(pose_x - self.ctrld_robot.get_current_position()[0])**2 \
+ (pose_y - self.ctrld_robot.get_current_position()[1])**2)
del self.dispersion2[0]
dispersion_average1 = sum(self.dispersion1) / len(
self.dispersion1)
dispersion_average2 = sum(self.dispersion2) / len(
self.dispersion2)
if dispersion_average1 > self.kick_feint_threshold1 \
or dispersion_average2 > self.kick_feint_threshold2 \
or should_wait:
pose_theta += np.pi / 3.
self.rot_dispersion.append(
(pose_theta -
self.ctrld_robot.get_current_orientation())**2)
del self.rot_dispersion[0]
rot_dispersion_average = sum(self.rot_dispersion) / len(
self.rot_dispersion)
# print("{} {}".format(dispersion_average, rot_dispersion_average))
if dispersion_average1 < self.kick_access_threshold1 and dispersion_average2 < self.kick_access_threshold2 and rot_dispersion_average < self.kick_rot_access_threshold:
self.pose_theta = pose_theta
# self.status.robot_status = "kick"
if not should_wait:
self._kick_start_time = rospy.Time.now()
self._dribble_start_pos = self.ball_params.get_current_position(
)
self.pass_stage = 1
self.pid.pid_linear(pose_x,
pose_y,
pose_theta,
ignore_penalty_area=ignore_penalty_area)
elif self.pass_stage == 1:
if not is_shoot:
kick_power_x = min(
math.sqrt(distance) * self.const,
config.MAX_KICK_POWER_X)
else:
# 12.0: フィールドの横幅
kick_power_x = config.MAX_KICK_POWER_X
if is_tip_kick:
kick_power_z = min(
math.sqrt(distance) * self.const,
config.MAX_KICK_POWER_Z)
self.kick_xz(power_x=kick_power_x,
power_z=kick_power_z,
ignore_penalty_area=ignore_penalty_area)
else:
if place:
kick_power_x = min(
math.sqrt(distance) * self.const_place,
config.MAX_KICK_POWER_X)
self.kick_xz(power_x=kick_power_x,
ignore_penalty_area=ignore_penalty_area)
"""
def pid_straight(self,
goal_pos_x,
goal_pos_y,
goal_pos_theta,
ignore_penalty_area=False,
avoid=True):
"""
Parameters
----------
goal_pos_x: float 目的地のx座標
goal_pos_y: float 目的地のy座標
goal_pos_theta: float 目的の角度
ignore_penalty_area: boolean Trueならペナルティエリアに進入する、Falseなら進入しない
avoid: boolean Trueなら障害物回避を行う
"""
#dt計算
self.dt = rospy.Time.now() - self.last_loop_time
self.last_loop_time = rospy.Time.now()
#経路再計算
if self.path_replan_flag == True:
self.path_replan_flag = False
self.recursion_count = 0
tmp_x = goal_pos_x
tmp_y = goal_pos_y
if not ignore_penalty_area:
tmp_x, tmp_y = self.avoid_penalty_area(tmp_x, tmp_y)
tmp_x, tmp_y = self.avoid_goal(tmp_x, tmp_y)
if avoid:
tmp_x, tmp_y = self.path_plan(tmp_x, tmp_y)
self.next_goal_pos_x = tmp_x
self.next_goal_pos_y = tmp_y
if self.next_goal_pos_x != self.prev_goal_pos_x or self.next_goal_pos_y != self.prev_goal_pos_y:
self.goal_change_flag = True
if self.goal_change_flag:
self.goal_change_flag = False
self.pid_line_param = functions.line_parameters(
self.ctrld_robot.get_current_position()[0],
self.ctrld_robot.get_current_position()[1],
self.next_goal_pos_x, self.next_goal_pos_y)
self.pid_start_pos = self.ctrld_robot.get_current_position()
dx = self.next_goal_pos_x - self.ctrld_robot.get_current_position()[0]
dy = self.next_goal_pos_y - self.ctrld_robot.get_current_position()[1]
dist = np.sqrt(dx**2 + dy**2)
perpendicular_foot = np.array([0.0, 0.0])
if self.pid_line_param[0] == 0. or self.pid_line_param[1] == 0.:
perpendicular_foot = self.ctrld_robot.get_current_position()
else:
perpendicular_foot[0] = (
-self.ctrld_robot.get_current_position()[1] *
self.pid_line_param[1] +
(self.pid_line_param[1]**2 / self.pid_line_param[0]) *
self.ctrld_robot.get_current_position()[0] -
self.pid_line_param[2]) / (
self.pid_line_param[0] +
self.pid_line_param[1]**2 / self.pid_line_param[0])
perpendicular_foot[1] = (
-self.pid_line_param[0] * perpendicular_foot[0] -
self.pid_line_param[2]) / self.pid_line_param[1]
dist_normal = perpendicular_foot - np.array(
self.ctrld_robot.get_current_position())
dist_projection = np.array(
[self.next_goal_pos_x, self.next_goal_pos_y]) - perpendicular_foot
d_theta = goal_pos_theta - self.ctrld_robot.get_current_orientation()
if d_theta < 0 and abs(d_theta) > np.pi:
d_theta = goal_pos_theta - (
self.ctrld_robot.get_current_orientation() - 2 * np.pi)
if d_theta > 0 and abs(d_theta) > np.pi:
d_theta = (goal_pos_theta -
2 * np.pi) - self.ctrld_robot.get_current_orientation()
self.pid_d_normal = dist_normal - self.pid_p_prev_normal
self.pid_d_projection = dist_projection - self.pid_p_prev_projection
self.pid_d_theta = d_theta - self.pid_p_prev_theta
self.pid_p_prev_normal = dist_normal
self.pid_p_prev_projection = dist_projection
self.pid_p_prev_theta = d_theta
self.pid_p_normal = dist_normal
self.pid_p_projection = dist_projection
self.pid_p_theta = d_theta
V_normal = self.Kpv_normal * self.pid_p_normal + self.Kdv_normal * self.pid_d_normal / self.dt.to_sec(
)
V_projection = self.Kpv_projection * self.pid_p_projection + self.Kdv_projection * self.pid_d_projection / self.dt.to_sec(
)
Vx = V_normal[0] + V_projection[0]
Vy = V_normal[1] + V_projection[1]
Vr = self.Kpr * self.pid_p_theta + self.Kdr * self.pid_d_theta / self.dt.to_sec(
)
max_velocity = self.ctrld_robot.get_max_velocity()
vel_vector = np.array([Vx, Vy])
vel_vector_norm = np.linalg.norm(vel_vector)
if vel_vector_norm > max_velocity:
vel_vector = vel_vector * max_velocity / vel_vector_norm
Vx = vel_vector[0]
Vy = vel_vector[1]
self.cmd.vel_x = Vx
self.cmd.vel_y = Vy
self.cmd.vel_surge = Vx * math.cos(
self.ctrld_robot.get_current_orientation()) + Vy * math.sin(
self.ctrld_robot.get_current_orientation())
self.cmd.vel_sway = -Vx * math.sin(
self.ctrld_robot.get_current_orientation()) + Vy * math.cos(
self.ctrld_robot.get_current_orientation())
self.cmd.omega = Vr
self.cmd.theta = goal_pos_theta
self.prev_goal_pos_x = self.next_goal_pos_x
self.prev_goal_pos_y = self.next_goal_pos_y
def _clip_penalty_area(self, goal_pos_x, goal_pos_y, offset=0.0):
"""
Parameters
----------
goal_pos_x, goal_pos_y 省略
offset: float offsetメートル分ペナルティエリアが各辺に対して広いと仮定して計算する
Return
----------
goal_pos_x, goal_pos_y: (float, float) 自分-目的地の直線とペナルティエリアの交点の座標を返す
"""
in_penalty_area = functions.in_penalty_area((goal_pos_x, goal_pos_y),
offset=offset)
if not in_penalty_area:
return goal_pos_x, goal_pos_y
penal_area_points = {}
penal_area_points['friend'] = {
'left_bot': [-6.0 - offset, -1.2 - offset],
'right_bot': [-4.8 + offset, -1.2 - offset],
'right_top': [-4.8 + offset, 1.2 + offset],
'left_top': [-6.0 - offset, 1.2 + offset],
}
penal_area_points['enemy'] = {
'right_top': [6.0 + offset, 1.2 + offset],
'left_top': [4.8 - offset, 1.2 + offset],
'left_bot': [4.8 + offset, -1.2 - offset],
'right_bot': [6.0 + offset, -1.2 - offset],
}
robo_x, robo_y = self.ctrld_robot.get_current_position()
line_robot_to_goal = \
functions.line_parameters(robo_x, robo_y, goal_pos_x, goal_pos_y)
line_penal_dict = {}
line_penal_dict['friend'] = {}
line_penal_dict['friend'][
'front'] = functions.line_parameters_vector_args(
penal_area_points['friend']['right_bot'],
penal_area_points['friend']['right_top'])
line_penal_dict['friend'][
'top'] = functions.line_parameters_vector_args(
penal_area_points['friend']['left_top'],
penal_area_points['friend']['right_top'],
)
line_penal_dict['friend'][
'bot'] = functions.line_parameters_vector_args(
penal_area_points['friend']['left_bot'],
penal_area_points['friend']['right_bot'],
)
line_penal_dict['enemy'] = {}
line_penal_dict['enemy'][
'front'] = functions.line_parameters_vector_args(
penal_area_points['enemy']['left_bot'],
penal_area_points['enemy']['left_top'])
line_penal_dict['enemy'][
'top'] = functions.line_parameters_vector_args(
penal_area_points['enemy']['left_top'],
penal_area_points['enemy']['right_top'])
line_penal_dict['enemy'][
'bot'] = functions.line_parameters_vector_args(
penal_area_points['enemy']['left_bot'],
penal_area_points['enemy']['right_bot'])
if in_penalty_area == "friend":
line_penal = line_penal_dict['friend']
else:
line_penal = line_penal_dict['enemy']
clip_pos_xys = []
for key in line_penal.keys():
tmp_cross_point = functions.cross_point(line_penal[key],
line_robot_to_goal)
if functions.in_penalty_area(tmp_cross_point, offset=offset + 0.1):
clip_pos_xys.append(tmp_cross_point)
sorted_clip_pos_xys = sorted(
clip_pos_xys,
key=lambda clip_pos_xy: functions.distance_btw_two_points(
clip_pos_xy, self.ctrld_robot.get_current_position()))
return sorted_clip_pos_xys[0]
def avoid_goal(self, goal_pos_x, goal_pos_y):
a, b, c = functions.line_parameters(
self.ctrld_robot.get_current_position()[0],
self.ctrld_robot.get_current_position()[1], goal_pos_x, goal_pos_y)
goal_place_l = [-5.9, -6.3, 0.72, -0.72]
goal_place_r = [6.3, 5.9, 0.72, -0.72]
crossing_flag_l1 = False
crossing_flag_l2 = False
crossing_flag_l3 = False
crossing_flag_l4 = False
crossing_flag_r1 = False
crossing_flag_r2 = False
crossing_flag_r3 = False
crossing_flag_r4 = False
within_goal_flag_l = False
within_goal_flag_r = False
sub_goal_l = [[-5.7, 0.92], [-5.7, -0.92], [-6.5, -0.92], [-6.5, 0.92]]
sub_goal_r = [[5.7, 0.92], [5.7, -0.92], [6.5, -0.92], [6.5, 0.92]]
if a != 0 and b != 0:
if goal_place_l[1] < ((-goal_place_l[2] * b - c) / a) < goal_place_l[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_l[2] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_l[2] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_l1 = True
if goal_place_r[1] < ((-goal_place_r[2] * b - c) / a) < goal_place_r[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_r[2] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_r[2] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_r1 = True
if goal_place_l[3] < ((-goal_place_l[0] * a - c) / b) < goal_place_l[2] \
and (self.ctrld_robot.get_current_position()[1] < ((-goal_place_l[0] * a - c) / b) < goal_pos_y \
or goal_pos_y < ((-goal_place_l[0] * a - c) / b) < self.ctrld_robot.get_current_position()[1]):
crossing_flag_l2 = True
if goal_place_r[3] < ((-goal_place_r[1] * a - c) / b) < goal_place_r[2] \
and (self.ctrld_robot.get_current_position()[1] < ((-goal_place_r[1] * a - c) / b) < goal_pos_y \
or goal_pos_y < ((-goal_place_r[1] * a - c) / b) < self.ctrld_robot.get_current_position()[1]):
crossing_flag_r2 = True
if goal_place_l[1] < ((-goal_place_l[3] * b - c) / a) < goal_place_l[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_l[3] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_l[3] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_l3 = True
if goal_place_r[1] < ((-goal_place_r[3] * b - c) / a) < goal_place_r[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_r[3] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_r[3] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_r3 = True
if goal_place_l[3] < ((-goal_place_l[1] * a - c) / b) < goal_place_l[2] \
and (self.ctrld_robot.get_current_position()[1] < ((-goal_place_l[1] * a - c) / b) < goal_pos_y \
or goal_pos_y < ((-goal_place_l[1] * a - c) / b) < self.ctrld_robot.get_current_position()[1]):
crossing_flag_l4 = True
if goal_place_r[3] < ((-goal_place_r[0] * a - c) / b) < goal_place_r[2] \
and (self.ctrld_robot.get_current_position()[1] < ((-goal_place_r[0] * a - c) / b) < goal_pos_y \
or goal_pos_y < ((-goal_place_r[0] * a - c) / b) < self.ctrld_robot.get_current_position()[1]):
crossing_flag_r4 = True
if goal_place_l[1] < self.ctrld_robot.get_current_position()[0] < goal_place_l[0] \
and goal_place_l[3] < self.ctrld_robot.get_current_position()[1] < goal_place_l[2]:
within_goal_flag_l = True
if goal_place_r[1] < self.ctrld_robot.get_current_position()[0] < goal_place_r[0] \
and goal_place_r[3] < self.ctrld_robot.get_current_position()[1] < goal_place_r[2]:
within_goal_flag_r = True
if (crossing_flag_l1 == True) and (crossing_flag_l2 == True):
return sub_goal_l[0][0], sub_goal_l[0][1]
elif (crossing_flag_l1 == True) and (crossing_flag_l4 == True):
return sub_goal_l[3][0], sub_goal_l[3][1]
elif (crossing_flag_l2 == True) and (crossing_flag_l3 == True):
return sub_goal_l[1][0], sub_goal_l[1][1]
elif (crossing_flag_l3 == True) and (crossing_flag_l4 == True):
return sub_goal_l[2][0], sub_goal_l[2][1]
elif (crossing_flag_l1 == True) and (crossing_flag_l3 == True):
if goal_pos_x >= (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y >= 0:
return sub_goal_l[1][0], sub_goal_l[1][1] - 0.3
elif goal_pos_x < (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y >= 0:
return sub_goal_l[2][0], sub_goal_l[2][1] - 0.3
elif goal_pos_x >= (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y < 0:
return sub_goal_l[0][0], sub_goal_l[0][1] + 0.3
elif goal_pos_x < (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y < 0:
return sub_goal_l[3][0], sub_goal_l[3][1] + 0.3
elif (crossing_flag_l2 == True) and (crossing_flag_l4 == True):
if goal_pos_x >= (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y >= 0:
return sub_goal_l[3][0], sub_goal_l[3][1] + 0.3
elif goal_pos_x < (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y >= 0:
return sub_goal_l[0][0], sub_goal_l[0][1] + 0.3
elif goal_pos_x >= (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y < 0:
return sub_goal_l[2][0], sub_goal_l[2][1] - 0.3
elif goal_pos_x < (goal_place_l[0] +
goal_place_l[1]) / 2. and goal_pos_y < 0:
return sub_goal_l[1][0], sub_goal_l[1][1] - 0.3
elif (within_goal_flag_l == True) and (
(crossing_flag_l1 == True) or (crossing_flag_l3 == True) or
(crossing_flag_l4 == True)):
return sub_goal_l[0][0], 0
if (crossing_flag_r1 == True) and (crossing_flag_r2 == True):
return sub_goal_r[0][0], sub_goal_r[0][1]
elif (crossing_flag_r1 == True) and (crossing_flag_r4 == True):
return sub_goal_r[3][0], sub_goal_r[3][1]
elif (crossing_flag_r2 == True) and (crossing_flag_r3 == True):
return sub_goal_r[1][0], sub_goal_r[1][1]
elif (crossing_flag_r3 == True) and (crossing_flag_r4 == True):
return sub_goal_r[2][0], sub_goal_r[2][1]
elif (crossing_flag_r1 == True) and (crossing_flag_r3 == True):
if goal_pos_x <= (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y >= 0:
return sub_goal_r[1][0], sub_goal_r[1][1] - 0.3
elif goal_pos_x > (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y >= 0:
return sub_goal_r[2][0], sub_goal_r[2][1] - 0.3
elif goal_pos_x <= (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y < 0:
return sub_goal_r[0][0], sub_goal_r[0][1] + 0.3
elif goal_pos_x > (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y < 0:
return sub_goal_r[3][0], sub_goal_r[3][1] + 0.3
elif (crossing_flag_r2 == True) and (crossing_flag_r4 == True):
if goal_pos_x <= (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y >= 0:
return sub_goal_r[3][0], sub_goal_r[3][1] + 0.3
elif goal_pos_x > (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y >= 0:
return sub_goal_r[0][0], sub_goal_r[0][1] + 0.3
elif goal_pos_x <= (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y < 0:
return sub_goal_r[2][0], sub_goal_r[2][1] - 0.3
elif goal_pos_x > (goal_place_r[0] +
goal_place_r[1]) / 2. and goal_pos_y < 0:
return sub_goal_r[1][0], sub_goal_r[1][1] - 0.3
elif (within_goal_flag_r == True) and (
(crossing_flag_r1 == True) or (crossing_flag_r3 == True) or
(crossing_flag_r4 == True)):
return sub_goal_r[0][0], 0
return goal_pos_x, goal_pos_y
def avoid_penalty_area(self, goal_pos_x, goal_pos_y):
a, b, c = functions.line_parameters(
self.ctrld_robot.get_current_position()[0],
self.ctrld_robot.get_current_position()[1], goal_pos_x, goal_pos_y)
goal_place_l = [-4.8, -6.0, 1.2, -1.2]
goal_place_r = [6.0, 4.8, 1.2, -1.2]
crossing_flag_l1 = False
crossing_flag_l2 = False
crossing_flag_l3 = False
crossing_flag_r1 = False
crossing_flag_r2 = False
crossing_flag_r3 = False
sub_goal_l = [[-4.5, 1.5], [-4.5, -1.5]]
sub_goal_r = [[4.5, 1.5], [4.5, -1.5]]
if a != 0 and b != 0:
if goal_place_l[1] < ((-goal_place_l[2] * b - c) / a) < goal_place_l[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_l[2] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_l[2] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_l1 = True
if goal_place_r[1] < ((-goal_place_r[2] * b - c) / a) < goal_place_r[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_r[2] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_r[2] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_r1 = True
if goal_place_l[3] < ((-goal_place_l[0] * a - c) / b) < goal_place_l[2] \
and (self.ctrld_robot.get_current_position()[1] < ((-goal_place_l[0] * a - c) / b) < goal_pos_y \
or goal_pos_y < ((-goal_place_l[0] * a - c) / b) < self.ctrld_robot.get_current_position()[1]):
crossing_flag_l2 = True
if goal_place_r[3] < ((-goal_place_r[1] * a - c) / b) < goal_place_r[2] \
and (self.ctrld_robot.get_current_position()[1] < ((-goal_place_r[1] * a - c) / b) < goal_pos_y \
or goal_pos_y < ((-goal_place_r[1] * a - c) / b) < self.ctrld_robot.get_current_position()[1]):
crossing_flag_r2 = True
if goal_place_l[1] < ((-goal_place_l[3] * b - c) / a) < goal_place_l[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_l[3] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_l[3] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_l3 = True
if goal_place_r[1] < ((-goal_place_r[3] * b - c) / a) < goal_place_r[0] \
and (self.ctrld_robot.get_current_position()[0] < ((-goal_place_r[3] * b - c) / a) < goal_pos_x \
or goal_pos_x < ((-goal_place_r[3] * b - c) / a) < self.ctrld_robot.get_current_position()[0]):
crossing_flag_r3 = True
if (crossing_flag_l1 == True) and (crossing_flag_l2 == True):
return sub_goal_l[0][0], sub_goal_l[0][1]
elif (crossing_flag_l2 == True) and (crossing_flag_l3 == True):
return sub_goal_l[1][0], sub_goal_l[1][1]
elif (crossing_flag_l1 == True) and (crossing_flag_l3 == True):
if self.ctrld_robot.get_current_position()[1] > 0:
return sub_goal_l[0][0], sub_goal_l[0][1]
else:
return sub_goal_l[1][0], sub_goal_l[1][1]
if (crossing_flag_r1 == True) and (crossing_flag_r2 == True):
return sub_goal_r[0][0], sub_goal_r[0][1]
elif (crossing_flag_r2 == True) and (crossing_flag_r3 == True):
return sub_goal_r[1][0], sub_goal_r[1][1]
elif (crossing_flag_r1 == True) and (crossing_flag_r3 == True):
if self.ctrld_robot.get_current_position()[1] > 0:
return sub_goal_r[0][0], sub_goal_r[0][1]
else:
return sub_goal_r[1][0], sub_goal_r[1][1]
return goal_pos_x, goal_pos_y
