dwb local plannerの実装を理解していきます。
自分で考える
source code解析をする前に自分で作ることを考えるとします。
前回、詳解確率ロボティクスのPythonをお手本として回転のみでgoalを目指す実装をしました。
障害物回避を実装します。
詳解確率ロボティクスのPythonはrobotの動作がわかりやすく拡張もやりやすいので利用させていただきます。
整理する
遠くにある障害物は無視します。
通ろうとしていた経路の近くの衝突してしまう障害物に対して少し余裕を持って避ける動作を実装します。
output
障害物を回避します。
goalに到着の条件を実装していないためgoalを目指し続けます。
自分の大きさ、形状を考慮していません。次の記事からはnav2の解析をします。その後に自分の大きさを考慮した実装にします。
sensorでobstacleの位置を把握するところまで実装すればobstacleの位置をworld座標系からrobot座標系にするという変換を削除できます。そこまでやれる時間あるかなー。
実装
実装です。
dwb_local_planner_002.cpp
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
import time
# ------------------------------------------------------------------------------
PI = 3.141592
DEGREE_1 = PI/180
DEGREE_179 = DEGREE_1*179
ANGULAR_VELOCITY_MAX = 0.1
COEFF = 0.2
COEFF_OB = 1.0
# ------------------------------------------------------------------------------
## x : robot position x
## y : robot position y
## yaw : robot position yaw
class Robot:
def __init__( self, pose, nu, omega, goal, obstacles, color="black" ):
self.pose = pose
self.r = 0.2
self.color = color
self.poses = [pose]
self.nu = nu
self.omega = omega
self.goal = goal
self.angle = 0.0
self.diff_angle = 0.0
self.g_x_in_robot_flame = 0.0
self.g_y_in_robot_flame = 0.0
self.tmp_x = 0.0
self.tmp_y = 0.0
self.obstacles = obstacles
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))
elems.append(ax.text( 2, -2, f'g_x - x: {self.tmp_x:.3f}', size=15 ))
elems.append(ax.text( 2, -3, f'g_y - y: {self.tmp_y:.3f}', size=15 ))
elems.append(ax.text( 2, -4, f'g_x_in_robot_flame: {self.g_x_in_robot_flame:.3f}', size=15 ))
elems.append(ax.text( 2, -5, f'g_y_in_robot_flame: {self.g_y_in_robot_flame:.3f}', size=15 ))
elems.append(ax.text( 2, -6, f'angle: {degrees(self.angle):.3f}', size=15 ))
elems.append(ax.text( 2, -7, f'yaw: {degrees(self.pose[2]):.3f}', size=15 ))
elems.append(ax.text( 2, -8, f'diff_angle: {degrees(self.diff_angle):.3f}', size=15 ))
elems.append(ax.text( 2, -9, f'omega: {self.omega:.3f}', size=15 ))
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")
@classmethod
def state_transition(cls, nu, omega, time, pose):
t0 = pose[2]
if math.fabs(omega) < 1e-10:
return pose + np.array( [nu*math.cos(t0),
nu*math.sin(t0),
omega ] ) * time
else:
return pose + np.array( [nu/omega*(math.sin(t0 + omega*time) - math.sin(t0)),
nu/omega*(-math.cos(t0 + omega*time) + math.cos(t0)),
omega*time ] )
def one_step(self, time_interval):
# nu, omega = self.run()
nu, omega = self.dwa()
self.pose = self.state_transition( nu, omega, time_interval, self.pose )
def run( self ):
return self.nu, self.omega
def go_stragiht( self ):
return self.nu, self.omega
def dwa( self ):
# ------------------------------------------------------------------------------
# robot pose in world flame
x, y, yaw = self.pose
## 0(deg) <= yaw < 360(deg)
if (2*PI) <= yaw:
yaw = yaw - 2*PI
if yaw < 0:
yaw = yaw + 2*PI
self.pose[2] = yaw
# ------------------------------------------------------------------------------
# goal in world flame
goal = self.goal
g_x = goal.pos[0]
g_y = goal.pos[1]
print(f'x: {g_x},y: {g_y}')
## gradient = slope
angle = math.atan2( g_y - y, g_x - x )
## 0(deg) <= angle < 360(deg)
## atan2 return value: -PI < angle <= PI
# if (2*PI) <= angle:
# angle = angle - 2*PI
if angle < 0:
angle = angle + 2*PI
self.angle = angle
print(f'robot yaw: {yaw}')
print(f'angle (rad): {angle}')
print(f'angle (deg): {math.degrees(angle)}')
print(f'robot yaw - angle difference: {math.degrees(yaw - angle)}')
# ------------------------------------------------------------------------------
# goal in robot flame
tmp_x = g_x - x
tmp_y = g_y - y
g_x_in_robot_flame = tmp_x * cos(-yaw) - tmp_y * sin(-yaw)
g_y_in_robot_flame = tmp_x * sin(-yaw) + tmp_y * cos(-yaw)
## for debug
self.g_x_in_robot_flame = g_x_in_robot_flame
self.g_y_in_robot_flame = g_y_in_robot_flame
self.tmp_x = tmp_x
self.tmp_y = tmp_y
# ------------------------------------------------------------------------------
# obstacles in world flame
obstacles = self.obstacles
# ------------------------------------------------------------------------------
# determine omega towards the goal
diff_angle = angle - yaw
self.diff_angle = diff_angle
## the goal is on the left
if 0 < g_y_in_robot_flame:
if abs(diff_angle) < DEGREE_1:
## move forward
change_omega = 0
else:
change_omega = abs(diff_angle) * COEFF
## the goal is on the right
if g_y_in_robot_flame < 0:
if DEGREE_179 < abs(diff_angle) and abs(diff_angle) < DEGREE_181:
## force clockwise
change_omega = ANGULAR_VELOCITY_MAX
else:
change_omega = - abs(diff_angle) * COEFF
# ------------------------------------------------------------------------------
# move around obstacle
# closest ranking
dist_ob = []
ob_in_robot_flame = []
for obstacle in obstacles:
# obstacles in robot flame
tmp_x = obstacle.pos[0] - x
tmp_y = obstacle.pos[1] - y
dist = sqrt( tmp_x * tmp_x + tmp_y * tmp_y )
ob_x_in_robot_flame = tmp_x * cos(-yaw) - tmp_y * sin(-yaw)
ob_y_in_robot_flame = tmp_x * sin(-yaw) + tmp_y * cos(-yaw)
dist_ob.append( (dist, ob_x_in_robot_flame, ob_y_in_robot_flame) )
# print( dist_ob )
dist_ob.sort(key=lambda x: x[0])
# print( dist_ob )
print(f'robot yaw: {yaw}')
print( f'dist: {dist_ob[0][0]}' )
print( f'ob_y_in_robot_flame: {dist_ob[0][2]}' )
# distance between robot and obstacle
if dist_ob[0][0] < 1.0:
if 0 < dist_ob[0][2]:
change_omega = - (1 / dist + 0.1) * COEFF_OB
if dist_ob[0][2] < 0:
change_omega = + (1 / dist + 0.1) * COEFF_OB
self.omega = change_omega
return self.nu, self.omega
# ------------------------------------------------------------------------------
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.goal = []
self.obstacles = []
def append_landmark(self, landmark):
landmark.id = len(self.landmarks)
self.landmarks.append(landmark)
def append_goal( self, goal ):
self.goal.append( goal )
def append_obstacle( self, obstacle ):
self.obstacles.append( obstacle )
def draw( self, ax, elems ):
for goal in self.goal: 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 ):
c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="*", label="goal", color="red")
elems.append(c)
elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
# ------------------------------------------------------------------------------
class Obstacle:
def __init__( self, x, y, r ):
self.pos = np.array([x, y]).T
self.id = None
self.radius = r
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))
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 )
# world = World( 120, 0.04 )
# world = World( 120, 0.01 )
m = Map()
goal = Goal( 4, 8 )
m.append_goal( goal )
obstacle = Obstacle( 5, 6, 1 )
m.append_obstacle( obstacle )
obstacle = Obstacle( 3, 5, 1 )
m.append_obstacle( obstacle )
world.append( m )
robot = Robot( np.array([0, -3, 0]).T, nu=0.2, omega=0.0, goal=goal, obstacles=m.obstacles )
# robot = Robot( np.array([0, 0, 1]).T, nu=0.2, omega=0.0, goal=goal )
# robot = Robot( np.array([0, 0, 0]).T, nu=1.0, omega=1.0, goal=goal )
world.append( robot )
world.draw()
まとめ
このくらいの複雑さからcontinuous integrationを組み込まないと検証できなくなる気がします。
入力と期待値の幅を用意することになる気がします。
それなりに厳しいobstacleの配置にしたけど、実機に搭載は怖くて出来ない。
広告
IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト
