投稿日: 投稿者: 1x2x3

dwb local planner 2

dwb local plannerの実装を理解していきます。

自分で考える

source code解析をする前に自分で作ることを考えるとします。

前回、詳解確率ロボティクスのPythonをお手本として回転のみでgoalを目指す実装をしました。
障害物回避を実装します。

詳解確率ロボティクスのPythonはrobotの動作がわかりやすく拡張もやりやすいので利用させていただきます。

整理する

遠くにある障害物は無視します。
通ろうとしていた経路の近くの衝突してしまう障害物に対して少し余裕を持って避ける動作を実装します。

output

障害物を回避します。
goalに到着の条件を実装していないためgoalを目指し続けます。
自分の大きさ、形状を考慮していません。次の記事からはnav2の解析をします。その後に自分の大きさを考慮した実装にします。

sensorでobstacleの位置を把握するところまで実装すればobstacleの位置をworld座標系からrobot座標系にするという変換を削除できます。そこまでやれる時間あるかなー。

実装

実装です。

dwb_local_planner_002.cpp

  1. import matplotlib
  2. # matplotlib.use('nbagg')
  3. import matplotlib.animation as anm
  4. import matplotlib.pyplot as plt
  5. import math
  6. from math import sin, cos, degrees, sqrt
  7. import matplotlib.patches as patches
  8. import numpy as np
  9. import random
  10. import time
  12. # ------------------------------------------------------------------------------
  14. PI = 3.141592
  15. DEGREE_1 = PI/180
  16. DEGREE_179 = DEGREE_1*179
  17. ANGULAR_VELOCITY_MAX = 0.1
  18. COEFF = 0.2
  19. COEFF_OB = 1.0
  21. # ------------------------------------------------------------------------------
  23. ## x : robot position x
  24. ## y : robot position y
  25. ## yaw : robot position yaw
  27. class Robot:
  28.     def __init__( self, pose, nu, omega, goal, obstacles, color="black" ):
  29.         self.pose = pose
  30.         self.r = 0.2
  31.         self.color = color
  32.         self.poses = [pose]
  33.         self.nu = nu
  34.         self.omega = omega
  35.         self.goal = goal
  36.         self.angle = 0.0
  37.         self.diff_angle = 0.0
  38.         self.g_x_in_robot_flame = 0.0
  39.         self.g_y_in_robot_flame = 0.0
  40.         self.tmp_x = 0.0
  41.         self.tmp_y = 0.0
  42.         self.obstacles = obstacles
  44.     def draw(self, ax, elems):
  45.         x, y, yaw = self.pose
  47.         xn = x + self.r * math.cos(yaw)
  48.         yn = y + self.r * math.sin(yaw)
  49.         ##
  50.         elems += ax.plot([x,xn], [y,yn], color=self.color)
  51.         ## robot's orientation
  52.         ax.plot([x,xn], [y,yn], color=self.color)
  53.         c = patches.Circle(xy=(x, y), radius=self.r, fill=False, color=self.color)
  54.         elems.append(ax.add_patch(c))
  55.         elems.append(ax.text( 2, -2, f'g_x - x: {self.tmp_x:.3f}', size=15 ))
  56.         elems.append(ax.text( 2, -3, f'g_y - y: {self.tmp_y:.3f}', size=15 ))
  57.         elems.append(ax.text( 2, -4, f'g_x_in_robot_flame: {self.g_x_in_robot_flame:.3f}', size=15 ))
  58.         elems.append(ax.text( 2, -5, f'g_y_in_robot_flame: {self.g_y_in_robot_flame:.3f}', size=15 ))
  59.         elems.append(ax.text( 2, -6, f'angle: {degrees(self.angle):.3f}', size=15 ))
  60.         elems.append(ax.text( 2, -7, f'yaw: {degrees(self.pose[2]):.3f}', size=15 ))
  61.         elems.append(ax.text( 2, -8, f'diff_angle: {degrees(self.diff_angle):.3f}', size=15 ))
  62.         elems.append(ax.text( 2, -9, f'omega: {self.omega:.3f}', size=15 ))
  64.         self.poses.append(self.pose)
  65.         elems += ax.plot([e[0] for e in self.poses], [e[1] for e in self.poses], linewidth=0.5, color="black")
  67.     @classmethod
  68.     def state_transition(cls, nu, omega, time, pose):
  69.         t0 = pose[2]
  70.         if math.fabs(omega) < 1e-10:
  71.             return pose + np.array( [nu*math.cos(t0),
  72.                                      nu*math.sin(t0),
  73.                                      omega ] ) * time
  74.         else:
  75.             return pose + np.array( [nu/omega*(math.sin(t0 + omega*time) - math.sin(t0)),
  76.                                      nu/omega*(-math.cos(t0 + omega*time) + math.cos(t0)),
  77.                                      omega*time ] )
  79.     def one_step(self, time_interval):
  80.         # nu, omega = self.run()
  81.         nu, omega = self.dwa()
  82.         self.pose = self.state_transition( nu, omega, time_interval, self.pose )
  84.     def run( self ):
  85.         return self.nu, self.omega
  87.     def go_stragiht( self ):
  88.         return self.nu, self.omega
  90.     def dwa( self ):
  91.         # ------------------------------------------------------------------------------
  92.         # robot pose in world flame
  94.         x, y, yaw = self.pose
  95.         ## 0(deg) <= yaw < 360(deg)
  96.         if (2*PI) <= yaw:
  97.             yaw = yaw - 2*PI
  98.         if yaw < 0:
  99.             yaw = yaw + 2*PI
  100.         self.pose[2] = yaw
  102.         # ------------------------------------------------------------------------------
  103.         # goal in world flame
  105.         goal = self.goal
  106.         g_x = goal.pos[0]
  107.         g_y = goal.pos[1]
  108.         print(f'x: {g_x},y: {g_y}')
  109.         ## gradient = slope
  110.         angle = math.atan2( g_y - y, g_x - x )
  111.         ## 0(deg) <= angle < 360(deg)
  112.         ## atan2 return value: -PI < angle <= PI
  113.         # if (2*PI) <= angle:
  114.         #     angle = angle - 2*PI
  115.         if angle < 0:
  116.             angle = angle + 2*PI
  117.         self.angle = angle
  118.         print(f'robot yaw: {yaw}')
  119.         print(f'angle (rad): {angle}')
  120.         print(f'angle (deg): {math.degrees(angle)}')
  121.         print(f'robot yaw - angle difference: {math.degrees(yaw - angle)}')
  123.         # ------------------------------------------------------------------------------
  124.         # goal in robot flame
  126.         tmp_x = g_x - x
  127.         tmp_y = g_y - y
  128.         g_x_in_robot_flame = tmp_x * cos(-yaw) - tmp_y * sin(-yaw)
  129.         g_y_in_robot_flame = tmp_x * sin(-yaw) + tmp_y * cos(-yaw)
  130.         ## for debug
  131.         self.g_x_in_robot_flame = g_x_in_robot_flame
  132.         self.g_y_in_robot_flame = g_y_in_robot_flame
  133.         self.tmp_x = tmp_x
  134.         self.tmp_y = tmp_y
  136.         # ------------------------------------------------------------------------------
  137.         # obstacles in world flame
  139.         obstacles = self.obstacles
  141.         # ------------------------------------------------------------------------------
  142.         # determine omega towards the goal
  144.         diff_angle = angle - yaw
  145.         self.diff_angle = diff_angle
  147.         ## the goal is on the left
  148.         if 0 < g_y_in_robot_flame:
  149.             if abs(diff_angle) < DEGREE_1:
  150.                 ## move forward
  151.                 change_omega = 0
  152.             else:
  153.                 change_omega = abs(diff_angle) * COEFF
  155.         ## the goal is on the right
  156.         if g_y_in_robot_flame < 0:
  157.             if DEGREE_179 < abs(diff_angle) and abs(diff_angle) < DEGREE_181:
  158.                 ## force clockwise
  159.                 change_omega = ANGULAR_VELOCITY_MAX
  160.             else:
  161.                 change_omega = - abs(diff_angle) * COEFF
  163.         # ------------------------------------------------------------------------------
  164.         # move around obstacle
  165.         # closest ranking
  167.         dist_ob = []
  168.         ob_in_robot_flame = []
  169.         for obstacle in obstacles:
  170.             # obstacles in robot flame
  171.             tmp_x = obstacle.pos[0] - x
  172.             tmp_y = obstacle.pos[1] - y
  173.             dist = sqrt( tmp_x * tmp_x + tmp_y * tmp_y )
  174.             ob_x_in_robot_flame = tmp_x * cos(-yaw) - tmp_y * sin(-yaw)
  175.             ob_y_in_robot_flame = tmp_x * sin(-yaw) + tmp_y * cos(-yaw)
  176.             dist_ob.append( (dist, ob_x_in_robot_flame, ob_y_in_robot_flame) )
  178.         # print( dist_ob )
  179.         dist_ob.sort(key=lambda x: x[0])
  180.         # print( dist_ob )
  181.         print(f'robot yaw: {yaw}')
  182.         print( f'dist: {dist_ob[0][0]}' )
  183.         print( f'ob_y_in_robot_flame: {dist_ob[0][2]}' )
  184.         # distance between robot and obstacle
  185.         if dist_ob[0][0] < 1.0:
  186.             if 0 < dist_ob[0][2]:
  187.                 change_omega = - (1 / dist + 0.1) * COEFF_OB
  188.             if dist_ob[0][2] < 0:
  189.                 change_omega = + (1 / dist + 0.1) * COEFF_OB
  191.         self.omega = change_omega
  192.         return self.nu, self.omega
  194. # ------------------------------------------------------------------------------
  196. class World:
  197.     def __init__( self, time_span, time_interval ):
  198.         self.objects = []
  199.         self.time_span = time_span
  200.         self.time_interval = time_interval
  201.         self.file_name_counter = 0
  203.     def append( self,obj ):
  204.         self.objects.append(obj)
  206.     def draw(self):
  207.         fig = plt.figure(figsize=(8,8)) # fig = plt.figure(figsize=(4,4))
  208.         ## make axes
  209.         ax = fig.add_subplot(111)
  210.         ax.set_aspect('equal')
  211.         ax.set_xlim(-10,10)
  212.         ax.set_ylim(-10,10)
  213.         ax.set_xlabel("X",fontsize=10)
  214.         ax.set_ylabel("Y",fontsize=10)
  216.         elems = []
  218.         tmp_1 = int( self.time_span/self.time_interval ) + 1
  219.         tmp_2 = int( self.time_interval )
  220.         self.ani = anm.FuncAnimation(fig, self.one_step, fargs=(elems, ax), frames=tmp_1, interval=tmp_2, repeat=False)
  221.         plt.show()
  223.     def one_step(self, i, elems, ax):
  224.         while elems: elems.pop().remove()
  225.         time_str = "t = %.2f[s]" % (self.time_interval*i)
  226.         elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
  227.         ##
  228.         elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
  229.         for obj in self.objects:
  230.             obj.draw(ax, elems)
  231.             if hasattr(obj, "one_step"): obj.one_step(self.time_interval)
  233. # ------------------------------------------------------------------------------
  235. class Map:
  236.     def __init__(self):
  237.         self.landmarks = []
  238.         self.goal = []
  239.         self.obstacles = []
  241.     def append_landmark(self, landmark):
  242.         landmark.id = len(self.landmarks)
  243.         self.landmarks.append(landmark)
  245.     def append_goal( self, goal ):
  246.         self.goal.append( goal )
  248.     def append_obstacle( self, obstacle ):
  249.         self.obstacles.append( obstacle )
  251.     def draw( self, ax, elems ):
  252.         for goal in self.goal: goal.draw(ax, elems)
  253.         for obstacle in self.obstacles: obstacle.draw(ax, elems)
  255. # ------------------------------------------------------------------------------
  257. class Goal:
  258.     def __init__( self, x, y ):
  259.         self.pos = np.array([x, y]).T
  260.         self.id = None
  262.     def draw( self, ax, elems ):
  263.         c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="*", label="goal", color="red")
  264.         elems.append(c)
  265.         elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
  267. # ------------------------------------------------------------------------------
  269. class Obstacle:
  270.     def __init__( self, x, y, r ):
  271.         self.pos = np.array([x, y]).T
  272.         self.id = None
  273.         self.radius = r
  275.     def draw( self, ax, elems ):
  276.         c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="o", label="obstacle", color="black")
  277.         elems.append(c)
  278.         elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
  279.         c = patches.Circle(xy=(self.pos[0], self.pos[1]), radius=self.radius, fill=False, color="blue")
  280.         elems.append(ax.add_patch(c))
  282. # ------------------------------------------------------------------------------
  283. if __name__ == '__main__':
  284.     ## 0.01 : very slow
  285.     # world = World( 15, 0.1 )
  286.     # world = World( 120, 0.1 )
  287.     world = World( 200, 0.2 )
  288.     # world = World( 120, 0.04 )
  289.     # world = World( 120, 0.01 )
  291.     m = Map()
  292.     goal = Goal( 4, 8 )
  293.     m.append_goal( goal )
  294.     obstacle = Obstacle( 5, 6, 1 )
  295.     m.append_obstacle( obstacle )
  296.     obstacle = Obstacle( 3, 5, 1 )
  297.     m.append_obstacle( obstacle )
  298.     world.append( m )
  300.     robot = Robot( np.array([0, -3, 0]).T, nu=0.2, omega=0.0, goal=goal, obstacles=m.obstacles )
  301.     # robot = Robot( np.array([0, 0, 1]).T, nu=0.2, omega=0.0, goal=goal )
  302.     # robot = Robot( np.array([0, 0, 0]).T, nu=1.0, omega=1.0, goal=goal )
  304.     world.append( robot )
  305.     world.draw()

まとめ

このくらいの複雑さからcontinuous integrationを組み込まないと検証できなくなる気がします。

入力と期待値の幅を用意することになる気がします。

それなりに厳しいobstacleの配置にしたけど、実機に搭載は怖くて出来ない。

広告

IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト



«       »