| 173 | |
| 174 | |
| 175 | class MotionDAR(Node): |
| 176 | dt = 0.02 |
| 177 | timer = None |
| 178 | _init_done = False |
| 179 | _start_infer = False |
| 180 | _counter = 0 |
| 181 | _gen_counter = -1 |
| 182 | _toggle_time = -1.0 |
| 183 | _block_index = 0 |
| 184 | _current_block_size = 0 |
| 185 | |
| 186 | _text_prompt: str = "stand" |
| 187 | _text_embedding: torch.Tensor = None |
| 188 | _ref_motion_dict: Dict[str, Any] |
| 189 | history_motion: torch.Tensor = None |
| 190 | history_abs_pose: AbsolutePose = None |
| 191 | |
| 192 | # Thread control |
| 193 | _keyboard_thread: threading.Thread = None |
| 194 | _shutdown_event: threading.Event = None |
| 195 | |
| 196 | def __init__( |
| 197 | self, |
| 198 | cfg: DictConfig, |
| 199 | ): |
| 200 | super().__init__("motion_dar") |
| 201 | # self.get_logger().set_level(logging.DEBUG) |
| 202 | |
| 203 | # Store configuration |
| 204 | self.config = cfg |
| 205 | |
| 206 | # Extract configuration parameters |
| 207 | self.ckpt = cfg.dar.ckpt |
| 208 | self.use_full_sample = cfg.dar.use_full_sample |
| 209 | self.guidance_scale = cfg.dar.guidance_scale |
| 210 | self._device = cfg.dar.device |
| 211 | |
| 212 | # Motion block parameters |
| 213 | self.block_size = cfg.motion_block.block_size |
| 214 | self.total_steps = cfg.motion_block.total_steps |
| 215 | self.num_joints = cfg.motion_block.num_joints |
| 216 | |
| 217 | self.dt = cfg.control_dt |
| 218 | |
| 219 | self.load_dar() |
| 220 | |
| 221 | # Create publisher for MotionBlock |
| 222 | self.motion_publisher = self.create_publisher(MotionBlock, |
| 223 | cfg.topics.motion_output, |
| 224 | 10) |
| 225 | |
| 226 | self.reset_subscriber = self.create_subscription( |
| 227 | Time, |
| 228 | cfg.topics.toggle, |
| 229 | self._toggle_callback, |
| 230 | 10, |
| 231 | ) |
| 232 | self.timer = self.create_timer(self.dt, self.loop) |