From d29b8be5e1579199670d91c83ffaf658d0dd1101 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Thu, 30 Mar 2023 13:54:32 +0200
Subject: [PATCH] groups_visible feature added
---
social_mpc/controller.py | 9 +++++----
1 file changed, 5 insertions(+), 4 deletions(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 1f6e8dc..223d0d3 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -293,7 +293,7 @@ class RobotController():
self.shared_humans[:n, 3:5] = state.humans_velocity[idx, :]
self.shared_groups[:, -1] = -1
- idx = np.where(state.groups_pose[:, -1] >= 0)[0]
+ idx = np.where(state.groups_visible == 0)[0]
n = len(idx)
self.shared_groups[:n, :2] = state.groups_pose[idx, :]
self.shared_groups[:n, -1] = state.groups_id[idx]
@@ -661,9 +661,10 @@ class RobotController():
groups_id = state.groups_id.tolist()
if self.pan_target_id[1] in groups_id:
idx = groups_id.index(self.pan_target_id[1])
- global_pos = state.groups_pose[idx]
- pos = utils.global_to_local(state.robot_pose, global_pos)
- self.pan_goal[:] = [*pos, 0, 0, 1]
+ if state.groups_visible[idx] == 0.:
+ global_pos = state.groups_pose[idx]
+ pos = utils.global_to_local(state.robot_pose, global_pos)
+ self.pan_goal[:] = [*pos, 0, 0, 1]
if self.pan_target_pose is not None:
self.pan_goal[:] = [*self.pan_target_pose, 0, 0, 1]
--
GitLab