intermediate_waypoint published and refined

social_mpc/controller.py

@@ -27,6 +27,9 @@ from social_mpc.ssn_model.social_spaces import SocialSpaces, Group
 from social_mpc.utils import constraint_angle, local_to_global, global_to_local
 from social_mpc.config.config import ControllerConfig
 
+from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped
+
 
 class RobotController():
     def __init__(self, config_filename=None):
@@ -86,6 +89,8 @@ class RobotController():
         else:
             self.smm_enabled = False
 
+        self._waypoints_pub = rospy.Publisher('/waypoints_path', Path, queue_size=10)
+
 
     def init_var(self):
         config = self.controller_config
@@ -197,6 +202,13 @@ class RobotController():
                 self.shared_groups = np.ndarray(shape=groups.shape,
                                             dtype=groups.dtype,
                                             buffer=self.shm_groups.buf)
+                
+            # Path computed by the Global Planner
+            path = np.zeros((102, 2))  # x, y
+            self.shm_path = self.smm.SharedMemory(size=path.nbytes)
+            self.shared_path = np.ndarray(shape=path.shape,
+                                        dtype=path.dtype,
+                                        buffer=self.shm_path.buf)
         else:
             list_type = list
 
@@ -266,7 +278,8 @@ class RobotController():
                                                   self.shared_target,
                                                   self.shared_robot_pose,
                                                   self.flags,
-                                                  self.lock))
+                                                  self.lock,
+                                                  self.shared_path))
         self.path_planner_process.start()
         # atexit.register(lambda x: self.on_subprocess_exit("Path planner"), self.path_planner_process)
 
@@ -563,6 +576,16 @@ class RobotController():
                                      0, 0, 1]
                 self.goto_goal[2] = constraint_angle(self.goto_goal[2])
                 self.goto_goal[-2] = 1
+                msg = Path()
+                msg.header.frame_id = 'map'
+                msg.header.stamp = rospy.Time.now()
+                for (x, y) in self.shared_path:
+                    if x != 0. and y != 0.:
+                        pose = PoseStamped()
+                        pose.pose.position.x = x
+                        pose.pose.position.y = y
+                        msg.poses.append(pose)
+                self._waypoints_pub.publish(msg)
 
         if self.mode == 2 or self.mode ==1:
             self.human_features = np.zeros(

social_mpc/path.py

@@ -17,7 +17,7 @@ from social_mpc.ssn_model.social_spaces import SocialSpaces
 
 
 class PathPlanner():
-    def __init__(self, state_config, robot_config, path_loop_time=None, max_d=1.0, max_d_orientation=1.0, max_target_angle=np.pi/4):
+    def __init__(self, state_config, robot_config, path_loop_time=None, max_d=1.0, max_d_orientation=1.0, max_target_angle=np.pi/5):
         self.state_config = state_config
         self.robot_config = robot_config
         self.max_d = max_d
@@ -30,6 +30,8 @@ class PathPlanner():
             bbox=world_size,
             map_shape=(self.state_config.global_map_height,
                        self.state_config.global_map_width))
+        self.waypoints_path = np.zeros((102, 2))
+        self.offset_max_distance = 0.3 # in meter
 
     def position_to_px_coord(self, pos):
         """ Global map position (x, y) in meter to position 
@@ -65,15 +67,22 @@ class PathPlanner():
     def get_next_waypoint(self, pos):
         x, y = self.ssn.x_coordinates, self.ssn.y_coordinates
         dx = 1/self.state_config.global_map_scale
+        max_distance = np.linalg.norm(np.array(pos[:2]) - np.array(self.target[:2]))
         xl, yl, vxl, vyl, dl = utils.optimal_path_2d(
             self.distance, pos[:2],
-            dx=dx,
+            dx=self.max_d,
             coords=(x, y),
-            #max_distance=self.max_d)
-            max_distance=self.max_d_orientation)        
+            max_distance=max_distance-self.offset_max_distance)
+            # max_distance=self.max_d_orientation)        
 
         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 dl[0] < self.max_d_orientation:  # if within range, set waypoint = target
             wpt_pos = [self.target[0], self.target[1], self.target[2]]
@@ -82,9 +91,9 @@ class PathPlanner():
                        self.target[1],
                        np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
         elif np.abs(d_angle) > self.max_target_angle:
-            wpt_pos = [xl[0], yl[0], np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
+            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[-1], yl[-1], np.arctan2(yl[-1] - pos[1], xl[-1] - pos[0])]
+            wpt_pos = [xl[0], yl[0], np.arctan2(yl[0] - pos[1], xl[0] - pos[0])]
         return wpt_pos
 
     def run(self,
@@ -93,7 +102,8 @@ class PathPlanner():
             shared_target,
             shared_robot_pose,
             flags,
-            lock):
+            lock,
+            shared_path):
         last_time = time.time()
 
         while flags[0]:
@@ -107,6 +117,7 @@ class PathPlanner():
                 sh_target = np.array([*shared_target])
                 sh_robot_pose = np.array([*shared_robot_pose])
                 sh_global_map = np.array([*shared_global_map])
+                shared_path[:] = np.zeros_like(self.waypoints_path)
             if not sh_target[0]:  # not ready
                 self.distance = None
                 with lock:
@@ -146,3 +157,4 @@ class PathPlanner():
                     shared_waypoint[1] = float(wpt[0])
                     shared_waypoint[2] = float(wpt[1])
                     shared_waypoint[3] = float(wpt[2])
+                    shared_path[:] = self.waypoints_path[:]

social_mpc/utils.py

@@ -166,7 +166,8 @@ def optimal_path_2d(travel_time, starting_point, dx, coords,
         d = get_distance(x)
         if ((max_distance is not None) and
                 (starting_distance - d > max_distance)):
-            break
+            if dl:
+                break
         xl.append(x[0])
         yl.append(x[1])
         vxl.append(v[0])