group_detector to MotorController beginning (should not break the current way...

group_detector to MotorController beginning (should not break the current way of controlling the robot)

social_mpc/controller.py

@@ -45,7 +45,7 @@ class RobotController():
         self.shared_global_map = None
         self.shared_humans = None
 
-        self.goto_target = False
+        self.goto_target_flag = False
         self.pan_target_id = None
         self.pan_target_flag = False
         self.pan_target_pose = None
@@ -196,7 +196,7 @@ class RobotController():
 
             # 6 = pos + angle + velocity + id
             if (state.humans_pose.shape[0] > 0):
-                humans = np.zeros((state.humans_pose.shape[0], 6))
+                humans = np.zeros((state.humans_pose.shape[0], 7))  # x, y, orientation, lin vel, ang vel, group id, id
                 self.shm_humans = self.smm.SharedMemory(size=humans.nbytes)
                 self.shared_humans = np.ndarray(shape=humans.shape,
                                             dtype=humans.dtype,
@@ -213,6 +213,7 @@ class RobotController():
                                                      0.0])  # theta
         self.shared_robot_pose = list_type([0.0, 0.0, 0.0])
         self.shared_goto_target_human_id = list_type([False, 0])
+        self.shared_goto_target_group_id = list_type([False, 0])
 
 
     def reset_shared_values(self):
@@ -228,6 +229,8 @@ class RobotController():
             self.shared_target[4] = 0.0
             self.shared_goto_target_human_id[0] = False
             self.shared_goto_target_human_id[1] = 0
+            self.shared_goto_target_group_id[0] = False
+            self.shared_goto_target_group_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)
@@ -280,6 +283,7 @@ class RobotController():
                 idx = np.where(state.humans_visible == 0)[0]
                 n = len(idx)
                 self.shared_humans[:n, -1] = state.humans_id[idx]
+                self.shared_humans[:n, -2] = state.humans_group_id[idx]
                 self.shared_humans[:n, :3] = state.humans_pose[idx, :]
                 self.shared_humans[:n, 3:5] = state.humans_velocity[idx, :]
                 # if self.shared_goto_target_human_id[0]:
@@ -294,6 +298,7 @@ class RobotController():
     def set_target_pose(self, pose):
         with self.lock:
             self.shared_goto_target_human_id[0] = False  # disable joining a person
+            self.shared_goto_target_group_id[0] = False  # disable joining a group
             self.shared_target[1] = True
             self.shared_target[2] = float(pose[0])
             self.shared_target[3] = float(pose[1])
@@ -304,14 +309,23 @@ class RobotController():
             self.shared_goto_target_human_id[0] = True
             self.shared_goto_target_human_id[1] = human_id
 
+    def set_target_group_id(self, group_id):
+        with self.lock:
+            self.shared_goto_target_group_id[0] = True
+            self.shared_goto_target_group_id[1] = group_id
+
     def set_goto_target(self, state, target):
-        self.goto_target = False
+        self.goto_target_flag = False
         self.is_failure = False
         if isinstance(target, int):
             if len(state.humans_id) > 0 and target in state.humans_id:
                 if target >= 0:
-                    self.goto_target = True
+                    self.goto_target_flag = True
                 self.set_target_human_id(target)
+            elif len(state.humans_group_id) > 0 and target in state.humans_group_id:
+                if target >= 0:
+                    self.goto_target_flag = True
+                self.set_target_group_id(target)
             else:
                 logger.warning('Target id does not exist for goto target')
                 self.is_failure = True
@@ -319,12 +333,14 @@ class RobotController():
                 with self.lock:
                     self.shared_goto_target_human_id[0] = False
                     self.shared_goto_target_human_id[1] = 0
+                    self.shared_goto_target_group_id[0] = False
+                    self.shared_goto_target_group_id[1] = 0
 
         elif len(target) == self.controller_config.target_pose_goto_dim and (
             isinstance(target, np.ndarray) or all(
                 isinstance(i, (int, float)) for i in target)):
             if target[-1]:
-                self.goto_target = True
+                self.goto_target_flag = True
                 if target[-2]:
                     new_target = [*utils.local_to_global(state.robot_pose, target[:2]), target[2]]
                     self.set_target_pose(new_target)
@@ -337,6 +353,8 @@ class RobotController():
             with self.lock:
                 self.shared_goto_target_human_id[0] = False
                 self.shared_goto_target_human_id[1] = 0
+                self.shared_goto_target_group_id[0] = False
+                self.shared_goto_target_group_id[1] = 0
 
     def set_human_target(self, state, target):
         self.human_target_id = None
@@ -364,6 +382,11 @@ class RobotController():
                 if target >= 0:
                     self.pan_target_id = target
                     self.pan_target_flag = True
+            elif len(state.humans_group_id) > 0 and target in state.humans_group_id:
+                if target >= 0:
+                    pass  # to integrate look at group
+                    # self.pan_target_id = target
+                    # self.pan_target_flag = True
             else:
                 logger.warning('Target id does not exist for pan target')
                 self.is_failure = True
@@ -421,6 +444,7 @@ class RobotController():
                                                  self.shared_target,
                                                  self.shared_robot_pose,
                                                  self.shared_goto_target_human_id,
+                                                 self.shared_goto_target_group_id,
                                                  self.shared_global_map,
                                                  self.flags,
                                                  self.lock))
@@ -620,7 +644,7 @@ class RobotController():
 
     def set_mode(self):
         self.mode = None
-        if self.goto_target:
+        if self.goto_target_flag:
             if self.human_target_id is None:
                 self.mode = 0  # join
             else:
@@ -679,7 +703,7 @@ class RobotController():
             self.update_goals(state)
 
         if self.do_step():
-            if not self.pan_target_flag and not self.goto_target and (self.human_target_id == -1 or self.human_target_id == None):
+            if not self.pan_target_flag and not self.goto_target_flag and (self.human_target_id == -1 or self.human_target_id == None):
                 self.reg_parameter[-1] = 100000
             self.actions = self.mpc.step(state=state.joint_angles,
                                          actions=self.actions + self.initial_action,

social_mpc/goal.py

@@ -38,6 +38,7 @@ class GoalFinder():
             shared_target,
             shared_robot_pose,
             shared_goto_target_human_id,
+            shared_goto_target_group_id,
             shared_global_map,
             flags,
             lock):
@@ -55,41 +56,53 @@ class GoalFinder():
             with lock:
                 robot_pose = (shared_robot_pose[0], shared_robot_pose[1])
                 sh_goto_target_human_id = np.array([*shared_goto_target_human_id])
+                sh_goto_target_group_id = np.array([*shared_goto_target_group_id])
                 if shared_humans is not None:
                     sh_humans = np.array([*shared_humans])
                 else:
                     continue
-            if not sh_goto_target_human_id[0]:
+            if not sh_goto_target_human_id[0] and not sh_goto_target_group_id[0]:
                 logger.debug("No target set")
                 continue
             n_human = np.sum(sh_humans[:, -1] >= 0)
+            if n_human < 1:
+                continue
             humans = np.zeros((n_human, 5))
             for j in range(n_human):
-                humans[j, :] = sh_humans[j, :5]
+                humans[j, :] = sh_humans[j, :-1]
                 humans[j, 2] = utils.robot_frame_to_ssn_frame_ang(humans[j, 2])
             human_ids = np.array(
                 sh_humans[sh_humans[:, -1] >= 0, -1],
                 dtype=int).tolist()
             logger.debug("human_ids : {}, {}, {}".format(human_ids, humans, n_human))
-            target_id = sh_goto_target_human_id[1]
-            if target_id not in human_ids:
-                logger.warning("Target human not in sight")
-                continue
+            target_id = None
+
+            if sh_goto_target_human_id[0]:
+                target_id = sh_goto_target_human_id[1]
+                if target_id not in human_ids:
+                    logger.warning("Target human not in sight")
+                    continue
+            if sh_goto_target_group_id[0]:
+                pass
 
-            if n_human >= 1:
+            groups = gd.gcff.identify_groups(humans, stride=self.controller_config.group_detection_stride)
+            if target_id:
                 idx = human_ids.index(target_id)
-                groups = gd.gcff.identify_groups(humans, stride=self.controller_config.group_detection_stride)
                 for group in groups:
                     if idx in group.person_ids:
                         group = group
                         break
                 humans_in_group = humans[group.person_ids, :]
-            else:
-                groups = None
-                humans_in_group = humans
 
             f_dsz = ssn.calc_dsz(humans, groups=groups)
-            goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=group.center, robot=robot_pose)
+            # 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[2] = np.arctan2(goal[1] - humans[idx][1], goal[0] - humans[idx][0])
+            else:
+                center = [0., 0.]  # TODO
+                humans_in_group = humans[:, :]  # TODO
+                goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=center, robot=robot_pose)
             logger.debug("goal : {}".format(goal))            
 
             with lock: