path update

social_mpc/path.py

@@ -68,33 +68,40 @@ class PathPlanner():
         x, y = self.ssn.x_coordinates, self.ssn.y_coordinates
         dx = 1/self.state_config.global_map_scale
         target_dist = np.linalg.norm(np.array(pos[:2]) - np.array(self.target[:2]))
-        xl, yl, vxl, vyl, dl = utils.optimal_path_2d(
+        ret = utils.optimal_path_2d(
             self.distance, pos[:2],
-            dx=self.max_d,
+            dx=dx,
             coords=(x, y),
-            max_distance=self.max_d,
-            goal=np.array(self.target[:2]))
-            # max_distance=self.max_d_orientation)        
+            goal=np.array(self.target[:2]),
+            max_d_orientation=self.max_d_orientation)
+        
+        if ret:
+            xl, yl, vxl, vyl, dl = ret
+        else :
+            return []
 
         if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
             return []
-        
+
         self.waypoints_path[0, :] =  [pos[0], pos[1]]
         for i, (x, y) in enumerate(zip(xl, yl)):
             self.waypoints_path[i+1, :] =  [x, y]
-
-        self.waypoints_path[i+2, :] =  [self.target[0], self.target[1]]
-        d_angle = utils.constraint_angle(np.arctan2(yl[0] - pos[1], xl[0] - pos[0]) - pos[2])
-        if target_dist < self.max_d_orientation:  # if within range, set waypoint = target
-            wpt_pos = [self.target[0], self.target[1], self.target[2]]
-        elif target_dist < self.max_d:
-            wpt_pos = [self.target[0],
-                       self.target[1],
-                       np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
-        elif np.abs(d_angle) > self.max_target_angle:
-            wpt_pos = [pos[0], pos[1], np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
-        else:  # else, point on shortest path with orientation towards target
-            wpt_pos = [xl[0], yl[0], np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
+        self.waypoints_path[i+2, :] =  [self.target[0], self.target[1]]        
+        waypoint_index = min(int(self.max_d / dx), len(dl)-1)  # get the index of the next waypoint at max_d with dx step
+        if target_dist < self.max_d_orientation:  # if we are near the target, we only turn on ourself
+            wpt_pos = [pos[0], pos[1], self.target[2]]
+        else :
+            angle=np.abs(utils.constraint_angle(np.arctan2(yl[waypoint_index] - pos[1], xl[waypoint_index] - pos[0]) - pos[2])) 
+            #  if the angle to the next point is too big, also first turn on ourself to its direction
+            if angle > self.max_target_angle:
+                    wpt_pos = [pos[0], pos[1], np.arctan2(yl[waypoint_index] - pos[1], xl[waypoint_index] - pos[0])]
+            else : 
+                if target_dist < self.max_d: # we are near the target
+                    wpt_pos = [self.target[0],
+                            self.target[1],
+                            np.arctan2(yl[-1] - pos[1], xl[-1] - pos[0])]
+                else:  # nominal case else, point on shortest path with orientation towards target
+                    wpt_pos = [xl[waypoint_index], yl[waypoint_index], np.arctan2(yl[waypoint_index] - pos[1], xl[waypoint_index] - pos[0])]
         return wpt_pos
 
     def run(self,