diff --git a/gym_duckietown/objects.py b/gym_duckietown/objects.py index 6d4f16a3..7fc9a106 100644 --- a/gym_duckietown/objects.py +++ b/gym_duckietown/objects.py @@ -130,6 +130,9 @@ def __init__(self, obj, domain_rand, safety_radius_mult, wheel_dist, self.robot_width = robot_width self.robot_length = robot_length + # Robot Safety circle radius + self.agent_safety_rad = (max(robot_length, robot_width) / 2) * safety_radius_mult + # FIXME: this does not follow the same signature as WorldOb def step(self, delta_time, closest_curve_point, objects): """ @@ -137,8 +140,7 @@ def step(self, delta_time, closest_curve_point, objects): """ # Find the curve point closest to the agent, and the tangent at that point - closest_point, closest_tangent = closest_curve_point(self.pos, self.angle) - + closest_point, closest_tangent = closest_curve_point(self.pos, self.angle) iterations = 0 lookup_distance = self.follow_dist @@ -163,6 +165,16 @@ def step(self, delta_time, closest_curve_point, objects): dot = np.dot(self.get_right_vec(self.angle), point_vec) steering = self.gain * -dot + # checking for other moving duckiebots to slow down if another bot is moving in front + for obj in objects: + if not obj.static and obj.kind == "duckiebot": + if abs(obj.pos[0] - self.pos[0]) <0.12: # to check if this object is on my same lane + collision_penalty = abs(obj.proximity(self.pos, self.agent_safety_rad)) + if collision_penalty > 0 : + # this means we are approaching and we need to slow down + self.velocity *= collision_penalty + break + self._update_pos([self.velocity, steering], delta_time) def get_dir_vec(self, angle):