def adjust_lin_velocity(self, v):
objects = self.laser_data['robots'] + self.laser_data['targets']
for o in objects:
la = LinAng()
la.setAllCoords(o.center.getAllCoords())
if 0 < la.linear < sp.min_distance_to_robot and abs(
la.angular) < 45:
print ">>>>> STOP <<<<<"
return 0.1
return v
def process(self, now):
t = now.secs + (now.nsecs / 1E+9)
delta_time, self.time = t - self.time, t
#self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
if self.has_target and self.has_robot: #Escorting
self.status = 3
robot_dist_norm = abs(
self.robot.center.radius -
sp.desired_distance_to_robot) / sp.sensor_cone_radius
# primeiro verifica se jah estah circunavegando...
if abs(self.target.center.radius - sp.desired_radius) < 0.5:
self.linear_speed = self.linear_speed * robot_dist_norm
#pacr = -1.0
#if self.target.angular <= sp.min_ctrl_angle:
# pacr = -1.0
#elif self.target.angular >= sp.max_ctrl_angle:
# pacr = 1.0
#else:
# pacr = 2 * (self.target.angular - sp.min_ctrl_angle) / (sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#self.propAngularCoefToRobot = pacr
#pacr = prop_ang_coef(self.laser_data["robots"][0])
#self.linear_speed = 2 * sp.linear_velocity
# self.proximityCoefToRobot = self.getProximityCoefToRobot() # * self.getPropAngularCoefToRobot()
# self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.propAngularCoefToRobot * self.proximityCoefToRobot
elif self.has_target and not self.has_robot: #Circulating
self.status = 2
tcoords = LinAng()
tcoords.setAllCoords(self.target.center.getAllCoords())
radius_coef = 1 / (1 + exp(-(tcoords.linear - sp.desired_radius) /
sp.boltzmann_time_constant))
#circRadiusCoef, appRadiusCoef = self.getTransitionCoefs()
kpA = prop_ang_coef(tcoords.angular)
error_radius = sp.desired_radius - tcoords.linear
#error_radius = sp.desired_radius - self.target.linear
self.app_radius = -(tcoords.x**2 + tcoords.y**2 - sp.desired_radius
**2) / (2 * (sp.desired_radius - tcoords.y))
#appRadius = -(self.target.x**2 + self.target.y**2 - sp.desired_radius**2) / (2 * (sp.desired_radius - self.target.y))
circ_radius = sp.desired_radius + error_radius
#circRadius = sp.desired_radius + error_radius
self.performed_radius = (
1 - radius_coef) * circ_radius + radius_coef * self.app_radius
# ou radius = circ_radius + radius * (app_radius - circ_radius)
ang_vel = self.linear_speed / self.performed_radius
hl_ang_vel = boundv(sp.min_angular_velocity, ang_vel,
sp.max_angular_velocity)
hl_ang_vel += kpA * sp.delta_angular
self.linear_speed = sp.linear_velocity
self.angular_speed = boundv(sp.min_angular_velocity, hl_ang_vel,
sp.max_angular_velocity)
elif not self.has_target and self.has_robot: #Avoiding
self.status = 1
speed = (sp.linear_velocity + self.linear_speed) / 2.0
if speed > 0:
time_to_collide = abs(self.robot.center.radius -
sp.desired_distance_to_robot) / speed
if (time_to_collide < 5) and abs(
(self.angular_speed * time_to_collide) % 180) < 15:
self.linear_speed = self.linear_speed * (
0.8 - 0.1 * (5 - time_to_collide))
else: #status == "Seeking"
self.status = 0
self.linear_speed = sp.linear_velocity
decimal_random_rate = np.random.random() / 10.0
self.angular_speed = sp.angular_velocity * (1 +
decimal_random_rate)
self.linear_speed = boundv(0.0, self.linear_speed,
sp.max_linear_velocity)
self.linear_speed = self.adjust_lin_velocity(self.linear_speed)
print "[" + str(t) + "] Robo=p" + str(self.num) + " status=" + str(
self.status)
self.update()
