From 071cf691bf5b3816e874bd57c581e5288b3b6e7d Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Mon, 25 Jul 2022 17:39:59 +0200
Subject: [PATCH] reset_shared_values
---
social_mpc/controller.py | 20 ++++++++++++++++++++
social_mpc/path.py | 3 ++-
2 files changed, 22 insertions(+), 1 deletion(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index dbb0f86..7114f50 100644
--- a/social_mpc/controller.py
+++ b/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())
diff --git a/social_mpc/path.py b/social_mpc/path.py
index 731274f..95511e5 100644
--- a/social_mpc/path.py
+++ b/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])
--
GitLab