From 80006876e31024f391948713dcb355ba016ee90c Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Wed, 29 Mar 2023 16:09:57 +0200
Subject: [PATCH] comments added to debug
---
social_mpc/goal.py | 18 +++++++++++++++---
1 file changed, 15 insertions(+), 3 deletions(-)
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 408931b..5402592 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -79,7 +79,8 @@ class GoalFinder():
n_group = np.sum(sh_groups[:, -1] >= 0)
sh_groups = sh_groups[n_group, :]
group_ids = np.array(sh_groups[sh_groups[:, -1] >= 0, -1], dtype=int).tolist()
- logger.debug("human_ids : {}, {}, {}".format(human_ids, humans, n_human))
+ print("human_ids : {}, {}, {}".format(human_ids, humans, n_human))
+ print("group_ids : {}, {}, {}".format(group_ids, sh_groups, n_group))
target = None
if sh_goto_target_human_id[0]:
@@ -119,6 +120,7 @@ class GoalFinder():
if target[0] == 'human_in_group':
center = [sh_groups[target[2], 0], sh_groups[target[2], 1]]
persons = humans[humans[:, -2] == target[2], :]
+ print('human_in_group : center {}, persons{}'.format(center, persons))
goal = ssn.calc_goal_pos(
f_dsz=f_dsz,
map=global_map,
@@ -126,30 +128,40 @@ class GoalFinder():
group_center=center,
robot=robot_pose)
goal = list(goal)
- goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0])
+ print('goal human_in_group before {}'.format(goal))
+ goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
+ print('goal human_in_group after {}'.format(goal))
elif target[0] == 'isolated_human':
person = humans[target[1], :]
ts_x = person[0] + np.cos(person[2]) * stride
ts_y = person[1] + np.sin(person[2]) * stride
center = [ts_x, ts_y]
+ print('isolated_human : center {}, persons{}'.format(center, persons))
goal = ssn.calc_goal_pos(
f_dsz=f_dsz,
map=global_map,
persons=person,
group_center=center,
robot=robot_pose)
+ goal = list(goal)
+ print('goal isolated_human before {}'.format(goal))
+ goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
+ print('goal isolated_human after {}'.format(goal))
elif target[0] == 'group':
center = [sh_groups[target[1], 0], sh_groups[target[1], 1]]
persons = humans[humans[:, -2] == target[1], :]
+ print('group : center {}, persons{}'.format(center, persons))
goal = ssn.calc_goal_pos(
f_dsz=f_dsz,
map=global_map,
persons=persons,
group_center=center,
robot=robot_pose)
+ goal = list(goal)
+ print('goal group {}'.format(goal))
else:
continue
- logger.debug("goal : {}".format(goal))
+ print("goal : {}".format(goal))
with lock:
shared_target[1] = True
--
GitLab