From 3e5f60c06d1566e8bbb2b834a654ebfbceccec37 Mon Sep 17 00:00:00 2001
From: AUTERNAUD Alex <alex.auternaud@inria.fr>
Date: Thu, 29 Feb 2024 09:22:55 +0100
Subject: [PATCH] utils update for global_path_planner
---
.../src/robot_behavior/utils.py | 77 +++++++++++++++++++
1 file changed, 77 insertions(+)
diff --git a/src/robot_behavior/src/robot_behavior/utils.py b/src/robot_behavior/src/robot_behavior/utils.py
index 4c8e5e2..866bff4 100755
--- a/src/robot_behavior/src/robot_behavior/utils.py
+++ b/src/robot_behavior/src/robot_behavior/utils.py
@@ -1,6 +1,8 @@
#!/usr/bin/env python3
import numpy as np
import tf
+from scipy.interpolate import RectBivariateSpline
+
def constraint_angle(angle, min_value=-np.pi, max_value=np.pi):
@@ -49,3 +51,78 @@ def meter_to_px(point, map_size, resolution):
point_px[1] = abs(map_size[1][1] - point[1])/resolution
return np.rint(point_px)
+
+def optimal_path_2d(travel_time, starting_point, dx, coords, goal=None, max_d_orientation=None, N=100):
+ """
+ Find the optimal path from starting_point to the zero contour
+ of travel_time. dx is the grid spacing
+ Solve the equation x_t = - grad t / | grad t |
+ """
+
+ grad_t_y, grad_t_x = np.gradient(travel_time, dx)
+ if isinstance(travel_time, np.ma.MaskedArray):
+ grad_t_y[grad_t_y.mask] = 0.0
+ grad_t_y = grad_t_y.data
+ grad_t_x[grad_t_x.mask] = 0.0
+ grad_t_x = grad_t_x.data
+
+ d_interp = RectBivariateSpline(coords[1], coords[0],
+ travel_time)
+ gradx_interp = RectBivariateSpline(coords[1], coords[0],
+ grad_t_x)
+ grady_interp = RectBivariateSpline(coords[1], coords[0],
+ grad_t_y)
+
+ def get_velocity(position):
+ """ return normalized velocity at pos """
+ x, y = position
+ vel = np.array([gradx_interp(y, x)[0][0],
+ grady_interp(y, x)[0][0]])
+ return vel / np.linalg.norm(vel)
+
+ def get_distance(position):
+ x, y = position
+ return d_interp(y, x)
+
+ def euler_point_update(pos, ds):
+ return pos - get_velocity(pos) * ds
+
+ def runge_kutta(pos, ds):
+ """ Fourth order Runge Kutta point update """
+ k1 = ds * get_velocity(pos)
+ k2 = ds * get_velocity(pos - k1/2.0)
+ k3 = ds * get_velocity(pos - k2/2.0)
+ k4 = ds * get_velocity(pos - k3)
+ v = (k1 + 2*k2 + 2*k3 + k4)/6.0
+ return pos - v, v
+
+ # def sym(x, v):
+ # x = x[0], 0.5 * (coords[1][-1] + coords[1][0]) - x[1]
+ # v = v[0], -v[1]
+ # return x, v
+ #
+ # starting_point, _ = sym(starting_point, [0, 0])
+
+ x, v = runge_kutta(starting_point, dx)
+ xl, yl, vxl, vyl, dl = [], [], [], [], []
+ starting_distance = get_distance(starting_point)
+
+ for i in range(N):
+ # xp, vp = sym(x, v)
+ d = get_distance(x)[0][0]
+ # if negative value d < 0, waypoint in an obstacle
+ if d < 0:
+ return False
+ # goal is implicit defined in the travel_time map but we need to test it explicitly because we can not really trust d
+ if ((goal is not None and max_d_orientation is not None) and
+ (np.linalg.norm(goal - np.asarray(x)) < max_d_orientation)):
+ if dl:
+ break
+ xl.append(x[0])
+ yl.append(x[1])
+ vxl.append(v[0])
+ vyl.append(v[1])
+ dl.append(d)
+ x, v = runge_kutta(x, dx)
+
+ return xl, yl, vxl, vyl, dl
--
GitLab