def _bounding_circle(self):
# NOTE: this method is meant to give a quick bounding circle
# the circle calculated may not be the "minimum bounding circle"
c = self._centroidish()
r = 0.0
for v in self.vertexes:
d = linalg.distance(c, v)
if d > r: r = d
return c, r
def _bounding_circle( self ):
# NOTE: this method is meant to give a quick bounding circle
# the circle calculated may not be the "minimum bounding circle"
c = self._centroidish()
r = 0.0
for v in self.vertexes:
d = linalg.distance( c, v )
if d > r: r = d
return c, r
def _draw_rich_traverse_path_to_frame(self):
# when robot is moving fast, draw small, opaque dots
# when robot is moving slow, draw large, transparent dots
d_min, d_max = 0.0, 0.01574 # possible distances between dots
r_min, r_max = 0.007, 0.02 # dot radius
a_min, a_max = 0.3, 0.55 # dot alpha value
m_r = (r_max - r_min) / (d_min - d_max)
b_r = r_max - m_r * d_min
m_a = (a_max - a_min) / (r_min - r_max)
b_a = a_max - m_a * r_min
prev_posn = self.traverse_path[0]
frame = self.viewer.current_frame
for posn in self.traverse_path[1::1]:
d = linalg.distance(posn, prev_posn)
r = (m_r * d) + b_r
a = (m_a * r) + b_a
frame.add_circle(pos=posn, radius=r, color="black", alpha=a)
prev_posn = posn
def _draw_rich_traverse_path_to_frame( self ):
# when robot is moving fast, draw small, opaque dots
# when robot is moving slow, draw large, transparent dots
d_min, d_max = 0.0, 0.01574 # possible distances between dots
r_min, r_max = 0.007, 0.02 # dot radius
a_min, a_max = 0.3, 0.55 # dot alpha value
m_r = ( r_max - r_min ) / ( d_min - d_max )
b_r = r_max - m_r*d_min
m_a = ( a_max - a_min ) / ( r_min - r_max )
b_a = a_max - m_a*r_min
prev_posn = self.traverse_path[0]
frame = self.viewer.current_frame
for posn in self.traverse_path[1::1]:
d = linalg.distance( posn, prev_posn )
r = ( m_r*d ) + b_r
a = ( m_a*r ) + b_a
frame.add_circle( pos = posn,
radius = r,
color = "black",
alpha = a)
prev_posn = posn
def add_new_goal(self, world=None):
"""
Adds a new goal
"""
i = 3000
max_dist = self.cfg["goal"]["max_distance"]
while i > 0:
i -= 1
goal = self.__generate_new_goal()
intersects = self.__check_obstacle_intersections(goal)
if not intersects and world is None:
self.current_goal = goal
break
elif not intersects and world is not None: # add a new goal not far from the robot
rob_x, rob_y = world.robots[
0].supervisor.estimated_pose.vposition()
distance_to_robot = linalg.distance([rob_x, rob_y], goal)
if distance_to_robot < 1 and distance_to_robot > 0.5: # being able to a new goal not far from the robot
self.current_goal = goal
break
if rob_x**2 + rob_y**2 > max_dist**2: # not being able to a new goal near the robot
self.current_goal = goal
break
def check_nearness(geometry1, geometry2):
c1, r1 = geometry1.bounding_circle
c2, r2 = geometry2.bounding_circle
return linalg.distance(c1, c2) <= r1 + r2
def _distance_to_goal(self):
return linalg.distance(self.supervisor.estimated_pose.vposition(),
self.supervisor.goal)
def condition_at_goal(self):
return linalg.distance(self.supervisor.estimated_pose.vposition(),
self.supervisor.goal) < D_STOP
def condition_at_goal(self):
return linalg.distance(self.supervisor.estimated_pose.vposition(), self.supervisor.goal) < self.cfg["goal_reached_distance"]
def _distance_to_goal( self ): return linalg.distance( self.supervisor.estimated_pose.vposition(), self.supervisor.goal )
def condition_at_goal( self ): return linalg.distance( self.supervisor.estimated_pose.vposition(), self.supervisor.goal ) < D_STOP
def check_nearness( geometry1, geometry2 ): c1, r1 = geometry1.bounding_circle c2, r2 = geometry2.bounding_circle return linalg.distance( c1, c2 ) <= r1 + r2
