あるrobotがある多角形の中にいるか判定します。
今回は、複数の多角形に対応します。
前の記事はこちら。
実装
下記、実装です。
Robot classのdraw関数のfor文で処理してます。
上記の説明と少し異なっているのはご容赦ください。
crossing_number_algorithm_multiple_areas.py
import matplotlib
# matplotlib.use('nbagg')
import matplotlib.animation as anm
import matplotlib.pyplot as plt
import math
from math import sin, cos, degrees, sqrt
import matplotlib.patches as patches
import numpy as np
import random
# ------------------------------------------------------------------------------
## x : robot position x
## y : robot position y
## yaw : robot position yaw
class Robot:
def __init__( self, pose, nu, omega, goals, obstacles, areas, color="black" ):
self.pose = pose
self.r = 0.2
self.color = color
self.poses = [pose]
self.nu = nu
self.omega = omega
self.goals = goals
self.obstacles = obstacles
self.areas = areas
def draw(self, ax, elems):
x, y, yaw = self.pose
xn = x + self.r * math.cos(yaw)
yn = y + self.r * math.sin(yaw)
##
elems += ax.plot([x,xn], [y,yn], color=self.color)
## robot's orientation
ax.plot([x,xn], [y,yn], color=self.color)
c = patches.Circle(xy=(x, y), radius=self.r, fill=False, color=self.color)
elems.append(ax.add_patch(c))
self.poses.append(self.pose)
elems += ax.plot([e[0] for e in self.poses], [e[1] for e in self.poses], linewidth=0.5, color="black")
## crossing number algorithm
goals = self.goals
# print(f'goals: {goals}')
areas = self.areas
print(f'areas: {areas}')
for i in range( len(goals) ):
g_x = goals[i].pos[0]
g_y = goals[i].pos[1]
crossing_counter = 0
for area in areas:
print(f'area {area}')
for index in range( len(area.positions) ):
print(f'----------------------------------------')
print(f'index: {index}')
# obstacles in robot flame
a = area.positions[index]
x_on_line = 0
if index == len(area.positions)-1:
b = area.positions[0]
else:
b = area.positions[index+1]
a_x = a.pos[0]
b_x = b.pos[0]
a_y = a.pos[1]
b_y = b.pos[1]
print( f'g_y: {g_y} a_x: {a_x} a_y: {a_y} b_X: {b_x} b_y: {b_y}' )
if (b_x - a_x) == 0:
x_on_line = a_x
if g_x < x_on_line:
print( f'g_x < x_on_line' )
if ( a_y < g_y and g_y < b_y ) or ( b_y < g_y and g_y < a_y ):
crossing_counter = crossing_counter + 1
print( f'crossing_counter: {crossing_counter}' )
else:
slope = (b_y - a_y) / (b_x - a_x)
constant = a_y - slope * a_x
# g_y = slope * x + constant
# slope * x = g_y - constant
x_on_line = (g_y - constant) / slope
print( f'slope: {slope} constant: {constant} x_on_line: {x_on_line}' )
if g_x < x_on_line:
print( f'g_x < x_on_line' )
if ( a_y < g_y and g_y < b_y ) or ( b_y < g_y and g_y < a_y ):
crossing_counter = crossing_counter + 1
print( f'crossing_counter: {crossing_counter}' )
if (crossing_counter % 2) != 0:
c = ax.scatter(g_x, g_y, s=100, marker="*", label="goal", color="red")
elems.append(c)
print( f'g_x: {g_x} g_y: {g_y} in' )
# print( f'crossing_counter: {crossing_counter}')
# quit()
else:
c = ax.scatter(g_x, g_y, s=100, marker="*", label="goal", color="blue")
elems.append(c)
print( f'g_x: {g_x} g_y: {g_y} out' )
# print( f'crossing_counter: {crossing_counter}')
# quit()
def one_step(self, time_interval):
nu, omega = self.crossing_number_algorithm()
def crossing_number_algorithm( self ):
return 0, 0
# ------------------------------------------------------------------------------
class World:
def __init__( self, time_span, time_interval ):
self.objects = []
self.time_span = time_span
self.time_interval = time_interval
self.file_name_counter = 0
def append( self,obj ):
self.objects.append(obj)
def draw(self):
fig = plt.figure(figsize=(8,8)) # fig = plt.figure(figsize=(4,4))
## make axes
ax = fig.add_subplot(111)
ax.set_aspect('equal')
ax.set_xlim(-10,10)
ax.set_ylim(-10,10)
ax.set_xlabel("X",fontsize=10)
ax.set_ylabel("Y",fontsize=10)
elems = []
tmp_1 = int( self.time_span/self.time_interval ) + 1
tmp_2 = int( self.time_interval )
self.ani = anm.FuncAnimation(fig, self.one_step, fargs=(elems, ax), frames=tmp_1, interval=tmp_2, repeat=False)
plt.show()
def one_step(self, i, elems, ax):
while elems: elems.pop().remove()
time_str = "t = %.2f[s]" % (self.time_interval*i)
elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
##
elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
for obj in self.objects:
obj.draw(ax, elems)
if hasattr(obj, "one_step"): obj.one_step(self.time_interval)
# ------------------------------------------------------------------------------
class Map:
def __init__(self):
self.landmarks = []
self.goals = []
self.obstacles = []
self.positions = []
self.areas = []
def append_landmark(self, landmark):
landmark.id = len(self.landmarks)
self.landmarks.append(landmark)
def append_goal( self, goal ):
self.goals.append( goal )
def append_obstacle( self, obstacle ):
self.obstacles.append( obstacle )
def append_position( self, position ):
self.positions.append( position )
def append_area( self, area ):
print(f'area: {area}')
self.areas.append( area )
self.positions.clear()
print(f'self.areas: {self.areas}')
def draw( self, ax, elems ):
for goal in self.goals: goal.draw(ax, elems)
for obstacle in self.obstacles: obstacle.draw(ax, elems)
# ------------------------------------------------------------------------------
class Goal:
def __init__( self, x, y ):
self.pos = np.array([x, y]).T
self.id = None
def draw( self, ax, elems ):
print("")
# ------------------------------------------------------------------------------
class Obstacle:
def __init__( self, x, y ):
self.pos = np.array([x, y]).T
self.id = None
def draw( self, ax, elems ):
c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="o", label="obstacle", color="black")
elems.append(c)
elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
# ------------------------------------------------------------------------------
class Position:
def __init__( self, x, y ):
self.pos = np.array([x, y]).T
self.id = None
def draw( self, ax, elems ):
c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="o", label="obstacle", color="black")
elems.append(c)
elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
# ------------------------------------------------------------------------------
## define area as simple polygon
class Area:
def __init__( self, positions ):
self.positions = positions
# self.position = np.array([x, y]).T
self.id = None
self.max_speed = 0.3 ## (m/s)
def draw( self, ax, elems ):
print("")
# c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="o", label="obstacle", color="black")
# elems.append(c)
# elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
# c = patches.Circle(xy=(self.pos[0], self.pos[1]), radius=self.radius, fill=False, color="blue")
# elems.append(ax.add_patch(c))
# ------------------------------------------------------------------------------
if __name__ == '__main__':
## 0.01 : very slow
# world = World( 15, 0.1 )
# world = World( 120, 0.1 )
# world = World( 200, 0.2 )
print('world before')
world = World( 20, 0.2 )
print('world after')
# world = World( 120, 0.04 )
# world = World( 120, 0.01 )
print('map before')
m = Map()
print('map after')
for i in range(2000):
goal = Goal( random.uniform(-10, 10), random.uniform(-10, 10) )
m.append_goal( goal )
areas = []
positions = []
position = Position( -9, -9 ); positions.append( position )
position = Position( -8, -8 ); positions.append( position )
position = Position( -6, -9.5 ); positions.append( position )
position = Position( -7, -4 ); positions.append( position )
position = Position( -7.5, -7 ); positions.append( position )
position = Position( -8.5, -6 ); positions.append( position )
area = Area( positions.copy() ); areas.append( area )
positions.clear()
position = Position( -5, -8 ); positions.append( position )
position = Position( -4, -7 ); positions.append( position )
position = Position( -2, -8.5 ); positions.append( position )
position = Position( -3, -3 ); positions.append( position )
position = Position( -3.5, -6 ); positions.append( position )
position = Position( -4.5, -5 ); positions.append( position )
area = Area( positions.copy() ); areas.append( area )
positions.clear()
position = Position( -1, -7 ); positions.append( position )
position = Position( 0, -6 ); positions.append( position )
position = Position( 2, -7.5 ); positions.append( position )
position = Position( 1, -2 ); positions.append( position )
position = Position( 0.5, -5 ); positions.append( position )
position = Position( -0.5, -4 ); positions.append( position )
area = Area( positions.copy() ); areas.append( area )
positions.clear()
world.append( m )
robot = Robot( np.array([0, -3, 0]).T, nu=0.2, omega=0.0, goals=m.goals, obstacles=m.obstacles, areas=areas )
world.append( robot )
world.draw()
動作確認
大丈夫そうです。
広告
IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト
