goal.py issue

social_mpc/goal.py

@@ -69,7 +69,7 @@ class GoalFinder():
                 continue
             humans = np.zeros((n_human, 5))
             for j in range(n_human):
-                humans[j, :] = sh_humans[j, :-1]
+                humans[j, :] = sh_humans[j, :-2]
                 humans[j, 2] = utils.robot_frame_to_ssn_frame_ang(humans[j, 2])
             human_ids = np.array(
                 sh_humans[sh_humans[:, -1] >= 0, -1],
@@ -98,6 +98,7 @@ class GoalFinder():
             # TODO group_id, group center, group members to integrate
             if target_id:
                 goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=group.center, robot=robot_pose)
+                goal = list(goal)
                 goal[2] = np.arctan2(goal[1] - humans[idx][1], goal[0] - humans[idx][0])
             else:
                 center = [0., 0.]  # TODO
@@ -109,4 +110,4 @@ class GoalFinder():
                 shared_target[1] = True
                 shared_target[2] = float(goal[0])
                 shared_target[3] = float(goal[1])
-                shared_target[4] = float(goal[2]) + np.pi/2
\ No newline at end of file
+                shared_target[4] = float(goal[2]) + np.pi/2