reset_shared_values

social_mpc/controller.py

@@ -212,6 +212,26 @@ class RobotController():
         self.shared_robot_pose = list_type([0.0, 0.0, 0.0])
         self.shared_goto_target_human_id = list_type([False, 0])
 
+    def reset_shared_values(self):
+        with self.lock:
+            self.shared_waypoint[0] = False
+            self.shared_waypoint[1] = 0.0
+            self.shared_waypoint[2] = 0.0
+            self.shared_waypoint[3] = 0.0
+            self.shared_target[0] = False
+            self.shared_target[1] = False
+            self.shared_target[2] = 0.0
+            self.shared_target[3] = 0.0
+            self.shared_target[4] = 0.0
+            self.shared_goto_target_human_id[0] = False
+            self.shared_goto_target_human_id[1] = 0
+        config = self.controller_config
+        self.goto_goal = np.zeros(config.goto_target_dim)
+        self.pan_goal = np.zeros(config.pan_target_dim)
+        self.human_features = np.zeros(config.human_target_dim)
+        self.last_human_features = np.zeros(config.human_target_dim)
+
+
     def on_subprocess_exit(self, name):
         if (self.flags[0] != 0):
             logger.error("Subprocess {0} crashed. Closing".format())

social_mpc/path.py

@@ -110,6 +110,7 @@ class PathPlanner():
                 sh_robot_pose = np.array([*shared_robot_pose])
                 sh_global_map = np.array([*shared_global_map])
             if not sh_target[0]:  # not ready
+                self.distance = None
                 with lock:
                     shared_waypoint[0] = False
                 continue
@@ -149,4 +150,4 @@ class PathPlanner():
                     shared_waypoint[0] = True
                     shared_waypoint[1] = float(wpt[0])
                     shared_waypoint[2] = float(wpt[1])
-                    shared_waypoint[3] = float(wpt[2])
\ No newline at end of file
+                    shared_waypoint[3] = float(wpt[2])