| 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) |
| 233 | |
| 234 | self.get_logger().info("MotionDAR Created with Hydra configuration") |
| 235 | self.get_logger().info( |
| 236 | f" - block_size: {self.block_size}, total_steps: {self.total_steps}, num_joints: {self.num_joints}" |
| 237 | ) |
| 238 | |
| 239 | # Initialize and start keyboard input thread |
| 240 | self._shutdown_event = threading.Event() |
| 241 | self._keyboard_thread = threading.Thread( |
| 242 | target=self._keyboard_input_loop, daemon=True) |
| 243 | self._keyboard_thread.start() |
| 244 | self.get_logger().info( |
| 245 | "Keyboard input thread started. Type text prompts and press Enter." |
| 246 | ) |
| 247 | |
| 248 | self.get_logger().info( |
| 249 | "Waiting for /dar/toggle to start generating...") |
| 250 | |
| 251 | def _create_msg_array(self, data, dimensions_info): |
| 252 | """Helper function to create Float32MultiArray with proper dimension info""" |