comments added to debug

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