diff --git a/.gitignore b/.gitignore index 9eb8f3d2d..29fa0bb1b 100755 --- a/.gitignore +++ b/.gitignore @@ -149,4 +149,4 @@ common/vision/lasr_vision_open_vocabulary/lasr_vision_open_vocabulary/models/l2_ *.sif common/vision/lasr_vision_open_vocabulary/models/* common/vision/lasr_vision_open_vocabulary/yolo*.pt -yolo*.pt \ No newline at end of file +yolo*.pt diff --git a/FETCH_HEAD b/FETCH_HEAD new file mode 100644 index 000000000..e69de29bb diff --git a/LICENSE b/LICENSE old mode 100755 new mode 100644 diff --git a/README.md b/README.md old mode 100755 new mode 100644 diff --git a/common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py b/common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py new file mode 100644 index 000000000..9073875d6 --- /dev/null +++ b/common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +import os +import tempfile + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node + +from lasr_vlm_interfaces.srv import VlmDescribePeople +from lasr_vlm.vlm_inference import ( + ModelConfig, + VLMInference, + visually_describe_people, +) + + +class VlmDescribePeopleService(Node): + """ + ROS 2 service node that wraps VLM inference to visually describe people. + Receives a ROS image, runs it through the VLM, and returns the attributes. + """ + + def __init__(self): + super().__init__("vlm_describe_people_service") + + self.create_service( + VlmDescribePeople, + "/vlm/describe_people", + self.describe_people_callback, + ) + + model_config = ModelConfig(model_name="moondream") + self.vlm = VLMInference(model_config, new_model=False) + self.get_logger().info("VLM Describe People service started") + + def _image_msg_to_bgr8(self, image_msg): + """Convert a ROS Image message to an OpenCV BGR image without cv_bridge.""" + if image_msg.encoding not in ("bgr8", "rgb8", "mono8"): + raise ValueError(f"Unsupported image encoding: {image_msg.encoding}") + + image = np.frombuffer(image_msg.data, dtype=np.uint8) + + if image_msg.encoding == "mono8": + image = image.reshape((image_msg.height, image_msg.width)) + return cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) + + image = image.reshape((image_msg.height, image_msg.width, 3)) + if image_msg.encoding == "rgb8": + return cv2.cvtColor(image, cv2.COLOR_RGB2BGR) + + return image + + def describe_people_callback(self, request, response): + """ + Handle incoming service requests. + Converts the ROS image to a file, runs VLM inference, and returns attributes. + """ + self.get_logger().info("Received request to describe person") + + try: + cv_image = self._image_msg_to_bgr8(request.image_raw) + + with tempfile.NamedTemporaryFile(suffix=".jpg", delete=False) as f: + tmp_path = f.name + cv2.imwrite(tmp_path, cv_image) + + result = visually_describe_people( + input_image=tmp_path, + inference=self.vlm, + ) + + os.unlink(tmp_path) + + def _get(key, default): + + val = result.get(key, [default]) + + return val[0] if isinstance(val, list) and val else default + + response.hair_color = str(_get("hair_color", "unknown")) + + response.hair_length = str(_get("hair_length", "unknown")) + + response.glasses = bool(_get("glasses", False)) + + response.hat = bool(_get("hat", False)) + + response.shirt_color = str(_get("shirt color", "unknown")) + + self.get_logger().info(f"VLM result: {result}") + + except Exception as e: + self.get_logger().error(f"Failed to describe person: {e}") + + return response + + +def main(args=None): + rclpy.init(args=args) + node = VlmDescribePeopleService() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py b/common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py new file mode 100644 index 000000000..a9363f8bb --- /dev/null +++ b/common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python3 +""" +Two-layer test for the VLM describe-people pipeline: + 1. Unit-test parse_vlm_response directly (no ROS, no GPU) + 2. End-to-end ROS service client (requires service + Ollama running) + +Usage: + # Unit tests only: + python3 test_vlm_describe_people.py --unit + + # End-to-end service call: + python3 test_vlm_describe_people.py --e2e /path/to/person.jpg +""" + +import sys +import argparse + +import numpy as np + +# ─── Layer 1: unit-test the parser ──────────────────────────────────────────── + + +def test_parser(): + """Test parse_vlm_response against realistic model outputs without needing ROS.""" + # Import directly from the inference module (no ROS needed) + from lasr_vlm.vlm_inference import parse_vlm_response + + attributes = ["hair_color", "hair_length", "glasses", "hat", "shirt color"] + + cases = [ + # (description, raw_response, expected_dict) + ( + "clean key:value format", + "hair_color: brown, hair_length: short, glasses: True, hat: False, shirt color: red.", + { + "hair_color": "brown", + "hair_length": "short", + "glasses": True, + "hat": False, + "shirt color": "red", + }, + ), + ( + "verbose sentence answer", + "The person has long blonde hair. They are not wearing glasses or a hat. Their shirt color is blue.", + # parser will likely fail on hair_color/hair_length here — shows the fragility + {}, # we just print, don't assert + ), + ( + "uppercase values", + "hair_color: Black, hair_length: Long, glasses: YES, hat: NO, shirt color: Green.", + { + "hair_color": "black", + "hair_length": "long", + "glasses": True, + "hat": False, + "shirt color": "green", + }, + ), + ( + "missing attributes", + "hair_color: red, glasses: False.", + { + "hair_color": "red", + "glasses": False, + "hair_length": "unknown", + "hat": "unknown", + "shirt color": "unknown", + }, + ), + ] + + print("=== Parser Unit Tests ===\n") + for desc, raw, expected in cases: + result = parse_vlm_response(raw, attributes) + print(f"[{desc}]") + print(f" Input : {raw!r}") + print(f" Parsed : {result}") + if expected: + passed = all(result.get(k) == v for k, v in expected.items()) + print(f" Status : {'PASS' if passed else 'FAIL'}") + if not passed: + for k, v in expected.items(): + if result.get(k) != v: + print( + f" MISMATCH {k!r}: got {result.get(k)!r}, expected {v!r}" + ) + print() + + +# ─── Layer 2: end-to-end ROS service call ───────────────────────────────────── + + +def test_service(image_path: str): + import cv2 + import rclpy + from rclpy.node import Node + from sensor_msgs.msg import Image + from lasr_vlm_interfaces.srv import VlmDescribePeople + + class VlmTestClient(Node): + def __init__(self): + super().__init__("vlm_test_client") + self.client = self.create_client(VlmDescribePeople, "/vlm/describe_people") + + def _cv2_to_image_msg(self, cv_image): + image_msg = Image() + image_msg.height, image_msg.width = cv_image.shape[:2] + image_msg.encoding = "bgr8" + image_msg.is_bigendian = False + image_msg.step = cv_image.shape[1] * cv_image.shape[2] + image_msg.data = np.ascontiguousarray(cv_image).tobytes() + return image_msg + + def run(self, image_path: str): + self.get_logger().info("Waiting for /vlm/describe_people...") + if not self.client.wait_for_service(timeout_sec=15.0): + self.get_logger().error("Service not available. Is the server running?") + return + + cv_image = cv2.imread(image_path) + if cv_image is None: + self.get_logger().error(f"Could not read image: {image_path}") + return + + self.get_logger().info(f"Image loaded: {cv_image.shape} from {image_path}") + + request = VlmDescribePeople.Request() + request.image_raw = self._cv2_to_image_msg(cv_image) + + self.get_logger().info( + "Sending request (Ollama inference may take ~10-30s)..." + ) + future = self.client.call_async(request) + rclpy.spin_until_future_complete(self, future, timeout_sec=60.0) + + if future.result() is None: + self.get_logger().error("Call timed out or returned None.") + return + + r = future.result() + print("\n=== Service Response ===") + print(f" hair_color : {r.hair_color}") + print(f" hair_length : {r.hair_length}") + print(f" glasses : {r.glasses}") + print(f" hat : {r.hat}") + print(f" shirt_color : {r.shirt_color}") + print("========================\n") + + # Flag unparsed fields + unknowns = [ + f + for f, v in [ + ("hair_color", r.hair_color), + ("hair_length", r.hair_length), + ("shirt_color", r.shirt_color), + ] + if v == "unknown" + ] + if unknowns: + print( + f"⚠ These came back 'unknown' — check the raw VLM output in the service logs: {unknowns}" + ) + + rclpy.init() + node = VlmTestClient() + try: + node.run(image_path) + finally: + node.destroy_node() + rclpy.shutdown() + + +# ─── Entry point ────────────────────────────────────────────────────────────── + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--unit", action="store_true", help="Run parser unit tests") + parser.add_argument( + "--e2e", metavar="IMAGE", help="Run end-to-end service test with this image" + ) + args = parser.parse_args() + + if not args.unit and not args.e2e: + parser.print_help() + sys.exit(1) + + if args.unit: + test_parser() + + if args.e2e: + test_service(args.e2e) diff --git a/common/foundation_models/lasr_vlm/lasr_vlm/vlm_inference.py b/common/foundation_models/lasr_vlm/lasr_vlm/vlm_inference.py index 861f855bb..9969b384c 100644 --- a/common/foundation_models/lasr_vlm/lasr_vlm/vlm_inference.py +++ b/common/foundation_models/lasr_vlm/lasr_vlm/vlm_inference.py @@ -142,10 +142,16 @@ def visually_describe_people(input_image, inference: VLMInference) -> dict[str, ) for attr in attributes: user_query += f"\n- {attr}" + # user_query_example = ( + # "\n\n The structure of the response should be a comma separated list of attribute: value pairs. For example, " + # "'hair_color: _, hair_length: _, glasses: _, hat: _, shirt color: _', where the _ is replaced with the model's answer for that attribute. " + # "For true or false attributes, the value should be simply true or false. For example, 'glasses: true' if the model thinks the person is wearing glasses, and 'hat: false' if the model thinks the person is not wearing a hat." + # ) user_query_example = ( "\n\n The structure of the response should be a comma separated list of attribute: value pairs. For example, " "'hair_color: _, hair_length: _, glasses: _, hat: _, shirt color: _', where the _ is replaced with the model's answer for that attribute. " - "For true or false attributes, the value should be simply true or false. For example, 'glasses: true' if the model thinks the person is wearing glasses, and 'hat: false' if the model thinks the person is not wearing a hat." + "For glasses and hat, only answer true if they are clearly and visibly present in the image. " + "If you are not sure, answer false. For example, 'glasses: false' means the person is definitely not wearing glasses." ) user_query += user_query_example diff --git a/common/foundation_models/lasr_vlm/requiremenets.in b/common/foundation_models/lasr_vlm/requiremenets.in deleted file mode 100644 index bbbf54f42..000000000 --- a/common/foundation_models/lasr_vlm/requiremenets.in +++ /dev/null @@ -1 +0,0 @@ -ollama~=0.6.2 \ No newline at end of file diff --git a/common/foundation_models/lasr_vlm/requiremenets.txt b/common/foundation_models/lasr_vlm/requiremenets.txt deleted file mode 100644 index e026dda39..000000000 --- a/common/foundation_models/lasr_vlm/requiremenets.txt +++ /dev/null @@ -1,37 +0,0 @@ -# -# This file is autogenerated by pip-compile with Python 3.13 -# by the following command: -# -# pip-compile requiremenets.in -# -annotated-types==0.7.0 - # via pydantic -anyio==4.13.0 - # via httpx -certifi==2026.4.22 - # via - # httpcore - # httpx -h11==0.16.0 - # via httpcore -httpcore==1.0.9 - # via httpx -httpx==0.28.1 - # via ollama -idna==3.15 - # via - # anyio - # httpx -ollama==0.6.2 - # via -r requiremenets.in -pydantic==2.13.4 - # via ollama -pydantic-core==2.46.4 - # via pydantic -typing-extensions==4.15.0 - # via - # pydantic - # pydantic-core - # typing-inspection -typing-inspection==0.4.2 - # via pydantic diff --git a/common/foundation_models/lasr_vlm/requirements.txt b/common/foundation_models/lasr_vlm/requirements.txt index e026dda39..8184a5195 100644 --- a/common/foundation_models/lasr_vlm/requirements.txt +++ b/common/foundation_models/lasr_vlm/requirements.txt @@ -4,6 +4,7 @@ # # pip-compile requiremenets.in # +numpy<2.0 annotated-types==0.7.0 # via pydantic anyio==4.13.0 diff --git a/common/foundation_models/lasr_vlm/setup.py b/common/foundation_models/lasr_vlm/setup.py index a166096ff..bfc97c43b 100644 --- a/common/foundation_models/lasr_vlm/setup.py +++ b/common/foundation_models/lasr_vlm/setup.py @@ -25,7 +25,6 @@ def run(self): name=package_name, version="0.0.0", packages=find_packages(exclude=["test"]), - cmdclass={"install": InstallCommand}, data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml", "requirements.txt"]), @@ -42,6 +41,6 @@ def run(self): ], }, entry_points={ - "console_scripts": [], + "console_scripts": ["vlm_service = lasr_vlm.nodes.vlm_service:main"], }, ) diff --git a/common/foundation_models/lasr_vlm_interfaces/CMakeLists.txt b/common/foundation_models/lasr_vlm_interfaces/CMakeLists.txt new file mode 100644 index 000000000..0a21b9614 --- /dev/null +++ b/common/foundation_models/lasr_vlm_interfaces/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.8) +project(lasr_vlm_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +# Specify service files +set(srv_files + "srv/VlmDescribePeople.srv" +) + +# Generate interfaces +rosidl_generate_interfaces(${PROJECT_NAME} + ${srv_files} + DEPENDENCIES std_msgs sensor_msgs builtin_interfaces +) + +# Export dependencies +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/common/foundation_models/lasr_vlm_interfaces/package.xml b/common/foundation_models/lasr_vlm_interfaces/package.xml new file mode 100644 index 000000000..2e73c7c32 --- /dev/null +++ b/common/foundation_models/lasr_vlm_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + lasr_vlm_interfaces + 0.0.0 + Common interfaces for use within LASR VLM packages. + robocup + TODO: License declaration + + ament_cmake + + rosidl_default_generators + std_msgs + sensor_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/common/foundation_models/lasr_vlm_interfaces/srv/VlmDescribePeople.srv b/common/foundation_models/lasr_vlm_interfaces/srv/VlmDescribePeople.srv new file mode 100644 index 000000000..3f476c8f4 --- /dev/null +++ b/common/foundation_models/lasr_vlm_interfaces/srv/VlmDescribePeople.srv @@ -0,0 +1,7 @@ +sensor_msgs/Image image_raw +--- +string hair_color +string hair_length +bool glasses +bool hat +string shirt_color \ No newline at end of file diff --git a/common/language/lasr_llm/lasr_llm/llm_inference.py b/common/language/lasr_llm/lasr_llm/llm_inference.py index 84d51a55a..35c616f7b 100644 --- a/common/language/lasr_llm/lasr_llm/llm_inference.py +++ b/common/language/lasr_llm/lasr_llm/llm_inference.py @@ -3,20 +3,14 @@ from dataclasses import dataclass from typing import Optional, List, Dict import re -import rclpy import logging import numpy as np -print(f"Numpy version: {np.__version__}") # For debugging purposes +import os +import ollama +from ollama import Client +import socket -from transformers import ( - pipeline, - AutoModelForCausalLM, - AutoTokenizer, - BitsAndBytesConfig, - AutoModelForTokenClassification, - AutoModelForQuestionAnswering, -) import torch import json @@ -45,72 +39,124 @@ class ModelConfig: ) task: Optional[str] = None # For pipeline models quantize: bool = True + ollama_host: str = "http://127.0.0.1:11434" # Ollama host for LLMs models = { - "BERT-ner": "dslim/bert-base-NER", # NER so mainly names - "Qwen": "Qwen/Qwen2.5-1.5B", # perfect - "QCode": "Qwen/Qwen2.5-Coder-1.5B", # perfect - "Mistral": "mistralai/Mistral-7B-v0.1", # empty output - "Gemma": "google/gemma-2b", # nope - "DeepSeekQwen": "deepseek-ai/DeepSeek-R1-Distill-Qwen-1.5B", # terrible lol + # "BERT-ner": "dslim/bert-base-NER", # NER so mainly names + "Qwen": "qwen2.5:3b", + # "QCode": "Qwen/Qwen2.5-Coder-1.5B", # perfect + # "Mistral": "mistralai/Mistral-7B-v0.1", # empty output + # "Gemma": "google/gemma-2b", # nope + # "DeepSeekQwen": "deepseek-ai/DeepSeek-R1-Distill-Qwen-1.5B", # terrible lol } class LLMInference: def __init__(self, model_config: ModelConfig): self.config = model_config - if self.config.quantize: - # Quantize the model - for using 1/4 (or 1/8) of the GPU RAM. Full example in the models' HugginFace docs - self.quantization_config = BitsAndBytesConfig( - load_in_4bit=True, - bnb_4bit_quant_type="nf4", - bnb_4bit_compute_dtype=torch.bfloat16, - ) - self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - print(f"Using device: {self.device}") + self.logger = logging.getLogger(__name__) + self.model_name = models["Qwen"] - self.model_name = self.config.model_name - cache_dir='/home/fadi/.cache/huggingface/hub' - self.tokenizer = AutoTokenizer.from_pretrained(self.model_name, local_files_only=True) + self.num_gpu = self._detect_gpu_layers() + self.client = Client(host=self.config.ollama_host) + self._ensure_model_available() - self.logger = logging.getLogger(__name__) + def run_inference( + self, query: str, context: Optional[str] = None, max_tokens: int = 56 + ) -> str: + messages = [] + + if context: + messages.append({"role": "system", "content": context}) + + messages.append({"role": "user", "content": query}) - if self.config.model_type == "pipeline": - if self.config.task: - self.task = self.config.task + try: + response = self.client.chat( + model=self.model_name, + messages=messages, + options={ + "num_predict": max_tokens, + "num_gpu": self.num_gpu, + }, + ) + result = response.message.content.strip() + # Strip the input query from the result if echoed back, matching original behaviour + generated_text = re.sub(re.escape(query), "", result).strip() + return generated_text + + except Exception as e: + raise RuntimeError(f"[LLMInference] Inference failed: {e}") + + def _is_online(self) -> bool: + """Check internet connectivity.""" + try: + socket.setdefaulttimeout(3) + socket.create_connection(("8.8.8.8", 53)) + return True + except OSError: + return False + + def _model_is_local(self) -> bool: + """Return True if the model is already pulled locally.""" + local_models = [m.model for m in self.client.list().models] + return self.model_name.split(":")[0] in [m.split(":")[0] for m in local_models] + + def _detect_gpu_layers(self) -> int: + try: + if torch.cuda.is_available(): + free_vram_gb = torch.cuda.mem_get_info(0)[0] / 1e9 + self.logger.info(f"[LLMInference] Free VRAM: {free_vram_gb:.1f} GB.") + if free_vram_gb >= 4: + return -1 # full GPU offload + else: + return 8 # Partial offload, rest to CPU else: - self.infer_task() - self.model = self.load_pipeline_model() - self.pipe = pipeline( - task=self.task, model=self.model, tokenizer=self.tokenizer - ) # , device=self.device) - - elif self.config.model_type == "llm": - self.model = self.load_llm_model() - else: - raise ValueError( - f"Model type {self.config.model_type} is unknown or not supported." + self.logger.info("[LLMInference] No GPU detected — running on CPU.") + return 0 + except ImportError: + self.logger.warning( + "[LLMInference] torch not available — defaulting to CPU." ) + return 0 - def process_query(self, query): + def _ensure_model_available(self): """ - Process the query to ensure it is in the correct format. - param query: str, list, or file path + Ensure the model is available locally, pulling it if necessary. + If no internet connection is available, raise an error. """ - if isinstance(query, str): - # If file path is provided, read the file - if os.path.isfile(query): - with open(query, "r") as file: - query = file.read() - return [query] - elif isinstance(query, list): - return query - else: - raise ValueError( - "Unsupported query type. Use 'string', 'list' or provide a file to a path containing text." + if self._model_is_local(): + self.logger.info(f"[LLMInference] Model '{self.model_name}' found locally.") + return + + # Model not local — need internet to pull it + if not self._is_online(): + raise RuntimeError( + f"[LLMInference] Model '{self.model_name}' is not available locally " + f"and there is no internet connection to pull it. " + f"Run with connectivity first so the model can be downloaded and cached." ) + self.logger.info( + f"[LLMInference] Pulling '{self.model_name}' (this only happens once)..." + ) + self.pull_model(self.model_name) + self.logger.info( + f"[LLMInference] '{self.model_name}' saved locally — offline use enabled." + ) + + def pull_model(self, model_name): + available = [m.model for m in ollama.list().models] + if model_name not in available: + for chunk in ollama.pull(model_name, stream=True): + if chunk.status == "pulling manifest" or chunk.completed: + pct = ( + f"{chunk.completed/chunk.total*100:.1f}%" if chunk.total else "" + ) + print(f"\r{chunk.status} {pct}", end="", flush=True) + print(f"\nDone.") + def infer_task(self) -> str: name = self.model_name.lower() if "ner" in name or "token" in name: @@ -122,83 +168,6 @@ def infer_task(self) -> str: f"Cannot infer task from model name: {self.model_name}. Please specify manually in model config." ) - def load_pipeline_model(self): - # device_map = "auto" if self.device != "cpu" else "cpu" - torch_dtype = torch.bfloat16 if self.device != "cpu" else torch.float32 - kwargs = {"torch_dtype": torch_dtype} - if self.config.quantize: - kwargs["quantization_config"] = self.quantization_config - - if self.task == "token-classification": - # Bert models for token classification do not support device_map - model_class = AutoModelForTokenClassification - elif self.task == "question-answering": - kwargs["device_map"] = "auto" if self.device != "cpu" else "cpu" - model_class = AutoModelForQuestionAnswering - else: - raise ValueError(f"Unsupported task: {self.task}") - - return model_class.from_pretrained(self.model_name, **kwargs) - - def load_llm_model(self): - if self.device == torch.device("cpu"): - self.logger.warning("[LLMInference] CPU detected — skipping quantization.") - return AutoModelForCausalLM.from_pretrained( - self.model_name, low_cpu_mem_usage=True, local_files_only=True - ) - - kwargs = {"low_cpu_mem_usage": True} - - if self.config.quantize: - from transformers import BitsAndBytesConfig - - try: - kwargs["quantization_config"] = BitsAndBytesConfig( - load_in_4bit=True, - bnb_4bit_quant_type="nf4", - bnb_4bit_compute_dtype=torch.bfloat16, - bnb_4bit_use_double_quant=True, - ) - kwargs["device_map"] = "auto" - except Exception as e: - self.get_logger().error( - f"[LLMInference] Failed to create quant config: {e}" - ) - self.get_logger().warning( - "[LLMInference] Falling back to full-precision model." - ) - else: - kwargs["torch_dtype"] = torch.bfloat16 - kwargs["device_map"] = {"": 0} - - model = AutoModelForCausalLM.from_pretrained(self.model_name, **kwargs) - model.eval() - print(f"[LLMInference] Model {self.model_name} loaded successfully.") - return model - - def run_inference( - self, query: str, context: Optional[str] = None, max_tokens=56 - ) -> str: - result = None - if self.config.model_type == "pipeline": - if self.task == "question-answering": - if not context: - raise ValueError("Question Answering task requires context.") - result = self.pipe(question=query, context=context) - else: - result = self.pipe(query) - elif self.config.model_type == "llm": - input_ids = self.tokenizer( - query, return_tensors="pt" - ) # .to(self.model.device) - with torch.inference_mode(): - output_ids = self.model.generate(max_new_tokens=max_tokens, **input_ids) - result = self.tokenizer.decode(output_ids[0], skip_special_tokens=True) - print(f"LLM output: {result}") - generated_text = re.sub(re.escape(query), "", result).strip() - - return generated_text - def serialise_output(self, output): """ Serialise the output to a format that can be saved to JSON. @@ -249,7 +218,7 @@ def interest_commonality_llm(interests: list[str]) -> str: :param interests: a list of interests :return: a sentence describing the commonalities """ - config = ModelConfig(model_name=models["Qwen"], model_type="llm", quantize=False) + config = ModelConfig(model_name=models["Qwen"], model_type="llm") sentence = ", ".join(interests) query = create_query( sentence, @@ -266,7 +235,7 @@ def introduce_llm(name: str, drink: str, interests: str) -> str: """ Create a sentence introducing a person using the given name, drink, and interests. """ - config = ModelConfig(model_name=models["Qwen"], model_type="llm", quantize=False) + config = ModelConfig(model_name=models["Qwen"], model_type="llm") input_summary = f"Name: {name}, Favorite drink: {drink}, Interests: {interests}" prompt = f"Create a sentence that introduces a person named {name}, mentioning their favorite drink ({drink}) and their interest in {interests}." @@ -284,31 +253,31 @@ def classify_category(objects: List[str]) -> str: :param objects: a list of interests :return: category """ - config = ModelConfig(model_name=models["Qwen"], model_type="llm", quantize=False) + config = ModelConfig(model_name=models["Qwen"], model_type="llm") sentence = ", ".join(objects) query = create_query( sentence, "Detect which category these or a object belongs to." ) - inference = LLMInference(config, query) - response = inference.run_inference() + inference = LLMInference(config) + response = inference.run_inference(query) # print(response) parsed_response = truncate_llm_output(response[0]) return parsed_response -def link_category(object: str, category: list[str]) -> str: +def link_category(object: str, categories: list[str]) -> str: """ Classify category between a list of objects. :param objects: a list of interests :return: category """ - config = ModelConfig(model_name=models["Qwen"], model_type="llm", quantize=False) + config = ModelConfig(model_name=models["Qwen"], model_type="llm") + categories_str = ", ".join(categories) query = create_query( - category, - "Detect which category {object} belongs to the most. If not appropreate category to go return 'new'", + f"Detect which category {object} belongs to the most from the following categories: {categories_str}. If not appropriate category to go return 'new'", ) - inference = LLMInference(config, query) - response = inference.run_inference() + inference = LLMInference(config) + response = inference.run_inference(query) # print(response) parsed_response = truncate_llm_output(response[0]) return parsed_response @@ -319,17 +288,17 @@ def extract_fields_llm(text: str, fields: List[str]) -> Dict: Extracts structured information from a sentence using an LLM. Returns a dictionary with all fields — missing ones are filled with 'Unknown'. """ - config = ModelConfig(model_name=models["Qwen"], model_type="llm", quantize=False) + config = ModelConfig(model_name=models["Qwen"], model_type="llm") if fields is None: fields = ["Name", "Favourite drink", "Interests"] query = create_query(text, "extract_fields", fields) - inference = LLMInference(config, query) - response = inference.run_inference() + inference = LLMInference(config) + response = inference.run_inference(query) # Parse the model response - parsed = parse_llm_output_to_dict(response[0], fields) + parsed = parse_llm_output_to_dict(response, fields) # Fill missing or empty fields with "Unknown" result = {field: parsed.get(field, "Unknown") or "Unknown" for field in fields} @@ -339,7 +308,7 @@ def extract_fields_llm(text: str, fields: List[str]) -> Dict: def main(): # # Examples for testing - # config = ModelConfig(model_name=models["Qwen"], model_type="llm", quantize=True) + # config = ModelConfig(model_name=models["Qwen"], model_type="llm") # # sentence = "My name is John, my favourite drink is green tea, and my interests are robotics." # # sentence = "I am John, I usually drink green tea, and I really like robotics. I also like to play chess and watch movies." @@ -352,31 +321,33 @@ def main(): # print(response) # inference.log_output(response) - print("\n🔍 TEST: extract_fields_llm") - sentence = "Oh hi yeah, I'm John erm I drink tea usually green, and I am a robotics enthusiast. I also like to play chess and watch movies when I can." - extracted = extract_fields_llm(sentence, ["Name", "Favourite drink", "Interests"]) + # print("\n🔍 TEST: extract_fields_llm") + # sentence = "Oh hi yeah, I'm John erm and I drink tea usually green." + sentence = "Hi Tiago, My name is Hayeong and my favourite drink is matcha." + print(f"Input sentence: {sentence}") + extracted = extract_fields_llm(sentence, ["Name", "Favourite drink"]) print("Extracted fields:", extracted) - print("\n🔍 TEST: interest_commonality_llm") - interests = ["robotics", "chess"] - commonality = interest_commonality_llm(interests) - print("Commonality summary:", commonality) - - print("\n🔍 TEST: introduce_llm") - intro = introduce_llm(name="Eunice", drink="green tea", interests="swimming") - print("Introduction:", intro) - - print("\n🔍 TEST: classify_category") - category = classify_category(["apple"]) # FIXED: must pass a list - print("Classified category:", category) - - print("\n🔍 TEST: link_category for 'apple'") - linked_apple = link_category("apple", ["fruit", "drink", "new"]) - print("Linked category:", linked_apple) - - print("\n🔍 TEST: link_category for 'basketball'") - linked_basketball = link_category("basketball", ["fruit", "drink", "new"]) - print("Linked category:", linked_basketball) + # print("\n🔍 TEST: interest_commonality_llm") + # interests = ["robotics", "chess"] + # commonality = interest_commonality_llm(interests) + # print("Commonality summary:", commonality) + # + # print("\n🔍 TEST: introduce_llm") + # intro = introduce_llm(name="Eunice", drink="green tea", interests="swimming") + # print("Introduction:", intro) + # + # print("\n🔍 TEST: classify_category") + # category = classify_category(["apple"]) # FIXED: must pass a list + # print("Classified category:", category) + # + # print("\n🔍 TEST: link_category for 'apple'") + # linked_apple = link_category("apple", ["fruit", "drink", "new"]) + # print("Linked category:", linked_apple) + # + # print("\n🔍 TEST: link_category for 'basketball'") + # linked_basketball = link_category("basketball", ["fruit", "drink", "new"]) + # print("Linked category:", linked_basketball) if __name__ == "__main__": diff --git a/common/language/lasr_llm/lasr_llm/nodes/hri_task_service.py b/common/language/lasr_llm/lasr_llm/nodes/hri_task_service.py index 1a7f3a189..b2714e5e1 100644 --- a/common/language/lasr_llm/lasr_llm/nodes/hri_task_service.py +++ b/common/language/lasr_llm/lasr_llm/nodes/hri_task_service.py @@ -20,7 +20,7 @@ class HRITaskLLMService(Node): def __init__(self): super().__init__("hri_task_query_llm_service") self.create_service(HRITaskQueryLlm, "/hri_task/query_llm", self.query_llm) - config = ModelConfig(model_name="Qwen/Qwen2.5-1.5B", model_type="llm", quantize=False) + config = ModelConfig(model_name="Qwen/Qwen2.5-1.5B", model_type="llm") self.llm_inference = LLMInference(config) self.get_logger().info("HRI Task Query LLM service started") @@ -29,7 +29,9 @@ def query_llm(self, request, response): Handle the query to the LLM. This function processes the request and returns a response. """ - self.get_logger().info(f"Received query: {request.llm_input}") + self.get_logger().info( + f"Received query: {request.llm_input}, and task is {request.task}" + ) task = request.task if task == "name": diff --git a/common/language/lasr_llm/lasr_llm/utils.py b/common/language/lasr_llm/lasr_llm/utils.py index 111e861ab..5962e508b 100644 --- a/common/language/lasr_llm/lasr_llm/utils.py +++ b/common/language/lasr_llm/lasr_llm/utils.py @@ -2,29 +2,52 @@ from typing import Optional, List, Dict +def _normalise_field_name(field: str) -> str: + field = field.lower().replace("_", " ").replace("favorite", "favourite") + return " ".join(field.split()) + + +def _field_aliases(field: str) -> List[str]: + aliases = [field] + normalised = _normalise_field_name(field) + + if normalised == "favourite drink": + aliases += ["Favorite drink", "Favourite_drink", "Favorite_drink", "Drink"] + elif normalised == "interests": + aliases.append("Interest") + + return sorted(set(aliases), key=len, reverse=True) + + +def _label_pattern(label: str) -> str: + words = re.split(r"[\s_]+", label.strip()) + return r"[\s_]+".join(re.escape(word) for word in words if word) + + def parse_llm_output_to_dict(output: str, fields: List[str]) -> Dict: - """ - Parse the llm response to get the requested fields into a dict where the keys are the field names, - and the values are the result from the response. - :param output: llm response - :param fields: the fields requested to extract - :return: dictionary of field names and their values - """ field_dict = {field: None for field in fields} - for field in fields: - field_pattern = field.replace(" ", "[_ ]") - pattern = re.compile( - rf"{field_pattern}:\s*(.*?)(?:\n|$)", re.IGNORECASE - ) # field: value - match = pattern.search(output) - if match: - field_dict[field] = match.group(1).strip() + separator = re.compile(r"\s*(?:[:=]|\s+-\s+)\s*") + # print(f"DEBUG: Parsing LLM output:\n{output}") + + for line in output.splitlines(): + sep_match = separator.search(line) + if not sep_match: + continue + + key = line[: sep_match.start()].strip().strip("\"'`") + value = line[sep_match.end() :].strip().strip(" \t\r\n,;:-\"'`") + + for field in fields: + if any(key.lower() == alias.lower() for alias in _field_aliases(field)): + field_dict[field] = value + break return field_dict def truncate_llm_output(output: str) -> str: """ - If the output is too long, truncate it to a reasonable length, after the first sentence. + If the output is too long, truncate it after the first sentence. + :param output: the output from the LLM :return: the parsed output """ @@ -49,14 +72,38 @@ def create_query(text: str, task: str, fields: Optional[List[str]] = None): assert ( fields is not None ), "Fields must be provided for the 'extract_fields' task." - field_str = "\n".join([f"- {field}" for field in fields]) - query = f"Extract the following fields from the sentence:\n{field_str}\n\n For example, the sentence ' my favourite drink is coca cola' should have the field favourite_drink matched to 'coca cola' \n\n Sentence: {text}." + field_str = "\n".join([f"- {field}" for field in fields]) # "\n- Name" + query = ( + "Extract the following fields from the sentence:\n" + f"{field_str}\n\n" + "Return only one line per requested field using this format:\n" + "Field: value\n" + "Do not list the requested fields. If a value is missing, leave " + "it empty after the colon.\n\n" + "For example, the sentence 'my favourite drink is coca cola' " + "should return:\n" + "Favourite drink: coca cola\n\n" + f"Sentence: {text}." + ) + # print(f"DEBUG query sent to LLM: {query}") elif task == "interest_commonality": - query = f"Extract the commonality (if it exists) of the following interests of two people:\n\nSentences: {text}. \n\n For example, the sentences 'I like football' and 'I like basketball' should have the commonality 'you both like sports'. If there is no common interest, say 'you have no common interests'\n\n" - # query = f"Extract the commonality (if it exists) of the following interests:\n\nInterests: {text}.\nFormat it as a sentence: 'you both have interests which are...'" + query = ( + "Extract the commonality (if it exists) of the following " + f"interests of two people:\n\nSentences: {text}.\n\n" + "For example, the sentences 'I like football' and 'I like " + "basketball' should have the commonality 'you both like sports'. " + "If there is no common interest, say 'you have no common " + "interests'\n\n" + ) + # query = ( + # "Extract the commonality (if it exists) of the following " + # f"interests:\n\nInterests: {text}.\nFormat it as a sentence: " + # "'you both have interests which are...'" + # ) else: raise ValueError( - f"Unknown task: {task}. Supported tasks are 'extract_fields' and 'interest_commonality'." + f"Unknown task: {task}. Supported tasks are 'extract_fields' and " + "'interest_commonality'." ) return query diff --git a/common/language/lasr_llm/requirements.in b/common/language/lasr_llm/requirements.in index 98e4207f3..5fefc7777 100644 --- a/common/language/lasr_llm/requirements.in +++ b/common/language/lasr_llm/requirements.in @@ -1,5 +1,3 @@ numpy<2.0 -transformers>=4.44,<4.46 -accelerate>=0.33,<1.0 -bitsandbytes>=0.49 torch +ollama diff --git a/common/language/lasr_llm/requirements.txt b/common/language/lasr_llm/requirements.txt index 98e4207f3..7722a1d5b 100644 --- a/common/language/lasr_llm/requirements.txt +++ b/common/language/lasr_llm/requirements.txt @@ -1,5 +1,108 @@ -numpy<2.0 -transformers>=4.44,<4.46 -accelerate>=0.33,<1.0 -bitsandbytes>=0.49 -torch +# +# This file is autogenerated by pip-compile with Python 3.13 +# by the following command: +# +# pip-compile requirements.in +# +annotated-types==0.7.0 + # via pydantic +anyio==4.14.0 + # via httpx +certifi==2026.6.17 + # via + # httpcore + # httpx +cuda-bindings==13.3.1 + # via torch +cuda-pathfinder==1.5.5 + # via cuda-bindings +cuda-toolkit[cudart,cufft,cufile,cupti,curand,cusolver,cusparse,nvjitlink,nvrtc,nvtx]==13.0.2 + # via torch +filelock==3.29.4 + # via torch +fsspec==2026.6.0 + # via torch +h11==0.16.0 + # via httpcore +httpcore==1.0.9 + # via httpx +httpx==0.28.1 + # via ollama +idna==3.18 + # via + # anyio + # httpx +jinja2==3.1.6 + # via torch +markupsafe==3.0.3 + # via jinja2 +mpmath==1.3.0 + # via sympy +networkx + # via torch +numpy==1.26.4 + # via -r requirements.in +nvidia-cublas==13.1.1.3 + # via + # nvidia-cudnn-cu13 + # nvidia-cusolver + # torch +nvidia-cuda-cupti==13.0.85 + # via cuda-toolkit +nvidia-cuda-nvrtc==13.0.88 + # via + # cuda-toolkit + # nvidia-cublas +nvidia-cuda-runtime==13.0.96 + # via cuda-toolkit +nvidia-cudnn-cu13==9.20.0.48 + # via torch +nvidia-cufft==12.0.0.61 + # via cuda-toolkit +nvidia-cufile==1.15.1.6 + # via cuda-toolkit +nvidia-curand==10.4.0.35 + # via cuda-toolkit +nvidia-cusolver==12.0.4.66 + # via cuda-toolkit +nvidia-cusparse==12.6.3.3 + # via + # cuda-toolkit + # nvidia-cusolver +nvidia-cusparselt-cu13==0.8.1 + # via torch +nvidia-nccl-cu13==2.29.7 + # via torch +nvidia-nvjitlink==13.0.88 + # via + # cuda-toolkit + # nvidia-cufft + # nvidia-cusolver + # nvidia-cusparse +nvidia-nvshmem-cu13==3.4.5 + # via torch +nvidia-nvtx==13.0.85 + # via cuda-toolkit +ollama==0.6.2 + # via -r requirements.in +pydantic==2.13.4 + # via ollama +pydantic-core==2.46.4 + # via pydantic +sympy==1.14.0 + # via torch +torch==2.12.0 + # via -r requirements.in +triton==3.7.0 + # via torch +typing-extensions==4.15.0 + # via + # pydantic + # pydantic-core + # torch + # typing-inspection +typing-inspection==0.4.2 + # via pydantic + +# The following packages are considered to be unsafe in a requirements file: +# setuptools diff --git a/common/language/lasr_llm/setup.py b/common/language/lasr_llm/setup.py index 72724b82f..e070f5b78 100644 --- a/common/language/lasr_llm/setup.py +++ b/common/language/lasr_llm/setup.py @@ -3,7 +3,6 @@ import setuptools.command.install import ament_virtualenv.install - _here = os.path.dirname(os.path.abspath(__file__)) @@ -22,6 +21,7 @@ def run(self): ) return + setup( name=package_name, version="0.0.0", diff --git a/common/language/lasr_llm/test/test_utils.py b/common/language/lasr_llm/test/test_utils.py new file mode 100644 index 000000000..5ae745d6b --- /dev/null +++ b/common/language/lasr_llm/test/test_utils.py @@ -0,0 +1,62 @@ +# Copyright 2026 King's College London +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import importlib.util +from pathlib import Path + +UTILS_PATH = Path(__file__).resolve().parents[1] / "lasr_llm" / "utils.py" +SPEC = importlib.util.spec_from_file_location("lasr_llm_utils", UTILS_PATH) +UTILS = importlib.util.module_from_spec(SPEC) +SPEC.loader.exec_module(UTILS) +parse_llm_output_to_dict = UTILS.parse_llm_output_to_dict + + +def test_parse_llm_output_accepts_equals_separator(): + parsed = parse_llm_output_to_dict( + "favourite drink = pepsi", + ["Favourite drink"], + ) + + assert parsed == {"Favourite drink": "pepsi"} + + +def test_parse_llm_output_accepts_case_and_spelling_variants(): + parsed = parse_llm_output_to_dict( + "NAME: Alice\nfavorite_drink: Orange juice\ninterest: robotics", + ["Name", "Favourite drink", "Interests"], + ) + + assert parsed == { + "Name": "Alice", + "Favourite drink": "Orange juice", + "Interests": "robotics", + } + + +def test_parse_llm_output_ignores_requested_fields_echo(): + parsed = parse_llm_output_to_dict( + "fields: name, favourite drink", + ["Name", "Favourite drink"], + ) + + assert parsed == {"Name": None, "Favourite drink": None} + + +def test_parse_llm_output_accepts_inline_fields(): + parsed = parse_llm_output_to_dict( + "Name: Alice, Favourite drink = pepsi", + ["Name", "Favourite drink"], + ) + + assert parsed == {"Name": "Alice", "Favourite drink": "pepsi"} diff --git a/common/language/lasr_llm_interfaces/CMakeLists.txt b/common/language/lasr_llm_interfaces/CMakeLists.txt index f87f6df38..902060678 100644 --- a/common/language/lasr_llm_interfaces/CMakeLists.txt +++ b/common/language/lasr_llm_interfaces/CMakeLists.txt @@ -57,4 +57,4 @@ endif() ament_export_dependencies(rosidl_default_runtime) -ament_package() +ament_package() \ No newline at end of file diff --git a/common/simulation/maps/label_locations.py b/common/simulation/maps/label_locations.py index 59caa2832..842061ab2 100644 --- a/common/simulation/maps/label_locations.py +++ b/common/simulation/maps/label_locations.py @@ -142,7 +142,10 @@ def on_click(event): )[0] fig.canvas.draw_idle() # focus the textbox - textbox.begin_typing(None) + try: + textbox.begin_typing() + except TypeError: + textbox.begin_typing(None) def on_key(event): if event.key == "u": diff --git a/common/simulation/maps/locations.yaml b/common/simulation/maps/locations.yaml index c51a82daa..988f61b79 100644 --- a/common/simulation/maps/locations.yaml +++ b/common/simulation/maps/locations.yaml @@ -1,4 +1,14 @@ locations: + bedroom: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: -0.765 + y: 2.524 + z: 0.0 kitchen: orientation: w: 1.0 @@ -6,6 +16,16 @@ locations: y: 0.0 z: 0.0 position: - x: 2.233 - y: -2.924 + x: 1.717 + y: -1.3 + z: 0.0 + living room: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 7.546 + y: 0.722 z: 0.0 diff --git a/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/__init__.py b/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/__init__.py index e69de29bb..f662b86a0 100755 --- a/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/__init__.py +++ b/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/__init__.py @@ -0,0 +1 @@ +from .cache import ModelCache diff --git a/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/cache.py b/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/cache.py index 259ffffa5..63ff376c7 100755 --- a/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/cache.py +++ b/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/cache.py @@ -45,13 +45,13 @@ def load_model( "lasr_speech_recognition_whisper", ) ) - example_fp = os.path.join(package_root, "test.m4a") - self.get_logger().info( - "Running transcription on example file to ensure model is loaded..." - ) - test_result: str = MODEL_CACHE[name].transcribe( - example_fp, fp16=device == "cuda" - ) - self.get_logger().info(f"Transcription test result: {test_result}") + # example_fp = os.path.join(package_root, "test.m4a") + # self.get_logger().info( + # "Running transcription on example file to ensure model is loaded..." + # ) + # test_result: str = MODEL_CACHE[name].transcribe( + # example_fp, fp16=device == "cuda" + # ) + # self.get_logger().info(f"Transcription test result: {test_result}") return MODEL_CACHE[name] diff --git a/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/transcribe_microphone_server.py b/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/transcribe_microphone_server.py index 1b28a1ca1..08eb8dd17 100755 --- a/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/transcribe_microphone_server.py +++ b/common/speech/lasr_speech_recognition_whisper/lasr_speech_recognition_whisper/transcribe_microphone_server.py @@ -41,7 +41,7 @@ class speech_model_params: pause_threshold (Optional[float]): Seconds of non-speaking audio before a phrase is considered complete. Defaults to 0.8 seconds. """ - model_name: str = "medium.en" + model_name: str = "small.en" device: str = "cuda" if torch.cuda.is_available() else "cpu" start_timeout: float = 5.0 phrase_duration: Optional[float] = 10 @@ -267,7 +267,7 @@ def parse_args() -> dict: parser.add_argument( "--model_name", type=str, - default="medium.en", + default="small.en", help="Name of the speech recognition model.", ) parser.add_argument( @@ -302,7 +302,7 @@ def parse_args() -> dict: parser.add_argument( "--energy_threshold", - type=Optional[int], + type=int, default=None, help="Energy threshold for silence detection. Using this disables automatic adjustment", ) @@ -330,7 +330,7 @@ def configure_model_params(config: dict) -> speech_model_params: """ model_params = speech_model_params() if config["model_name"]: - model_params.model_name = config["model_name"] + model_params.model_name = "small.en" if config["device"]: model_params.device = config["device"] if config["start_timeout"]: diff --git a/common/speech/lasr_speech_recognition_whisper/scripts/microphone_tuning_test.py b/common/speech/lasr_speech_recognition_whisper/scripts/microphone_tuning_test.py index 026ab2875..f12f07b7d 100755 --- a/common/speech/lasr_speech_recognition_whisper/scripts/microphone_tuning_test.py +++ b/common/speech/lasr_speech_recognition_whisper/scripts/microphone_tuning_test.py @@ -5,7 +5,7 @@ import numpy as np from pathlib import Path import speech_recognition as sr -from src import ModelCache # type: ignore +from lasr_speech_recognition_whisper import ModelCache import sounddevice # needed to remove ALSA error messages from typing import Dict import rclpy @@ -38,7 +38,7 @@ def main(args=None): recognizer = sr.Recognizer() recognizer.pause_threshold = 2 microphone = sr.Microphone(device_index=args["device_index"], sample_rate=16000) - threshold = 100 + threshold = 10000 recognizer.dynamic_energy_threshold = False recognizer.energy_threshold = threshold model_cache = ModelCache() @@ -67,7 +67,7 @@ def main(args=None): print( f"Transcription: {transcription_result} at energy threshold {recognizer.energy_threshold}" ) - threshold += 100 + threshold += 10000 recognizer.energy_threshold = threshold diff --git a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py index 02844c3b4..34d854a5e 100644 --- a/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py +++ b/common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py @@ -5,12 +5,14 @@ from shapely.geometry.polygon import Polygon as ShapelyPolygon from shapely.validation import explain_validity import rospy -from rclpy.wait_for_message import wait_for_message from cv2_img import cv2_img_to_msg, msg_to_cv2_img from cv2_pcl import pcl_to_cv2 from sensor_msgs.msg import Image, PointCloud2 from geometry_msgs.msg import Point, Polygon, PoseWithCovarianceStamped +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + + from lasr_vision_interfaces.msg import CDRequest, CDResponse, Detection, Detection3D from lasr_vision_interfaces.srv import ( CroppedDetection, @@ -296,10 +298,10 @@ def process_single_detection_request( node, request: CDRequest, rgb_image_topic: str = "/head_front_camera/rgb/image_raw", - depth_image_topic: str = "/head_front_camera/depth/points", + depth_image_topic: str = "/head_front_camera/depth/image_raw", yolo_2d_service_name: str = "/yolov8/detect", yolo_3d_service_name: str = "/yolov8/detect3d", - robot_pose_topic: str = "/robot_pose", + robot_pose_topic: str = "/amcl_pose", debug_topic: str = "/lasr_vision/cropped_detection/debug", ) -> CDResponse: """Dispatches a detection request to the appropriate bounding box/mask 2D or 3D cropped @@ -324,6 +326,42 @@ def process_single_detection_request( "top-most", "bottom-most", ] + + rgb_image = None + robot_pose = None + pointcloud_msg = None + + camera_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + ) + + pose_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) + + def camera_cb(msg): + global rgb_image + rgb_image = msg + + def pose_cb(msg): + global robot_pose + robot_pose = msg + + def point_cb(msg): + global pointcloud_msg + pointcloud_msg = msg + + node.create_subscription(Image, rgb_image_topic, camera_cb, qos_profile=camera_qos) + node.create_subscription( + PoseWithCovarianceStamped, robot_pose_topic, pose_cb, qos_profile=pose_qos + ) + node.create_subscription(PointCloud2, depth_image_topic, point_cb, 10) + valid_3d_crop_methods = ["closest", "furthest"] response = CDResponse() combined_mask = None @@ -343,8 +381,7 @@ def process_single_detection_request( if request.rgb_image.data: rgb_image = request.rgb_image else: - success, rgb_image = wait_for_message(Image, node, rgb_image_topic) - if not success: + if rgb_image is None: node.get_logger().error( f"Failed to receive rgb image from {rgb_image_topic}" ) @@ -389,10 +426,7 @@ def process_single_detection_request( yolo_3d_client = yolo_client_cache[key] - success, robot_pose = wait_for_message( - PoseWithCovarianceStamped, node, robot_pose_topic - ) - if not success: + if robot_pose is None: node.get_logger().error( f"Failed to receive robot pose from {robot_pose_topic}" ) @@ -402,10 +436,7 @@ def process_single_detection_request( if request.pointcloud.data: pointcloud_msg = request.pointcloud else: - success, pointcloud_msg = wait_for_message( - PointCloud2, node, depth_image_topic - ) - if not success: + if pointcloud_msg is None: node.get_logger().error( f"Failed to receive pointcloud from {depth_image_topic}" ) diff --git a/common/vision/lasr_vision_eye_tracker/lasr_vision_eye_tracker/eye_tracker_action_server.py b/common/vision/lasr_vision_eye_tracker/lasr_vision_eye_tracker/eye_tracker_action_server.py index e27e4ee20..3f405f550 100644 --- a/common/vision/lasr_vision_eye_tracker/lasr_vision_eye_tracker/eye_tracker_action_server.py +++ b/common/vision/lasr_vision_eye_tracker/lasr_vision_eye_tracker/eye_tracker_action_server.py @@ -1,14 +1,11 @@ import rclpy from rclpy.node import Node from rclpy.action import ActionServer, ActionClient, CancelResponse, GoalResponse -from rclpy.callback_groups import ( - MutuallyExclusiveCallbackGroup, - ReentrantCallbackGroup, -) +from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy from rclpy.executors import MultiThreadedExecutor import message_filters -import threading +from threading import RLock, Event from typing import Tuple, Optional # ROS2 message imports (same as ROS1, just rclpy instead of rospy) @@ -36,6 +33,36 @@ from sensor_msgs.msg import Image, CameraInfo from std_msgs.msg import Header +import time + + +class WaitForFuture: + def __init__(self): + self.event = Event() + self.lock = RLock() + self.response = None + self.result = None + self.status = None + self.handle = None + + def set(self): + self.event.clear() + + def handle_goal(self, future): + with self.lock: + self.handle = future.result() + get_result_future = self.handle.get_result_async() + get_result_future.add_done_callback(self.handle_result) + + def handle_result(self, future): + self.result = future.result().result + self.status = future.result().status + self.event.set() + + def handle_resp(self, future): + self.response = future.result() + self.event.set() + class EyeTracker(Node): def __init__(self, max_eye_distance: float = 1.5): @@ -43,29 +70,30 @@ def __init__(self, max_eye_distance: float = 1.5): # Humble deadlock avoidance: callbacks that make blocking service/action calls # must not share one mutually-exclusive group with their done-callbacks. - self._action_cb_group = MutuallyExclusiveCallbackGroup() - self._work_cb_group = ReentrantCallbackGroup() self._done: bool = False self._eyes: Optional[Point] = None self._robot_point: Optional[Point] = None self._max_eye_distance: float = max_eye_distance - self._move_up_count: float = 0.0 + self._move_up_count: int = 0 self._max_move_up_count: int = 2 - + + self._action_cb_group = ReentrantCallbackGroup() + self._work_cb_group = ReentrantCallbackGroup() + self.camera_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, ) - + amcl_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, ) - + self._robot_pose_sub = self.create_subscription( PoseWithCovarianceStamped, "/amcl_pose", @@ -73,21 +101,18 @@ def __init__(self, max_eye_distance: float = 1.5): qos_profile=amcl_qos, callback_group=self._work_cb_group, ) + self._yolo_keypoint_client = self.create_client( YoloPoseDetection3D, "/yolo/detect3d_pose", callback_group=self._work_cb_group, ) - while not self._yolo_keypoint_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info("Waiting for YOLO keypoint service...") self._head_state_client = self.create_client( QueryTrajectoryState, "/head_controller/query_state", callback_group=self._work_cb_group, ) - while not self._head_state_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info("Waiting for head state service...") self._head_action_client = ActionClient( self, @@ -95,10 +120,6 @@ def __init__(self, max_eye_distance: float = 1.5): "/head_controller/follow_joint_trajectory", callback_group=self._work_cb_group, ) - while not self._head_action_client.wait_for_server(timeout_sec=1.0): - self.get_logger().info( - "Waiting for follow joint trajectory action server..." - ) self._head_point_action_client = ActionClient( self, @@ -106,8 +127,16 @@ def __init__(self, max_eye_distance: float = 1.5): "/head_controller/point_head_action", callback_group=self._work_cb_group, ) - while not self._head_point_action_client.wait_for_server(timeout_sec=1.0): - self.get_logger().info("Waiting for point head action server...") + + while ( + not self._head_point_action_client.wait_for_server(timeout_sec=1.0) + or not self._head_action_client.wait_for_server(timeout_sec=1.0) + or not self._yolo_keypoint_client.wait_for_service(timeout_sec=1.0) + or not self._head_action_client.wait_for_server(timeout_sec=1.0) + ): + self.get_logger().info( + "Waiting for point head action server, and head action client and yolo to all be ready..." + ) self._action_server = ActionServer( self, @@ -137,32 +166,21 @@ def _robot_pose_callback(self, msg: PoseWithCovarianceStamped) -> None: def _get_head_join_values(self) -> Optional[Tuple[float, float]]: """Returns the x,y position of the head joints.""" - try: - request = QueryTrajectoryState.Request() - request.time = self.get_clock().now().to_msg() - - response = self._head_state_client.call(request) - if response is None or len(response.position) < 2: - self.get_logger().warn("Head state response was empty or invalid.") - return None - return (response.position[0], response.position[1]) - except Exception as e: - self.get_logger().error(f"Service call failed: {e}") - return None + request = QueryTrajectoryState.Request() + request.time = self.get_clock().now().to_msg() - def _wait_for_future_result(self, future, timeout_sec: float, what: str): - done_event = threading.Event() - future.add_done_callback(lambda _: done_event.set()) + wait = WaitForFuture() + wait.set() - if not done_event.wait(timeout_sec): - self.get_logger().error(f"Timed out waiting for {what}.") - return None + future = self._head_state_client.call_async(request) + future.add_done_callback(wait.handle_resp) + while not wait.event.wait(): + pass - try: - return future.result() - except Exception as e: - self.get_logger().error(f"{what} failed: {e}") + if len(wait.response.position) < 2: + self.get_logger().warn("Head state response was empty or invalid.") return None + return (wait.response.position[0], wait.response.position[1]) def _look_centre(self) -> None: """Moves the head to look at the centre position.""" @@ -172,14 +190,14 @@ def _look_centre(self) -> None: point.positions = [0.0, 0.0] # Look Center point.time_from_start = rclpy.duration.Duration(seconds=1.0).to_msg() goal.trajectory.points.append(point) + + wait = WaitForFuture() + wait.set() + send_goal_future = self._head_action_client.send_goal_async(goal) - goal_handle = self._wait_for_future_result( - send_goal_future, - timeout_sec=2.0, - what="centre head goal response", - ) - if goal_handle is None or not goal_handle.accepted: - self.get_logger().warn("Centre head goal was not accepted.") + send_goal_future.add_done_callback(wait.handle_goal) + while not wait.event.wait(0.5): + break def _move_head_up( self, current_head_position: Tuple[float, float], y_delta: float = 0.25 @@ -201,29 +219,95 @@ def _move_head_up( current_head_position[0], current_head_position[1] + y_delta, ] - point.time_from_start = rclpy.duration.Duration(seconds=1.0).to_msg() + point.time_from_start = rclpy.duration.Duration(seconds=2.0).to_msg() goal.trajectory.points.append(point) + wait = WaitForFuture() + wait.set() + send_goal_future = self._head_action_client.send_goal_async(goal) - goal_handle = self._wait_for_future_result( - send_goal_future, - timeout_sec=2.0, - what="move head up goal response", + send_goal_future.add_done_callback(wait.handle_goal) + while not wait.event.wait(0.5): + break + self._move_up_count += 1 + + def detect_cb(self, image: Image, depth_image: Image): + """Callback for detection from synced messages.""" + req = YoloPoseDetection3D.Request( + image_raw=image, + depth_image=depth_image, + depth_camera_info=self.depth_camera_info_cache.getLast(), + model="yolo11n-pose.pt", + confidence=0.5, + target_frame="map", ) - if goal_handle is None or not goal_handle.accepted: - self.get_logger().warn("Move-head-up goal was not accepted.") - self._move_up_count += 1 + wait = WaitForFuture() + wait.set() + + future = self._yolo_keypoint_client.call_async(req) + future.add_done_callback(wait.handle_resp) + while not wait.event.wait(): + pass + + detected_keypoints = wait.response.detections + left_eye_point = None + right_eye_point = None + if not detected_keypoints: + self._eyes = None + return + if not self._robot_point: + self._eyes = None + return + closest_eye_midpoint = None + closest_distance = self._max_eye_distance + for det in detected_keypoints: + eye_midpoint = None + for keypoint in det.keypoints: + if keypoint.keypoint_name == "left_eye": + left_eye_point = keypoint.point + elif keypoint.keypoint_name == "right_eye": + right_eye_point = keypoint.point + + if left_eye_point and right_eye_point: + # Calculate the midpoint of the two eyes + midpoint_x = (left_eye_point.x + right_eye_point.x) / 2.0 + midpoint_y = (left_eye_point.y + right_eye_point.y) / 2.0 + midpoint_z = (left_eye_point.z + right_eye_point.z) / 2.0 + + eye_midpoint = Point(x=midpoint_x, y=midpoint_y, z=midpoint_z) + elif left_eye_point: + eye_midpoint = Point( + x=left_eye_point.x, y=left_eye_point.y, z=left_eye_point.z + ) + elif right_eye_point: + eye_midpoint = Point( + x=right_eye_point.x, + y=right_eye_point.y, + z=right_eye_point.z, + ) + if eye_midpoint is not None: + # Calculate the distance from the robot point to the eye midpoint + distance = ( + (eye_midpoint.x - self._robot_point.x) ** 2 + + (eye_midpoint.y - self._robot_point.y) ** 2 + ) ** 0.5 + if distance < closest_distance: + closest_distance = distance + closest_eye_midpoint = eye_midpoint + if closest_eye_midpoint is not None: + self._eyes = closest_eye_midpoint def _execute_callback(self, goal_handle): """Execute the eye tracking goal.""" self.get_logger().info("Beginning eye tracking...") goal = goal_handle.request - if self._robot_point is None: - self.get_logger().warn( - "No /robot_pose received yet; continuing and waiting asynchronously." - ) + feedback_msg = EyeTrackerAction.Feedback() + feedback_msg.running = False + + while self._robot_point is None: + self.get_logger().warn("Waiting for robot pose") # First, look to person_point if goal.person_point is None: @@ -231,6 +315,26 @@ def _execute_callback(self, goal_handle): goal_handle.abort() return EyeTrackerAction.Result() + self.image_sub = message_filters.Subscriber( + self, Image, "/head_front_camera/rgb/image_raw", self.camera_qos + ) + self.depth_sub = message_filters.Subscriber( + self, Image, "/head_front_camera/depth/image_raw", self.camera_qos + ) + self.depth_camera_info_sub = message_filters.Subscriber( + self, CameraInfo, "/head_front_camera/depth/camera_info", self.camera_qos + ) + self.depth_camera_info_cache = message_filters.Cache(self.depth_camera_info_sub) + + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.image_sub, self.depth_sub], 10, 0.1 + ) + + self.subs = [self.image_sub, self.depth_sub, self.depth_camera_info_sub] + + wait = WaitForFuture() + wait.set() + g = PointHead.Goal( pointing_frame="head_2_link", pointing_axis=Vector3(x=1.0, y=0.0, z=0.0), @@ -243,105 +347,17 @@ def _execute_callback(self, goal_handle): # Send point head goal and wait send_goal_future = self._head_point_action_client.send_goal_async(g) - goal_response = self._wait_for_future_result( - send_goal_future, - timeout_sec=2.0, - what="initial point-head goal response", - ) - if goal_response is None or not goal_response.accepted: - self.get_logger().warn("Initial point-head goal was not accepted.") - - def detect_cb(image: Image, depth_image: Image, depth_camera_info: CameraInfo): - """Callback for detection from synced messages.""" - req = YoloPoseDetection3D.Request( - image_raw=image, - depth_image=depth_image, - depth_camera_info=depth_camera_info, - model="yolo11n-pose.pt", - confidence=0.5, - target_frame="map", - ) + send_goal_future.add_done_callback(wait.handle_goal) - try: - response = self._yolo_keypoint_client.call(req) - except Exception as e: - self.get_logger().error(f"YOLO service call failed: {e}") - return - - detected_keypoints = response.detections - left_eye_point = None - right_eye_point = None - if not detected_keypoints: - self._eyes = None - return - if not self._robot_point: - self._eyes = None - return - closest_eye_midpoint = None - closest_distance = self._max_eye_distance - for det in detected_keypoints: - eye_midpoint = None - for keypoint in det.keypoints: - if keypoint.keypoint_name == "left_eye": - left_eye_point = keypoint.point - elif keypoint.keypoint_name == "right_eye": - right_eye_point = keypoint.point - - if left_eye_point and right_eye_point: - # Calculate the midpoint of the two eyes - midpoint_x = (left_eye_point.x + right_eye_point.x) / 2.0 - midpoint_y = (left_eye_point.y + right_eye_point.y) / 2.0 - midpoint_z = (left_eye_point.z + right_eye_point.z) / 2.0 - - eye_midpoint = Point(x=midpoint_x, y=midpoint_y, z=midpoint_z) - elif left_eye_point: - eye_midpoint = Point( - x=left_eye_point.x, y=left_eye_point.y, z=left_eye_point.z - ) - elif right_eye_point: - eye_midpoint = Point( - x=right_eye_point.x, - y=right_eye_point.y, - z=right_eye_point.z, - ) - if eye_midpoint is not None: - # Calculate the distance from the robot point to the eye midpoint - distance = ( - (eye_midpoint.x - self._robot_point.x) ** 2 - + (eye_midpoint.y - self._robot_point.y) ** 2 - ) ** 0.5 - if distance < closest_distance: - closest_distance = distance - closest_eye_midpoint = eye_midpoint - if closest_eye_midpoint is not None: - self._eyes = closest_eye_midpoint - - image_sub = message_filters.Subscriber( - self, - Image, - "/head_front_camera/rgb/image_raw", - self.camera_qos - - ) - depth_sub = message_filters.Subscriber( - self, - Image, - "/head_front_camera/depth/image_raw", - self.camera_qos - ) - depth_camera_info_sub = message_filters.Subscriber( - self, - CameraInfo, - "/head_front_camera/depth/camera_info", - self.camera_qos - ) - ts = message_filters.ApproximateTimeSynchronizer( - [image_sub, depth_sub, depth_camera_info_sub], 10, 0.1 - ) - ts.registerCallback(detect_cb) + while not wait.event.wait(0.5): + break + + self.ts.registerCallback(self.detect_cb) self._done = False + feedback_msg.running = True while rclpy.ok() and not self._done: + goal_handle.publish_feedback(feedback_msg) if self._eyes is None: current_head_position = self._get_head_join_values() if current_head_position is None: @@ -352,39 +368,38 @@ def detect_cb(image: Image, depth_image: Image, depth_camera_info: CameraInfo): g = PointHead.Goal( pointing_frame="head_2_link", pointing_axis=Vector3(x=1.0, y=0.0, z=0.0), - max_velocity=2.0, + max_velocity=1.0, target=PointStamped( header=Header(frame_id="map"), point=self._eyes, ), ) + + wait = WaitForFuture() + wait.set() + send_goal_future = self._head_point_action_client.send_goal_async(g) - goal_response = self._wait_for_future_result( - send_goal_future, - timeout_sec=2.0, - what="tracking point-head goal response", - ) - if goal_response is None or not goal_response.accepted: - self.get_logger().warn("Tracking point-head goal was not accepted.") + send_goal_future.add_done_callback(wait.handle_goal) + while wait.event.wait(0.5): + break if goal_handle.is_cancel_requested: self.get_logger().info( - "Eye Tracker Action Server preempted, stopping tracking." + "Eye Tracker Action Server canceled, stopping tracking." ) self._look_centre() - goal_handle.canceled() - image_sub.unregister() - depth_sub.unregister() - depth_camera_info_sub.unregister() + for sub in self.subs: + self.destroy_subscription(sub.sub) self._done = True + goal_handle.canceled() + + self.get_logger().info("Canceled EYE TRACKER") + return EyeTrackerAction.Result() - self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.25)) + time.sleep(0.25) goal_handle.succeed() - image_sub.unregister() - depth_sub.unregister() - depth_camera_info_sub.unregister() return EyeTrackerAction.Result() @@ -396,7 +411,7 @@ def main(args=None): eye_tracker = EyeTracker() # This allows the action server to handle concurrent goals - executor = MultiThreadedExecutor(num_threads=4) + executor = MultiThreadedExecutor() executor.add_node(eye_tracker) try: diff --git a/common/vision/lasr_vision_interfaces/action/EyeTracker.action b/common/vision/lasr_vision_interfaces/action/EyeTracker.action index 1dc3cdeb4..885fdfdca 100644 --- a/common/vision/lasr_vision_interfaces/action/EyeTracker.action +++ b/common/vision/lasr_vision_interfaces/action/EyeTracker.action @@ -3,4 +3,6 @@ geometry_msgs/Point person_point --- # result bool person_lost ---- \ No newline at end of file +--- +# feedback +bool running \ No newline at end of file diff --git a/common/vision/lasr_vision_open_vocabulary/config/params.yaml b/common/vision/lasr_vision_open_vocabulary/config/params.yaml index 2279ea798..bd231c0cf 100644 --- a/common/vision/lasr_vision_open_vocabulary/config/params.yaml +++ b/common/vision/lasr_vision_open_vocabulary/config/params.yaml @@ -2,7 +2,7 @@ lasr_vision_open_vocabulary: ros__parameters: model: 'grounding_dino' model_device: 'cuda' - grounding_dino_weights: '/home/michele/Desktop/Projects/lasr/branch_new/Base/common/vision/lasr_vision_open_vocabulary/models/grounding-dino-base' + grounding_dino_weights: '' yoloworld_weights: 'yolov8s-world.pt' use_sam: false sam_encoder_path: '' diff --git a/common/vision/lasr_vision_open_vocabulary/lasr_vision_open_vocabulary/node.py b/common/vision/lasr_vision_open_vocabulary/lasr_vision_open_vocabulary/node.py index cd80c3865..ea0d3a9a0 100644 --- a/common/vision/lasr_vision_open_vocabulary/lasr_vision_open_vocabulary/node.py +++ b/common/vision/lasr_vision_open_vocabulary/lasr_vision_open_vocabulary/node.py @@ -14,7 +14,6 @@ VitSam, ) - class OpenVocabNode(Node): def __init__(self): super().__init__("lasr_vision_open_vocabulary") @@ -36,6 +35,24 @@ def __init__(self): encoder_path = self.get_parameter("sam_encoder_path").value decoder_path = self.get_parameter("sam_decoder_path").value + # CLIP bounding box. + self.declare_parameter("clip_rerank", True) + self.declare_parameter("clip_candidates", []) + self.declare_parameter("clip_model", "openai/clip-vit-base-patch32") + self.declare_parameter("clip_prompt_template", "a photo of a {}") + self._clip_rerank = bool(self.get_parameter("clip_rerank").value) + self._clip_candidates = [ + c for c in (self.get_parameter("clip_candidates").value or []) if c + ] + self._clip_model_name = self.get_parameter("clip_model").value + self._clip_template = self.get_parameter("clip_prompt_template").value + self._clip_device = device + self._clip = None # (model, processor, torch) - lazy + if self._clip_rerank and self._clip_candidates: + self.get_logger().info( + f"CLIP rerank ENABLED ({len(self._clip_candidates)} candidates)" + ) + self.bridge = CvBridge() self.detector = None self.vitsam = None @@ -96,7 +113,20 @@ def handle_detect(self, request, response): ) self.get_logger().info(f"Found {len(detections)} detections") + do_rerank = self._clip_rerank and bool(self._clip_candidates) for label, score, x1, y1, x2, y2 in detections: + if do_rerank: + new_label, clip_conf = self._rerank_label( + cv_image, x1, y1, x2, y2, label + ) + if new_label != label: + self.get_logger().info( + f"CLIP rerank: '{label}' -> '{new_label}'" + + (f" ({clip_conf:.2f})" if clip_conf is not None else "") + ) + label = new_label + if clip_conf is not None: + score = clip_conf cx, cy = (x1 + x2) / 2, (y1 + y2) / 2 w, h = x2 - x1, y2 - y1 det = Detection() @@ -107,6 +137,67 @@ def handle_detect(self, request, response): return response + # --- CLIP recognition rerank --- + def _load_clip(self): + """Lazily load CLIP (model, processor, torch). Disables rerank on failure.""" + if self._clip is not None: + return self._clip + try: + import torch + from transformers import CLIPModel, CLIPProcessor + + self.get_logger().info( + f"Loading CLIP ({self._clip_model_name}) on {self._clip_device}..." + ) + model = ( + CLIPModel.from_pretrained(self._clip_model_name) + .to(self._clip_device) + .eval() + ) + processor = CLIPProcessor.from_pretrained(self._clip_model_name) + self._clip = (model, processor, torch) + self.get_logger().info("CLIP loaded successfully") + except Exception as e: + self.get_logger().error(f"Failed loading CLIP - rerank disabled: {e}") + self._clip_rerank = False + self._clip = None + return self._clip + + def _rerank_label(self, cv_image, x1, y1, x2, y2, orig_label): + """Crop the box and pick the best-matching candidate via CLIP. + + Returns (label, confidence). Falls back to orig_label on any problem. + """ + clip = self._load_clip() + if clip is None: + return orig_label, None + model, processor, torch = clip + + h, w = cv_image.shape[:2] + x1 = max(0, min(int(x1), w - 1)) + x2 = max(0, min(int(x2), w)) + y1 = max(0, min(int(y1), h - 1)) + y2 = max(0, min(int(y2), h)) + if x2 - x1 < 2 or y2 - y1 < 2: + return orig_label, None + + try: + from PIL import Image as PILImage + + crop = cv2.cvtColor(cv_image[y1:y2, x1:x2], cv2.COLOR_BGR2RGB) + pil = PILImage.fromarray(crop) + prompts = [self._clip_template.format(c) for c in self._clip_candidates] + inputs = processor( + text=prompts, images=pil, return_tensors="pt", padding=True + ).to(self._clip_device) + with torch.no_grad(): + probs = model(**inputs).logits_per_image.softmax(dim=-1)[0] + best = int(probs.argmax()) + return self._clip_candidates[best], float(probs[best]) + except Exception as e: + self.get_logger().warn(f"CLIP rerank failed for a crop: {e}") + return orig_label, None + def handle_detect_and_segment(self, request, response): detect_resp = OpenVocabDetect.Response() detect_resp = self.handle_detect(request, detect_resp) diff --git a/common/vision/lasr_vision_reid/lasr_vision_reid/add_face.py b/common/vision/lasr_vision_reid/lasr_vision_reid/add_face.py index 4a3205acb..e10482b01 100644 --- a/common/vision/lasr_vision_reid/lasr_vision_reid/add_face.py +++ b/common/vision/lasr_vision_reid/lasr_vision_reid/add_face.py @@ -3,6 +3,9 @@ from sensor_msgs.msg import Image from lasr_vision_interfaces.srv import AddFace import sys +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + +qos = QoSProfile(depth=10, reliability=QoSReliabilityPolicy.BEST_EFFORT) def add_face(node: Node, name: str, num_images: int, image_topic: str): @@ -60,7 +63,8 @@ def service_response_callback(future): # Use call_async() instead of call() to avoid blocking the event loop add_face_srv.call_async(req).add_done_callback(service_response_callback) - image_sub = node.create_subscription(Image, image_topic, handle_image, 10) + # image_sub = node.create_subscription(Image, image_topic, handle_image, 10) + image_sub = node.create_subscription(Image, image_topic, handle_image, qos) # Spin until collection is complete, checking in a loop to allow graceful exit while not collection_complete and rclpy.ok(): @@ -78,13 +82,13 @@ def main(): image_topic = f"/{camera}/rgb/image_raw" # camera = node.declare_parameter("~camera", "head_front_camera").value - name = node.declare_parameter("~name", "fadi").value # originally jared + name = node.declare_parameter("~name", "guest1").value # originally jared num_images = node.declare_parameter("~num_images", 10).value - image_topic = "image_raw" + # image_topic = "/image_raw" node.get_logger().info(f"Image topic: {image_topic}") - node.declare_parameter("name", "fadi") + node.declare_parameter("name", "guest1") name = node.get_parameter("name").value node.declare_parameter("num_images", 10) diff --git a/common/vision/lasr_vision_reid/lasr_vision_reid/relay_2d.py b/common/vision/lasr_vision_reid/lasr_vision_reid/relay_2d.py deleted file mode 100644 index f733ba85b..000000000 --- a/common/vision/lasr_vision_reid/lasr_vision_reid/relay_2d.py +++ /dev/null @@ -1,66 +0,0 @@ -import rclpy -from rclpy.node import Node -import message_filters -import sys -from threading import Thread - -from sensor_msgs.msg import Image, CameraInfo -from lasr_vision_interfaces.srv import Recognise - - -def relay_2d(node: Node, image_topic: str) -> None: - - recognise = node.create_client(Recognise, "/lasr_vision_reid/recognise/twod") - while not recognise.wait_for_service(timeout_sec=1.0): - node.get_logger().info("Service not available, waiting again...") - node.get_logger().info("Service is ready!") - - def detect_cb(image: Image): - def response_callback(future): - try: - response = future.result() - node.get_logger().info(str(response)) - except Exception as e: - node.get_logger().error(f"Service call failed: {e}") - - request = Recognise.Request( - image_raw=image, - confidence=0.5, - ) - # Use async with threading - callback executes in spin thread where TF is updated - recognise.call_async(request).add_done_callback(response_callback) - - image_sub = node.create_subscription(Image, image_topic, detect_cb, 10) - - -def main(): - rclpy.init(args=sys.argv) - node = rclpy.create_node("lasr_vision_reid_relay_2d") - - image_topic = "image_raw" - node.get_logger().info(f"Image topic: {image_topic}") - - relay_2d( - node=node, - image_topic=image_topic, - ) - - # Run spin in a separate thread so service calls don't block the event loop - spin_thread = Thread(target=rclpy.spin, args=(node,), daemon=True) - spin_thread.start() - - # Keep the main thread alive - try: - while True: - spin_thread.join(timeout=1.0) - if not spin_thread.is_alive(): - break - except KeyboardInterrupt: - pass - - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/common/vision/lasr_vision_reid/lasr_vision_reid/relay_3d.py b/common/vision/lasr_vision_reid/lasr_vision_reid/relay_3d.py index fde20ca02..1b7ef3c02 100644 --- a/common/vision/lasr_vision_reid/lasr_vision_reid/relay_3d.py +++ b/common/vision/lasr_vision_reid/lasr_vision_reid/relay_3d.py @@ -12,7 +12,7 @@ def relay_3d( node: Node, image_topic: str, depth_topic: str, depth_camera_info_topic: str ) -> None: - recognise = node.create_client(Recognise3D, "/lasr_vision_reid/recognise/threed") + recognise = node.create_client(Recognise3D, "/lasr_vision_reid/recognise") while not recognise.wait_for_service(timeout_sec=1.0): node.get_logger().info("Service not available, waiting again...") node.get_logger().info("Service is ready!") @@ -63,7 +63,7 @@ def response_callback(future): node, Image, depth_topic, qos_profile=camera_qos ) ts = message_filters.ApproximateTimeSynchronizer( - [image_sub, depth_sub], queue_size=30, slop=0.2 + [image_sub, depth_sub], queue_size=30, slop=0.1 ) ts.registerCallback(detect_cb) diff --git a/common/vision/lasr_vision_reid/lasr_vision_reid/service.py b/common/vision/lasr_vision_reid/lasr_vision_reid/service.py index 58ee8ff8c..81a27eaca 100644 --- a/common/vision/lasr_vision_reid/lasr_vision_reid/service.py +++ b/common/vision/lasr_vision_reid/lasr_vision_reid/service.py @@ -9,9 +9,11 @@ """ from typing import Dict, Tuple, Optional, List +import os import rclpy from rclpy.node import Node +from rclpy.duration import Duration import numpy as np from sklearn.metrics.pairwise import cosine_similarity @@ -23,6 +25,8 @@ from lasr_vision_interfaces.srv import Recognise3D, AddFace, Recognise from lasr_vision_interfaces.msg import Detection3D, Detection +from tensorflow.compat.v1 import ConfigProto +from tensorflow.compat.v1 import InteractiveSession from geometry_msgs.msg import Point, PointStamped from sensor_msgs.msg import Image @@ -50,7 +54,7 @@ def __init__(self): self._db = {} self._bridge = CvBridge() - self._tf_buffer = tf.Buffer(cache_time=rclpy.duration.Duration(seconds=10)) + self._tf_buffer = tf.Buffer(cache_time=Duration(seconds=10)) self._tf_listener = tf.TransformListener(self._tf_buffer, self) self._image_publisher = self.create_publisher( @@ -60,12 +64,10 @@ def __init__(self): Marker, "/lasr_vision_reid/recognise/points", 10 ) - self._recognise_3d_service = self.create_service( - Recognise3D, "/lasr_vision_reid/recognise/threed", self._recognise_3d - ) - self._recognise_2d_service = self.create_service( - Recognise, "/lasr_vision_reid/recognise/twod", self._recognise_2d + self._recognise_service = self.create_service( + Recognise3D, "/lasr_vision_reid/recognise", self._recognise ) + self._add_face_service = self.create_service( AddFace, "/lasr_vision_reid/add_face", self._add_face ) @@ -75,81 +77,12 @@ def _extract_embeddings(self, im: np.ndarray) -> List[np.ndarray]: Use DeepFace to extract an embedding of a face. """ results = DeepFace.represent( - img_path=im, model_name="VGG-Face", enforce_detection=True + img_path=im, model_name="VGG-Face", enforce_detection=False ) embeddings = [np.array(entry["embedding"]) for entry in results] return embeddings - def _recognise_2d( - self, request: Recognise.Request, response: Recognise.Response - ) -> Recognise.Response: - response.detections = [] - - try: - cv_im = self._bridge.imgmsg_to_cv2( - request.image_raw, desired_encoding="rgb8" - ) - except Exception as e: - self.get_logger().error(f"Failed to convert image: {e}") - return response - - h, w, _ = cv_im.shape - - try: - # Get face embeddings and bounding boxes from DeepFace (face detection + embedding) - results = DeepFace.represent( - img_path=cv_im, - model_name="VGG-Face", - enforce_detection=True, - detector_backend="retinaface", - align=True, - max_faces=None, - ) - except Exception as e: - self.get_logger().warning(f"DeepFace representation failed: {e}") - return response - - if not results: - self.get_logger().info("No faces detected.", once=True) - return response - - for face_data in results: - embedding = np.array(face_data["embedding"]) - region = face_data["facial_area"] - x1, y1 = region["x"], region["y"] - x2, y2 = x1 + region["w"], y1 + region["h"] - - # Clamp bounding box within image - x1, y1 = max(0, x1), max(0, y1) - x2, y2 = min(w, x2), min(h, y2) - - # Compare embedding with database entries - best_label = "unknown" - best_score = -1.0 - for label, embeddings in self._db.items(): - sims = [ - cosine_similarity([embedding], [db_emb])[0][0] - for db_emb in embeddings - ] - avg_sim = np.mean(sims) - if avg_sim > best_score: - best_score = avg_sim - best_label = label - - if best_score < request.confidence: - continue - - detection = Detection() - detection.name = best_label - detection.confidence = best_score - detection.xywh = [x1, y1, x2 - x1, y2 - y1] - - response.detections.append(detection) - - self._publish_results_2d(response, cv_im, request.image_raw.header.frame_id) - return response - - def _recognise_3d( + def _recognise( self, request: Recognise3D.Request, response: Recognise3D.Response ) -> Recognise3D.Response: response.detections = [] @@ -171,7 +104,6 @@ def _recognise_3d( fx, fy = K[0], K[4] cx, cy = K[2], K[5] - transform = None try: transform = self._tf_buffer.lookup_transform( target_frame, @@ -184,16 +116,17 @@ def _recognise_3d( tf.ConnectivityException, tf.ExtrapolationException, ) as e: - self.get_logger().debug( - f"Transform lookup failed: {type(e).__name__}. Returning detections in camera frame." + self.get_logger().error( + f"Failed to find transform between {request.depth_image.header.frame_id}, and the target frame {target_frame}" ) + return response try: # Get face embeddings and bounding boxes from DeepFace (face detection + embedding) results = DeepFace.represent( img_path=cv_im, model_name="VGG-Face", - enforce_detection=True, + enforce_detection=False, detector_backend="retinaface", align=True, max_faces=None, @@ -252,29 +185,24 @@ def _recognise_3d( x, y, z = np.median(points, axis=0) point = Point() - point.x = x - point.y = y - point.z = z + point.x = x / 1000 + point.y = y / 1000 + point.z = z / 1000 point_stamped = PointStamped() point_stamped.header = request.depth_image.header point_stamped.point = point - # Transform if available, otherwise use camera frame - if transform is not None: - try: - point_transformed = do_transform_point(point_stamped, transform) - detection.point = point_transformed.point - except Exception as e: - self.get_logger().debug( - f"Point transformation failed: {e}. Using camera frame." - ) - detection.point = point - else: - detection.point = point + try: + point_transformed = do_transform_point(point_stamped, transform) + detection.point = point_transformed.point + except Exception as e: + self.get_logger().warning + (f"Point transformation failed: {e}.") + continue response.detections.append(detection) - self._publish_results_3d(response, cv_im, target_frame) + self._publish_results(response, cv_im, target_frame) return response def _add_face( @@ -296,7 +224,7 @@ def _add_face( results = DeepFace.represent( img_path=cv_im, model_name="VGG-Face", - enforce_detection=True, # allow detection attempts even if uncertain + enforce_detection=False, # allow detection attempts even if uncertain detector_backend="retinaface", align=True, max_faces=1, @@ -328,29 +256,7 @@ def _add_face( return response - def _publish_results_2d( - self, response: Recognise.Response, cv_im: Mat, frame_id: str - ) -> None: - annotated = cv_im.copy() - for detection in response.detections: - x, y, w, h = detection.xywh - cv2.rectangle(annotated, (x, y), (x + w, y + h), (0, 255, 0), 2) - label = f"{detection.name} ({detection.confidence:.2f})" - cv2.putText( - annotated, - label, - (x, y - 5), - cv2.FONT_HERSHEY_SIMPLEX, - 0.5, - (0, 255, 0), - 2, - ) - - self._image_publisher.publish( - self._bridge.cv2_to_imgmsg(annotated, encoding="rgb8") - ) - - def _publish_results_3d( + def _publish_results( self, response: Recognise3D.Response, cv_im: Mat, frame_id: str ) -> None: @@ -374,29 +280,34 @@ def _publish_results_3d( ) for i, detection in enumerate(response.detections): - marker = Marker() - marker.header.frame_id = frame_id - marker.header.stamp = ( - self.get_clock().now().to_msg() - ) # Convert to message type - marker.id = i - marker.type = Marker.SPHERE - marker.action = Marker.ADD - marker.pose.position = detection.point - marker.scale.x = 0.1 - marker.scale.y = 0.1 - marker.scale.z = 0.1 - marker.color.r = 0.0 - marker.color.g = 0.5 - marker.color.b = 1.0 - marker.color.a = 1.0 - - self._marker_publisher.publish(marker) + if detection.name == "guest1" or detection.name == "guest2": + marker = Marker() + marker.header.frame_id = frame_id + marker.header.stamp = ( + self.get_clock().now().to_msg() + ) # Convert to message type + marker.id = i + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose.position = detection.point + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + marker.color.r = 0.0 + marker.color.g = 0.5 + marker.color.b = 1.0 + marker.color.a = 1.0 + + self._marker_publisher.publish(marker) def main(): rclpy.init() + config = ConfigProto() + config.gpu_options.per_process_gpu_memory_fraction = 0.20 + session = InteractiveSession(config=config) + reid = ReID() reid.get_logger().info("Vision reid service is ready!", once=True) diff --git a/common/vision/lasr_vision_reid/requirements.in b/common/vision/lasr_vision_reid/requirements.in index 3c59665ce..538dd66be 100644 --- a/common/vision/lasr_vision_reid/requirements.in +++ b/common/vision/lasr_vision_reid/requirements.in @@ -1,4 +1,4 @@ deepface -tensorflow +tensorflow[and-cuda] numpy scikit-learn \ No newline at end of file diff --git a/common/vision/lasr_vision_reid/requirements.txt b/common/vision/lasr_vision_reid/requirements.txt index 3b83007f5..c221db9cb 100644 --- a/common/vision/lasr_vision_reid/requirements.txt +++ b/common/vision/lasr_vision_reid/requirements.txt @@ -1,72 +1,27 @@ -absl-py==2.3.0 # via tensorboard, tensorflow -astunparse==1.6.3 # via tensorflow -beautifulsoup4==4.13.4 # via gdown -blinker==1.8.2 # via flask -cachetools==5.5.2 # via google-auth -certifi==2025.6.15 # via requests -charset-normalizer==3.4.2 # via requests -click==8.1.8 # via flask -deepface==0.0.93 # via -r requirements.in -filelock==3.16.1 # via gdown -fire==0.7.0 # via deepface -flask==3.0.3 # via deepface, flask-cors -flask-cors==5.0.0 # via deepface -flatbuffers==25.2.10 # via tensorflow -gast==0.4.0 # via tensorflow -gdown==5.2.0 # via deepface, retina-face -google-auth==2.40.3 # via google-auth-oauthlib, tensorboard -google-auth-oauthlib==1.0.0 # via tensorboard -google-pasta==0.2.0 # via tensorflow -grpcio==1.70.0 # via tensorboard, tensorflow -gunicorn==23.0.0 # via deepface -h5py==3.11.0 # via tensorflow -idna==3.10 # via requests -importlib-metadata==8.5.0 # via flask, markdown -itsdangerous==2.2.0 # via flask -jinja2==3.1.6 # via flask -joblib==1.4.2 # via scikit-learn -keras==2.13.1 # via deepface, mtcnn, tensorflow -libclang==18.1.1 # via tensorflow -markdown==3.7 # via tensorboard -markupsafe==2.1.5 # via jinja2, werkzeug -mtcnn==0.1.1 # via deepface -numpy==1.24.3 # via -r requirements.in, deepface, h5py, opencv-python, pandas, retina-face, scikit-learn, scipy, tensorboard, tensorflow -oauthlib==3.3.0 # via requests-oauthlib -opencv-python==4.11.0.86 # via deepface, mtcnn, retina-face -opt-einsum==3.4.0 # via tensorflow -packaging==25.0 # via gunicorn, tensorflow -pandas==2.0.3 # via deepface -pillow==10.4.0 # via deepface, retina-face -protobuf==4.25.8 # via tensorboard, tensorflow -pyasn1==0.6.1 # via pyasn1-modules, rsa -pyasn1-modules==0.4.2 # via google-auth -pysocks==1.7.1 # via requests -python-dateutil==2.9.0.post0 # via pandas -pytz==2025.2 # via pandas -requests[socks]==2.32.4 # via deepface, gdown, requests-oauthlib, tensorboard -requests-oauthlib==2.0.0 # via google-auth-oauthlib -retina-face==0.0.17 # via deepface -rsa==4.9.1 # via google-auth -scikit-learn==1.3.2 # via -r requirements.in -scipy==1.10.1 # via scikit-learn -six==1.17.0 # via astunparse, google-pasta, python-dateutil, tensorflow -soupsieve==2.7 # via beautifulsoup4 -tensorboard==2.13.0 # via tensorflow -tensorboard-data-server==0.7.2 # via tensorboard -tensorflow==2.13.1 # via -r requirements.in, deepface, retina-face -tensorflow-estimator==2.13.0 # via tensorflow -tensorflow-io-gcs-filesystem==0.34.0 # via tensorflow -termcolor==2.4.0 # via fire, tensorflow -threadpoolctl==3.5.0 # via scikit-learn -tqdm==4.67.1 # via deepface, gdown -tzdata==2025.2 # via pandas -urllib3==2.2.3 # via requests -werkzeug==3.0.6 # via flask, tensorboard -wheel==0.45.1 # via astunparse, tensorboard -wrapt==1.17.2 # via tensorflow -zipp==3.20.2 # via importlib-metadata -GitPython>=2.1.5 -psutil>=5.2.2 -# The following packages are considered to be unsafe in a requirements file: -# setuptools -# typing-extensions==4.5.0 via beautifulsoup4, tensorflow \ No newline at end of file +# requirements.txt +# For lasr_vision_reid DeepFace service with TensorFlow GPU support +# Tested-style target: Linux / Apptainer --nv / Python 3.10-3.12 + +# Core face recognition +deepface==0.0.100 +retina-face==0.0.18 + +# TensorFlow GPU stack +# Let TensorFlow choose the correct nvidia-* dependency versions. +tensorflow[and-cuda]==2.16.2 +tf-keras==2.16.0 + +# Numeric / ML utilities +numpy<2.0 +scikit-learn>=1.4,<1.8 +scipy>=1.11,<1.16 +pandas>=2.0,<2.4 + +# Image handling +opencv-python>=4.8,<5 +pillow>=10,<13 + +# DeepFace download / utility deps +gdown>=5,<6.2 +tqdm>=4.66,<5 +requests>=2.31,<3 \ No newline at end of file diff --git a/common/vision/lasr_vision_reid/setup.py b/common/vision/lasr_vision_reid/setup.py index 94b5421f9..0eb0bf7bb 100644 --- a/common/vision/lasr_vision_reid/setup.py +++ b/common/vision/lasr_vision_reid/setup.py @@ -47,7 +47,6 @@ def run(self): "console_scripts": [ "service = lasr_vision_reid.service:main", "relay_3d = lasr_vision_reid.relay_3d:main", - "relay_2d = lasr_vision_reid.relay_2d:main", "add_face = lasr_vision_reid.add_face:main", ], }, diff --git a/common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py b/common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py index b1305d4c3..3e94407c1 100644 --- a/common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py +++ b/common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py @@ -2,7 +2,7 @@ import os from ament_index_python import packages - +from ament_index_python.packages import get_package_share_directory from typing import Dict, Union, List, Tuple import rclpy @@ -96,12 +96,14 @@ def __init__(self, node: Node): self.node = node self._cache = {} self.node.declare_parameter( - "~device", "cuda:0" if torch.cuda.is_available() else "cpu" + "device", "cuda:0" if torch.cuda.is_available() else "cpu" ) - self._device = self.node.get_parameter("~device").value + self._device = self.node.get_parameter("device").value - self.node.declare_parameter("~preload", ["yolo11n-seg.pt"]) - self.preload_param_list = self.node.get_parameter("~preload").value + self.node.declare_parameter( + "preload", ["/home/yara/ros2_ws/src/Base/common/vision/lasr_vision_yolo/models/best.pt"] + ) + self.preload_param_list = self.node.get_parameter("preload").value for model in self.preload_param_list: self._maybe_load_model(model) @@ -115,7 +117,7 @@ def __init__(self, node: Node): history=HistoryPolicy.KEEP_LAST, ) - self._tf_buffer = Buffer(cache_time=Duration(seconds=10)) + self._tf_buffer = Buffer(cache_time=Duration(seconds=10.0)) self._tf_listener = tf.TransformListener(self._tf_buffer, self.node) self.node.create_service(YoloDetection, "/yolo/detect", self._detect) @@ -201,19 +203,11 @@ def _lookup_transform(self, target_frame, source_frame, stamp): return self._tf_buffer.lookup_transform( target_frame, source_frame, stamp, Duration(seconds=1.0) ) - except Exception as e: - self.node.get_logger().debug( - f"TF {target_frame}<-{source_frame} at image stamp failed ({e}); using latest" - ) - try: - return self._tf_buffer.lookup_transform( - target_frame, source_frame, Time(), Duration(seconds=1.0) - ) except Exception as e: self.node.get_logger().error( - f"TF {target_frame}<-{source_frame} lookup failed: {e}" + f"TF {target_frame}<-{source_frame} at image stamp failed ({e})" ) - raise + return None def _detect3d( self, req: YoloDetection3D.Request, res: YoloDetection3D.Response @@ -241,6 +235,8 @@ def _detect3d( req.depth_image.header.frame_id, req.depth_image.header.stamp, ) + if transform is None: + return response for result in results: detection = Detection3D() @@ -333,6 +329,8 @@ def _detect_keypoints3d( req.depth_image.header.frame_id, req.depth_image.header.stamp, ) + if transform is None: + return response for result in results: keypoints = Keypoint3DList() @@ -373,13 +371,30 @@ def _detect_keypoints3d( return response - def _maybe_load_model(self, model_name: str) -> ultralytics.YOLO: + # def _maybe_load_model(self, model_name: str) -> ultralytics.YOLO: + # if model_name in self._cache: + # return self._cache[model_name] + + # model = self._cache[model_name] = ultralytics.YOLO(model_name).to(self._device) + + # self.node.get_logger().info(f"Loaded {model_name} model on {self._device}") + # return model + + def _maybe_load_model(self, model_name: str): if model_name in self._cache: return self._cache[model_name] - model = self._cache[model_name] = ultralytics.YOLO(model_name).to(self._device) + # If already an absolute path, keep it + if os.path.isabs(model_name): + model_path = model_name + else: + package_share = get_package_share_directory("lasr_vision_yolo") + model_path = os.path.join(package_share, "models", model_name) + + self.node.get_logger().info(f"Loading model from: {model_path}") - self.node.get_logger().info(f"Loaded {model_name} model on {self._device}") + model = ultralytics.YOLO(model_path).to(self._device) + self._cache[model_name] = model return model def _publish_results( @@ -520,17 +535,26 @@ def _yolo( return results +from rclpy.executors import MultiThreadedExecutor + + def main(args=None): rclpy.init(args=args) node = Node("yolo_service") YOLOServiceNode(node) + + executor = MultiThreadedExecutor() + executor.add_node(node) + try: - rclpy.spin(node) + executor.spin() except KeyboardInterrupt: pass finally: + executor.shutdown() node.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/common/vision/lasr_vision_yolo/launch/service_launch.xml b/common/vision/lasr_vision_yolo/launch/service_launch.xml index 9d72fe5f4..80f30f32c 100644 --- a/common/vision/lasr_vision_yolo/launch/service_launch.xml +++ b/common/vision/lasr_vision_yolo/launch/service_launch.xml @@ -1,5 +1,5 @@ - + diff --git a/common/vision/lasr_vision_yolo/setup.py b/common/vision/lasr_vision_yolo/setup.py index 4f9c5b2c2..fb88715fa 100755 --- a/common/vision/lasr_vision_yolo/setup.py +++ b/common/vision/lasr_vision_yolo/setup.py @@ -28,6 +28,7 @@ def run(self): ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml", "requirements.txt"]), (os.path.join("share", package_name, "launch"), glob("launch/*")), + (os.path.join("share", package_name, "models"), glob("models/*.pt")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/log.txt b/log.txt new file mode 100644 index 000000000..d7897d4a1 --- /dev/null +++ b/log.txt @@ -0,0 +1,2662 @@ +[INFO] [launch]: All log files can be found below /home/rexy/.ros/log/2026-06-24-16-18-29-014477-beedrill-72272 +[INFO] [launch]: Default logging verbosity is set to INFO +[INFO] [ros2-1]: process started with pid [72276] +[INFO] [yolo_service_node-2]: process started with pid [72278] +[INFO] [service-3]: process started with pid [72280] +[INFO] [vlm_service-4]: process started with pid [72282] +[INFO] [eye_tracker_action_server-5]: process started with pid [72284] +[INFO] [transcribe_microphone_server-6]: process started with pid [72286] +[INFO] [hri_task_service-7]: process started with pid [72288] +[INFO] [sm-8]: process started with pid [72290] +[sm-8] [WARN] [1782314310.160663139] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[sm-8] [INFO] [1782314310.218874906] [hri]: [ros_clients_cache.py:get_or_create_action_client:99] Creating new action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.228116313] [hri]: [ros_clients_cache.py:get_or_create_action_client:99] Creating new action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [WARN] [1782314310.232799494] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[vlm_service-4] [INFO] [1782314310.335737519] [lasr_vlm_service]: VLM Describe People service started +[sm-8] [INFO] [1782314310.380999502] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314310.382122523] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [WARN] [1782314310.382898050] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[transcribe_microphone_server-6] /usr/lib/python3/dist-packages/scipy/__init__.py:146: UserWarning: A NumPy version >=1.17.3 and <1.25.0 is required for this version of SciPy (detected version 1.26.4 +[transcribe_microphone_server-6] warnings.warn(f"A NumPy version >={np_minversion} and <{np_maxversion}" +[service-3] 2026-06-24 16:18:30.551411: I tensorflow/core/util/port.cc:113] oneDNN custom operations are on. You may see slightly different numerical results due to floating-point round-off errors from different computation orders. To turn them off, set the environment variable `TF_ENABLE_ONEDNN_OPTS=0`. +[service-3] 2026-06-24 16:18:30.564422: E external/local_xla/xla/stream_executor/cuda/cuda_fft.cc:479] Unable to register cuFFT factory: Attempting to register factory for plugin cuFFT when one has already been registered +[service-3] 2026-06-24 16:18:30.583692: E external/local_xla/xla/stream_executor/cuda/cuda_dnn.cc:10575] Unable to register cuDNN factory: Attempting to register factory for plugin cuDNN when one has already been registered +[service-3] 2026-06-24 16:18:30.583723: E external/local_xla/xla/stream_executor/cuda/cuda_blas.cc:1442] Unable to register cuBLAS factory: Attempting to register factory for plugin cuBLAS when one has already been registered +[sm-8] [INFO] [1782314310.591038799] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[service-3] 2026-06-24 16:18:30.594600: I tensorflow/core/platform/cpu_feature_guard.cc:210] This TensorFlow binary is optimized to use available CPU instructions in performance-critical operations. +[service-3] To enable the following instructions: AVX2 AVX_VNNI FMA, in other operations, rebuild TensorFlow with the appropriate compiler flags. +[sm-8] [INFO] [1782314310.597337737] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/vlm/describe_people' of type 'lasr_vlm_interfaces.srv._vlm_describe_people.VlmDescribePeople' +[sm-8] [INFO] [1782314310.602453923] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314310.622014722] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/yolo/detect_pose' of type 'lasr_vision_interfaces.srv._yolo_pose_detection.YoloPoseDetection' +[sm-8] [INFO] [1782314310.634489757] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/lasr_vision_reid/add_face' of type 'lasr_vision_interfaces.srv._add_face.AddFace' +[sm-8] [INFO] [1782314310.639097807] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/hri_task/query_llm' of type 'lasr_llm_interfaces.srv._hri_task_query_llm.HRITaskQueryLlm' +[sm-8] [INFO] [1782314310.650803909] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/hri_task/query_llm' of type 'lasr_llm_interfaces.srv._hri_task_query_llm.HRITaskQueryLlm' +[sm-8] [INFO] [1782314310.654636922] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.655887713] [hri]: [ros_clients_cache.py:get_or_create_action_client:99] Creating new action client for 'transcribe_speech' of type 'lasr_speech_recognition_interfaces.action._transcribe_speech.TranscribeSpeech' +[sm-8] [INFO] [1782314310.685550493] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.686668641] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.687793390] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/clear_octomap' of type 'std_srvs.srv._empty.Empty' +[sm-8] [INFO] [1782314310.695173013] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314310.696080478] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.697022153] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314310.697766200] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314310.698459656] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.699216777] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314310.699914362] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314310.700624244] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.701409183] [hri]: [ros_clients_cache.py:get_or_create_action_client:99] Creating new action client for '/lasr_vision_eye_tracker/track_eyes' of type 'lasr_vision_interfaces.action._eye_tracker.EyeTracker' +[sm-8] [INFO] [1782314310.727718056] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314310.729238613] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314310.749830594] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [WARN] [1782314310.752976529] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[transcribe_microphone_server-6] [WARN] [1782314311.006406971] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[eye_tracker_action_server-5] [INFO] [1782314311.037425362] [eye_tracker_action_server]: Waiting for point head action server, and head action client and yolo to all be ready... +[transcribe_microphone_server-6] [INFO] [1782314311.045665477] [whisper_mic_server]: Loading model small.en +[ros2-1] Set parameter motions.reach_arm_vertical_gripper.joints successful +[ros2-1] Set parameter motions.reach_arm_vertical_gripper.positions successful +[ros2-1] Set parameter motions.reach_arm_vertical_gripper.times_from_start successful +[ros2-1] Set parameter motions.reach_arm_horizontal_gripper.joints successful +[ros2-1] Set parameter motions.reach_arm_horizontal_gripper.positions successful +[ros2-1] Set parameter motions.reach_arm_horizontal_gripper.times_from_start successful +[ros2-1] Set parameter motions.cml_arm_away.joints successful +[ros2-1] Set parameter motions.cml_arm_away.positions successful +[ros2-1] Set parameter motions.cml_arm_away.times_from_start successful +[ros2-1] Set parameter motions.open_gripper.joints successful +[ros2-1] Set parameter motions.open_gripper.positions successful +[ros2-1] Set parameter motions.open_gripper.times_from_start successful +[ros2-1] Set parameter motions.pre_navigation.joints successful +[ros2-1] Set parameter motions.pre_navigation.positions successful +[ros2-1] Set parameter motions.pre_navigation.times_from_start successful +[ros2-1] Set parameter motions.post_navigation.joints successful +[ros2-1] Set parameter motions.post_navigation.positions successful +[ros2-1] Set parameter motions.post_navigation.times_from_start successful +[ros2-1] Set parameter motions.look_left.joints successful +[ros2-1] Set parameter motions.look_left.positions successful +[ros2-1] Set parameter motions.look_left.times_from_start successful +[ros2-1] Set parameter motions.look_down_left.joints successful +[ros2-1] Set parameter motions.look_down_left.positions successful +[ros2-1] Set parameter motions.look_down_left.times_from_start successful +[ros2-1] Set parameter motions.look_right.joints successful +[ros2-1] Set parameter motions.look_right.positions successful +[ros2-1] Set parameter motions.look_right.times_from_start successful +[ros2-1] Set parameter motions.look_down_right.joints successful +[ros2-1] Set parameter motions.look_down_right.positions successful +[ros2-1] Set parameter motions.look_down_right.times_from_start successful +[ros2-1] Set parameter motions.look_centre.joints successful +[ros2-1] Set parameter motions.look_centre.positions successful +[ros2-1] Set parameter motions.look_centre.times_from_start successful +[ros2-1] Set parameter motions.look_down_centre.joints successful +[ros2-1] Set parameter motions.look_down_centre.positions successful +[ros2-1] Set parameter motions.look_down_centre.times_from_start successful +[ros2-1] Set parameter motions.raise_torso.joints successful +[ros2-1] Set parameter motions.raise_torso.positions successful +[ros2-1] Set parameter motions.raise_torso.times_from_start successful +[ros2-1] Set parameter motions.pointing_to_the_right.joints successful +[ros2-1] Set parameter motions.pointing_to_the_right.positions successful +[ros2-1] Set parameter motions.pointing_to_the_right.times_from_start successful +[ros2-1] Set parameter motions.pointing_to_the_left.joints successful +[ros2-1] Set parameter motions.pointing_to_the_left.positions successful +[ros2-1] Set parameter motions.pointing_to_the_left.times_from_start successful +[ros2-1] Set parameter motions.raising_right_arm.joints successful +[ros2-1] Set parameter motions.raising_right_arm.positions successful +[ros2-1] Set parameter motions.raising_right_arm.times_from_start successful +[ros2-1] Set parameter motions.raising_left_arm.joints successful +[ros2-1] Set parameter motions.raising_left_arm.positions successful +[ros2-1] Set parameter motions.raising_left_arm.times_from_start successful +[ros2-1] Set parameter motions.u1l.joints successful +[ros2-1] Set parameter motions.u1l.positions successful +[ros2-1] Set parameter motions.u1l.times_from_start successful +[ros2-1] Set parameter motions.u1m.joints successful +[ros2-1] Set parameter motions.u1m.positions successful +[ros2-1] Set parameter motions.u1m.times_from_start successful +[ros2-1] Set parameter motions.u1r.joints successful +[ros2-1] Set parameter motions.u1r.positions successful +[ros2-1] Set parameter motions.u1r.times_from_start successful +[ros2-1] Set parameter motions.ml.joints successful +[ros2-1] Set parameter motions.ml.positions successful +[ros2-1] Set parameter motions.ml.times_from_start successful +[ros2-1] Set parameter motions.mm.joints successful +[ros2-1] Set parameter motions.mm.positions successful +[ros2-1] Set parameter motions.mm.times_from_start successful +[ros2-1] Set parameter motions.mr.joints successful +[ros2-1] Set parameter motions.mr.positions successful +[ros2-1] Set parameter motions.mr.times_from_start successful +[sm-8] [INFO] [1782314311.172044569] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314311.173825042] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314311.174665055] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314311.180708030] [hri]: [ros_clients_cache.py:get_or_create_publisher:198] Creating new publisher for topic '/detect_all_in_polygon/debug' of type 'sensor_msgs.msg._image.Image' +[sm-8] [INFO] [1782314311.222825242] [hri]: [ros_clients_cache.py:get_or_create_action_client:99] Creating new action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[INFO] [ros2-1]: process has finished cleanly [pid 72276] +[INFO] [ros2-9]: process started with pid [72450] +[sm-8] [INFO] [1782314311.284998467] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[hri_task_service-7] [INFO] [1782314311.314681979] [llm]: HRI Task Query LLM service started +[sm-8] [INFO] [1782314311.339011377] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314311.339823176] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314311.340552566] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314311.366404057] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect_pose' of type 'lasr_vision_interfaces.srv._yolo_pose_detection.YoloPoseDetection' +[sm-8] [INFO] [1782314311.391790383] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/lasr_vision_reid/add_face' of type 'lasr_vision_interfaces.srv._add_face.AddFace' +[sm-8] [INFO] [1782314311.392646672] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314311.393309714] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314311.393986282] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[service-3] 2026-06-24 16:18:31.394186: W tensorflow/compiler/tf2tensorrt/utils/py_utils.cc:38] TF-TRT Warning: Could not find TensorRT +[sm-8] [INFO] [1782314311.394742236] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [WARN] [1782314311.399415372] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[sm-8] [INFO] [1782314311.942748260] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.024562894] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/vlm/describe_people' of type 'lasr_vlm_interfaces.srv._vlm_describe_people.VlmDescribePeople' +[sm-8] [INFO] [1782314312.025619982] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[eye_tracker_action_server-5] [INFO] [1782314312.040487980] [eye_tracker_action_server]: Waiting for point head action server, and head action client and yolo to all be ready... +[sm-8] [INFO] [1782314312.062096536] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect_pose' of type 'lasr_vision_interfaces.srv._yolo_pose_detection.YoloPoseDetection' +[sm-8] [INFO] [1782314312.090694984] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/lasr_vision_reid/add_face' of type 'lasr_vision_interfaces.srv._add_face.AddFace' +[sm-8] [INFO] [1782314312.091695696] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/hri_task/query_llm' of type 'lasr_llm_interfaces.srv._hri_task_query_llm.HRITaskQueryLlm' +[sm-8] [INFO] [1782314312.092372555] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/hri_task/query_llm' of type 'lasr_llm_interfaces.srv._hri_task_query_llm.HRITaskQueryLlm' +[sm-8] [INFO] [1782314312.093272245] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.093961110] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for 'transcribe_speech' of type 'lasr_speech_recognition_interfaces.action._transcribe_speech.TranscribeSpeech' +[sm-8] [INFO] [1782314312.094574531] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.095274021] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.096030573] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/clear_octomap' of type 'std_srvs.srv._empty.Empty' +[sm-8] [INFO] [1782314312.096659198] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.097259556] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.097871420] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.098492880] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.100731271] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.101505119] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.107790243] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.108963692] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.110537819] [hri]: [ros_clients_cache.py:get_or_create_action_client:99] Creating new action client for '/lasr_vision_eye_tracker/track_eyes' of type 'lasr_vision_interfaces.action._eye_tracker.EyeTracker' +[sm-8] [INFO] [1782314312.191666252] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314312.192989823] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[ros2-9] Transitioning successful +[sm-8] [INFO] [1782314312.236231020] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [WARN] [1782314312.242368204] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[service-3] 2026-06-24 16:18:32.358630: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:18:32.393606: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:18:32.394870: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[INFO] [ros2-9]: process has finished cleanly [pid 72450] +[INFO] [ros2-10]: process started with pid [72506] +[service-3] 2026-06-24 16:18:32.524495: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:18:32.525555: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:18:32.526534: W tensorflow/core/common_runtime/gpu/gpu_bfc_allocator.cc:47] Overriding orig_value setting because the TF_FORCE_GPU_ALLOW_GROWTH environment variable is set. Original config value was 0. +[service-3] 2026-06-24 16:18:32.526631: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:18:32.527618: I tensorflow/core/common_runtime/gpu/gpu_device.cc:1928] Created device /job:localhost/replica:0/task:0/device:GPU:0 with 1568 MB memory: -> device: 0, name: NVIDIA RTX A2000 8GB Laptop GPU, pci bus id: 0000:01:00.0, compute capability: 8.6 +[service-3] [INFO] [1782314312.814977060] [lasr_vision_reid]: Vision reid service is ready! +[sm-8] [INFO] [1782314312.906737121] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314312.941180488] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314313.015887604] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[eye_tracker_action_server-5] [INFO] [1782314313.043288938] [eye_tracker_action_server]: Waiting for point head action server, and head action client and yolo to all be ready... +[sm-8] [INFO] [1782314313.062121373] [hri]: [ros_clients_cache.py:get_or_create_publisher:192] Reusing existing publisher for topic '/detect_all_in_polygon/debug' of type 'sensor_msgs.msg._image.Image' +[sm-8] [INFO] [1782314313.128720067] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314313.144572628] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314313.250464083] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314313.255578793] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314313.256621231] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314313.322502321] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect_pose' of type 'lasr_vision_interfaces.srv._yolo_pose_detection.YoloPoseDetection' +[sm-8] [INFO] [1782314313.356861257] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/lasr_vision_reid/add_face' of type 'lasr_vision_interfaces.srv._add_face.AddFace' +[sm-8] [INFO] [1782314313.357729761] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314313.358307921] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314313.375076326] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314313.388312421] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314313.389320347] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/lasr_vision_reid/recognise' of type 'lasr_vision_interfaces.srv._recognise3_d.Recognise3D' +[ros2-10] Transitioning successful +[sm-8] [INFO] [1782314313.556018462] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314313.557046557] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314313.557795747] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314313.558473671] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314313.560433558] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314313.561149495] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [WARN] [1782314313.592471987] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[INFO] [ros2-10]: process has finished cleanly [pid 72506] +[INFO] [ros2-11]: process started with pid [72552] +[eye_tracker_action_server-5] [INFO] [1782314314.047610579] [eye_tracker_action_server]: Waiting for point head action server, and head action client and yolo to all be ready... +[yolo_service_node-2] [INFO] [1782314314.558525882] [yolo_service]: Loaded yolo11n-seg.pt model on cuda:0 +[yolo_service_node-2] [INFO] [1782314314.617874566] [yolo_service]: Loaded yolo11n.pt model on cuda:0 +[transcribe_microphone_server-6] [INFO] [1782314314.681806808] [whisper_mic_server]: Sucessfully loaded model small.en on cuda +[yolo_service_node-2] [INFO] [1782314314.742882737] [yolo_service]: Loaded yolo11n-pose.pt model on cuda:0 +[yolo_service_node-2] [INFO] [1782314314.753604394] [yolo_service]: YOLO service started +[eye_tracker_action_server-5] [INFO] [1782314314.805565342] [eye_tracker_action_server]: Eye Tracker Action Server started. +[sm-8] [INFO] [1782314314.819321024] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314314.893654162] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314315.030248228] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314315.100914683] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[ros2-11] Transitioning successful +[sm-8] [INFO] [1782314315.187404010] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314315.275643653] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314315.373391167] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[INFO] [ros2-11]: process has finished cleanly [pid 72552] +[INFO] [ros2-12]: process started with pid [72588] +[sm-8] [INFO] [1782314315.409774808] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314315.421023258] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [INFO] [1782314315.540583788] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314315.577173931] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[transcribe_microphone_server-6] [INFO] [1782314315.674529311] [whisper_mic_server]: Speech Action server transcribe_speech started +[sm-8] [INFO] [1782314315.692592546] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314315.720059735] [hri]: [ros_clients_cache.py:get_or_create_service_client:142] Reusing existing service client for '/yolo/detect3d' of type 'lasr_vision_interfaces.srv._yolo_detection3_d.YoloDetection3D' +[sm-8] [WARN] [1782314315.789419706] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[ros2-12] Transitioning successful +[INFO] [ros2-12]: process has finished cleanly [pid 72588] +[sm-8] [INFO] [1782314316.732984410] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314316.791569918] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314316.835411656] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314316.872516687] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for 'transcribe_speech' of type 'lasr_speech_recognition_interfaces.action._transcribe_speech.TranscribeSpeech' +[sm-8] [INFO] [1782314316.909737179] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314316.960697092] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314316.993323546] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314317.045342434] [hri]: [ros_clients_cache.py:get_or_create_service_client:148] Creating new service client for '/yolo/detect3d_pose' of type 'lasr_vision_interfaces.srv._yolo_pose_detection3_d.YoloPoseDetection3D' +[sm-8] [INFO] [1782314317.135384655] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314317.167051102] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314317.177680167] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314317.206832428] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314317.238166084] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [WARN] [1782314317.263141822] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. +[sm-8] [INFO] [1782314318.300193446] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/head_controller/point_head_action' of type 'control_msgs.action._point_head.PointHead' +[sm-8] [INFO] [1782314318.321151616] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314318.375995993] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314318.426407384] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314318.473129054] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314318.497269860] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/play_motion2' of type 'play_motion2_msgs.action._play_motion2.PlayMotion2' +[sm-8] [INFO] [1782314318.519031971] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314318.530551311] [hri]: [ros_clients_cache.py:get_or_create_action_client:93] Reusing existing action client for '/tts_engine/tts' of type 'tts_msgs.action._tts.TTS' +[sm-8] [INFO] [1782314318.600644328] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'WAIT_START' +[sm-8] [INFO] [1782314341.177405082] [hri]: [monitor_state.py:execute:166] Processing msg from topic '/hri/start' +[sm-8] [INFO] [1782314341.178012858] [hri]: [state_machine.py:wait_cb:31] RECEIVED START SIGNAL +[sm-8] [INFO] [1782314341.178356528] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT_START' : 'succeeded' --> 'START_TIMER' +[sm-8] [INFO] [1782314341.178895979] [hri]: [timer_states.py:execute:19] Timer started at: 1782314341.1784122 +[sm-8] [INFO] [1782314341.179222070] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'START_TIMER' : 'succeeded' --> 'START_CON' +[sm-8] [INFO] [1782314341.180301827] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_DOOR_OPENING' +[sm-8] [INFO] [1782314341.180461719] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314341.181998932] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314341.182236512] [hri]: [detect_door_opening.py:execute:99] Waiting for door to open... +[sm-8] [INFO] [1782314345.936369233] [hri]: [detect_door_opening.py:_is_door_opened:71] Door has been opened. +[sm-8] [INFO] [1782314346.374708414] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_DOOR_OPENING' : 'door_opened' --> 'GO_TO_START' +[sm-8] [INFO] [1782314346.375156599] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PRE_NAV' +[sm-8] [INFO] [1782314346.375474830] [hri]: GIVING GOAL of pre_navigation +[sm-8] [INFO] [1782314346.375849338] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314346.376697756] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314349.071027065] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314349.073517597] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PRE_NAV' : 'succeeded' --> 'GO_TO_START_POSE' +[sm-8] [INFO] [1782314349.094194893] [hri]: Navigating to goal: 2.620011965794007 0.4284228083916832... +[sm-8] [INFO] [1782314371.229517845] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_START_POSE' : 'succeeded' --> 'POST_NAV' +[sm-8] [INFO] [1782314371.231069318] [hri]: GIVING GOAL of post_navigation +[sm-8] [INFO] [1782314371.231369178] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314371.232202193] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314374.053640279] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314374.054045356] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'POST_NAV' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314374.054331042] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314374.054595317] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_START' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314374.054851807] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314374.055466836] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'START_CON' : 'succeeded' --> 'GO_TO_DOOR' +[sm-8] [INFO] [1782314374.055741022] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PRE_NAV' +[sm-8] [INFO] [1782314374.056084963] [hri]: GIVING GOAL of pre_navigation +[sm-8] [INFO] [1782314374.056445324] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314374.057313199] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314376.884713980] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314376.885016752] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PRE_NAV' : 'succeeded' --> 'GO_TO_DOOR_POSE' +[sm-8] [INFO] [1782314376.885819207] [hri]: Navigating to goal: 0.9737232865224763 0.6644210227864706... +[sm-8] [INFO] [1782314382.410154416] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_DOOR_POSE' : 'succeeded' --> 'POST_NAV' +[sm-8] [INFO] [1782314382.411336708] [hri]: GIVING GOAL of post_navigation +[sm-8] [INFO] [1782314382.412928754] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314382.413815205] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314385.115156399] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314385.115512170] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'POST_NAV' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314385.115776510] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314385.116073480] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_DOOR' : 'succeeded' --> 'GREET' +[sm-8] [INFO] [1782314385.116429422] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'SAY_WAITING_FOR_GUEST' +[sm-8] [INFO] [1782314385.116887266] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314385.117813277] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314387.214955934] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_WAITING_FOR_GUEST' : 'succeeded' --> 'WAIT_FOR_GUEST' +[sm-8] [INFO] [1782314387.215261097] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314387.215497497] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314387.481121715] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314387.492656197] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314388.404141494] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314388.404500072] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314388.441243581] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314388.442303110] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314388.442581639] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314388.442949461] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314388.443202009] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314388.696856693] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314388.697552816] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314388.769137528] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314388.770001938] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314388.809185211] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314388.809520923] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314388.809792411] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314388.810158860] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314388.810421653] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314389.065612182] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314389.067701764] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314389.101892738] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314389.102243372] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314389.143842899] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314389.144222615] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314389.144537512] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314389.144987601] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314389.145276288] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314389.406128043] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314389.406898705] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314389.432076058] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314389.432412444] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314389.471573805] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314389.473443631] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314389.473753268] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314389.474192151] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314389.474476349] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314389.730354678] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314389.733411805] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314389.764435325] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314389.765087627] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314389.790952658] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314389.794920182] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314389.809999801] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314389.810525779] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314389.810891384] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314390.061767796] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314390.062582008] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314390.095773884] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314390.096164574] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314390.130750827] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314390.134391345] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314390.134659730] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314390.135112807] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314390.135370208] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314390.395560938] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314390.396320349] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314390.432387739] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314390.432754116] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314390.469541353] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314390.474268799] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314390.475301637] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314390.475650884] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314390.475902192] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314390.753590549] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314390.765846112] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314390.799425332] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314390.801331807] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314390.838191447] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314390.838573013] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314390.838890038] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314390.839420731] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314390.839677163] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314391.101328323] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314391.102217791] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314391.135441495] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314391.135804467] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314391.168479386] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314391.168975365] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314391.169420881] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314391.169880190] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314391.170281884] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314391.435896074] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314391.436542227] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314391.467184294] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314391.468308500] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314391.505172037] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314391.505621046] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314391.506023531] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314391.506525163] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314391.506875537] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314391.759833006] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314391.760457843] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314391.803867714] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314391.804296958] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314391.831152661] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314391.831447727] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314391.831718711] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314391.832137140] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314391.832403299] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314392.097506256] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314392.098391084] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314392.127338619] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314392.128051200] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314392.165511112] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314392.165833789] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314392.166107702] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314392.166471335] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314392.166719515] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314392.435643960] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314392.457604136] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314392.494261110] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314392.494583226] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314392.534551601] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314392.534991498] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314392.535315958] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314392.535746768] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314392.536073604] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314392.790614354] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314392.791447815] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314392.826836737] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314392.827177498] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314392.862636627] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314392.862999706] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314392.866342234] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314392.867098210] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314392.867380380] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314393.124104600] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314393.124745051] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314393.163105185] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314393.163564720] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314393.196416615] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314393.196812940] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314393.199780047] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314393.202339276] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314393.202658139] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314393.457341289] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314393.461935596] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314393.493984667] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314393.494305232] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314393.534155217] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314393.536632237] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314393.537901867] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314393.538348849] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314393.538621202] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314393.799331432] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314393.803237132] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314393.834829698] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314393.835243718] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314393.868441653] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314393.868812330] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314393.869112543] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314393.869507536] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314393.869799245] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314394.129106046] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314394.129920947] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314394.157649555] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314394.158074523] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314394.202294927] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314394.202647057] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314394.202956352] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314394.203343411] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314394.203614986] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314394.456633660] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314394.457411548] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314394.491890436] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314394.492238703] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314394.523995433] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314394.524358456] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314394.524638118] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314394.525065052] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314394.525309627] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314394.787270368] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314394.795271079] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314394.825193953] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314394.825595722] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314394.860356606] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314394.860669495] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314394.860948368] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314394.861312069] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314394.861535420] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314395.123168041] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314395.124357188] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314395.158100459] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314395.158456354] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314395.194309804] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314395.199204488] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314395.222080371] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314395.223514930] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314395.223895259] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314395.487140148] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314395.487876553] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314395.523444756] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314395.524099976] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314395.566004826] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314395.566356228] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314395.566606716] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314395.567048297] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314395.567299479] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314395.822974496] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314395.823661200] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314395.954592607] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314395.954947882] [hri]: [detect_3d.py:response_handler:121] person at (-0.35, 0.52, 1.20) +[sm-8] [INFO] [1782314395.955269203] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314395.983420852] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.34512436138449376, y:0.5201997670753591, z:1.1993635525482622 +[sm-8] [INFO] [1782314395.994148630] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314395.994589248] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314395.995016593] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314395.995508850] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314395.995928978] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314396.253256794] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314396.254098705] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314396.292513132] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314396.292835685] [hri]: [detect_3d.py:response_handler:121] person at (-0.12, 0.76, 1.21) +[sm-8] [INFO] [1782314396.293227836] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314396.327675959] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.12336103453084213, y:0.7615448795529552, z:1.2085109431381835 +[sm-8] [INFO] [1782314396.328378247] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314396.328725674] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314396.329058919] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314396.329555530] [hri]: [wait_for_person_in_area.py:execute:22] Found 1 people in wait area. +[sm-8] [INFO] [1782314396.329866765] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'done' --> 'succeeded' +[sm-8] [INFO] [1782314396.330155303] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314396.330380833] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT_FOR_GUEST' : 'succeeded' --> 'GET_PERSON_POINT' +[sm-8] [INFO] [1782314396.335300661] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_PERSON_POINT' : 'succeeded' --> 'LOOK_AND_GREET' +[sm-8] [INFO] [1782314396.361794646] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'GREET_AND_ASK_GUEST' +[sm-8] [INFO] [1782314396.362022380] [hri]: [action_state.py:execute:165] Waiting for action '/lasr_vision_eye_tracker/track_eyes' +[sm-8] [INFO] [1782314396.363300099] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'SAY' +[sm-8] [INFO] [1782314396.363617137] [hri]: [action_state.py:execute:189] Sending goal to action '/lasr_vision_eye_tracker/track_eyes' +[sm-8] [INFO] [1782314396.364016750] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314396.364884061] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[eye_tracker_action_server-5] [INFO] [1782314396.365155360] [eye_tracker_action_server]: Received eye tracker goal +[eye_tracker_action_server-5] [INFO] [1782314396.366320832] [eye_tracker_action_server]: Beginning eye tracking... +[sm-8] [INFO] [1782314402.556218583] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY' : 'succeeded' --> 'LISTEN' +[sm-8] [INFO] [1782314402.556642323] [hri]: [action_state.py:execute:165] Waiting for action 'transcribe_speech' +[sm-8] [INFO] [1782314402.557715755] [hri]: [action_state.py:execute:189] Sending goal to action 'transcribe_speech' +[transcribe_microphone_server-6] [INFO] [1782314402.559651478] [whisper_mic_server]: Request Received +[transcribe_microphone_server-6] [INFO] [1782314408.939430643] [whisper_mic_server]: Transcribing phrase with Whisper... +[transcribe_microphone_server-6] [INFO] [1782314409.799771966] [whisper_mic_server]: Transcription finished! +[transcribe_microphone_server-6] [INFO] [1782314409.800055898] [whisper_mic_server]: Time taken: 0.86s +[transcribe_microphone_server-6] [INFO] [1782314409.800382678] [whisper_mic_server]: Transcribed phrase: Hi Tiago, my name is Jeff. My favourite drink is vodka. +[transcribe_microphone_server-6] [INFO] [1782314409.800634678] [whisper_mic_server]: transcribe_speech has succeeded +[sm-8] [INFO] [1782314409.817050109] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LISTEN' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314409.817410032] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314409.817731117] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GREET_AND_ASK_GUEST' : 'succeeded' --> 'GET_NAME_DRINK_FACE' +[sm-8] [INFO] [1782314409.820987916] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PARSE_NAME' +[sm-8] [INFO] [1782314409.821051912] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'INITIALISE_DETECTION_FLAG' +[sm-8] [INFO] [1782314409.821736466] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_3D' +[sm-8] [INFO] [1782314409.822417239] [hri]: [service_state.py:execute:138] Waiting for service '/hri_task/query_llm' +[sm-8] [INFO] [1782314409.822988475] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'INITIALISE_DETECTION_FLAG' : 'succeeded' --> 'GET_GUEST_ATTRIBUTES' +[sm-8] [INFO] [1782314409.853873962] [hri]: [service_state.py:execute:153] Sending request to service '/hri_task/query_llm' +[sm-8] [INFO] [1782314409.854596042] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'GET_IMAGE' +[hri_task_service-7] [INFO] [1782314409.854722834] [llm]: Received query: Hi Tiago, my name is Jeff. My favourite drink is vodka., and task is name +[sm-8] [INFO] [1782314409.855587765] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_IMAGE' : 'succeeded' --> 'GET_ATTRIBUTES' +[sm-8] [INFO] [1782314409.886556333] [hri]: [service_state.py:execute:138] Waiting for service '/vlm/describe_people' +[sm-8] [INFO] [1782314409.888040784] [hri]: [service_state.py:execute:153] Sending request to service '/vlm/describe_people' +[vlm_service-4] [INFO] [1782314409.894790038] [lasr_vlm_service]: Received request to describe person +[sm-8] [INFO] [1782314410.111717291] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314410.112703960] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314410.182934604] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314410.183579032] [hri]: [detect_3d.py:response_handler:121] person at (-0.04, 1.01, 1.43) +[sm-8] [INFO] [1782314410.184122963] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314410.184748034] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314410.185582207] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314410.250235537] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314410.252828186] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314410.254875146] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 114897 / 921600 +[sm-8] [INFO] [1782314410.291338968] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314410.315459902] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314410.316396694] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] 2026-06-24 16:20:10.327360: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.333350: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.336871: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.339977: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.345465: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.349577: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.354787: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.361329: I external/local_xla/xla/stream_executor/cuda/cuda_executor.cc:998] successful NUMA node read from SysFS had negative value (-1), but there must be at least one NUMA node, so returning NUMA node zero. See more at https://github.com/torvalds/linux/blob/v6.0/Documentation/ABI/testing/sysfs-bus-pci#L344-L355 +[service-3] 2026-06-24 16:20:10.363720: I tensorflow/core/common_runtime/gpu/gpu_device.cc:1928] Created device /job:localhost/replica:0/task:0/device:GPU:0 with 1568 MB memory: -> device: 0, name: NVIDIA RTX A2000 8GB Laptop GPU, pci bus id: 0000:01:00.0, compute capability: 8.6 +[vlm_service-4] [INFO] [1782314424.869386219] [lasr_vlm_service]: VLM result: {'hair_color': ['black'], 'hair_length': ['shoulderlength'], 'glasses': [True], 'hat': [True], 'shirt color': ['black']} +[sm-8] [INFO] [1782314424.895810641] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_ATTRIBUTES' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314424.897098254] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314424.897450820] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_GUEST_ATTRIBUTES' : 'succeeded' --> 'HANDLE_GUEST_ATTRIBUTES' +[sm-8] [INFO] [1782314424.898047871] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'HANDLE_GUEST_ATTRIBUTES' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314424.925036438] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[service-3] 2026-06-24 16:20:26.600316: I external/local_xla/xla/stream_executor/cuda/cuda_dnn.cc:465] Loaded cuDNN version 8907 +[service-3] 2026-06-24 16:20:29.098072: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 4.12GiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] 2026-06-24 16:20:29.154755: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 2.07GiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] [INFO] [1782314430.683487676] [lasr_vision_reid]: Added face embedding for guest1, total samples: 1 +[sm-8] [INFO] [1782314430.689488888] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314430.690398226] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 1/10. +[sm-8] [INFO] [1782314430.690869754] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314430.954923545] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314430.956116573] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314431.016539637] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314431.016984042] [hri]: [detect_3d.py:response_handler:121] person at (-0.12, 1.06, 1.40) +[sm-8] [INFO] [1782314431.017413776] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314431.017940335] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314431.033676593] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314431.087479329] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314431.088555630] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314431.112900098] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 103509 / 921600 +[sm-8] [INFO] [1782314431.114901407] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314431.115780944] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314431.116512538] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314431.301554918] [lasr_vision_reid]: Added face embedding for guest1, total samples: 2 +[sm-8] [INFO] [1782314431.316290370] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314431.317168728] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 2/10. +[sm-8] [INFO] [1782314431.317679851] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314431.584985311] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314431.585745892] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314431.651792958] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314431.652307634] [hri]: [detect_3d.py:response_handler:121] person at (-0.14, 1.07, 1.40) +[sm-8] [INFO] [1782314431.652790786] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314431.653395125] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314431.654165168] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314431.715124523] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314431.716240963] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314431.721641080] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 102024 / 921600 +[sm-8] [INFO] [1782314431.722835136] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314431.723700100] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314431.768199129] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314431.951383581] [lasr_vision_reid]: Added face embedding for guest1, total samples: 3 +[sm-8] [INFO] [1782314431.956249868] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314431.987987226] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 3/10. +[sm-8] [INFO] [1782314431.989595156] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314432.247945177] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314432.248705623] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314432.309980664] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314432.310433469] [hri]: [detect_3d.py:response_handler:121] person at (-0.14, 1.07, 1.39) +[sm-8] [INFO] [1782314432.310857930] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314432.311367499] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314432.312085254] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314432.375980318] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314432.377472665] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314432.379412658] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 104388 / 921600 +[sm-8] [INFO] [1782314432.380296784] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314432.380975504] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314432.381656404] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314432.563331240] [lasr_vision_reid]: Added face embedding for guest1, total samples: 4 +[sm-8] [INFO] [1782314432.585852693] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314432.586584392] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 4/10. +[sm-8] [INFO] [1782314432.586941679] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314432.844371459] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314432.845215437] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314432.910263242] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314432.910702734] [hri]: [detect_3d.py:response_handler:121] person at (-0.13, 1.06, 1.38) +[sm-8] [INFO] [1782314432.911832306] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314432.912326997] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314432.914355817] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314432.952128963] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314432.953006411] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314432.977072357] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 105897 / 921600 +[sm-8] [INFO] [1782314432.978308366] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314432.979148786] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314432.979975772] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314433.163809685] [lasr_vision_reid]: Added face embedding for guest1, total samples: 5 +[sm-8] [INFO] [1782314433.198105072] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314433.213541369] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 5/10. +[sm-8] [INFO] [1782314433.213947331] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314433.509554507] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314433.510903096] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314433.577873231] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314433.578403052] [hri]: [detect_3d.py:response_handler:121] person at (-0.13, 1.07, 1.38) +[sm-8] [INFO] [1782314433.578838308] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314433.579361046] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314433.580086944] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314433.619500257] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314433.643210846] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314433.645301944] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 107139 / 921600 +[sm-8] [INFO] [1782314433.646560980] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314433.647535928] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314433.648381873] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314433.835112025] [lasr_vision_reid]: Added face embedding for guest1, total samples: 6 +[sm-8] [INFO] [1782314433.847967004] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314433.851558013] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 6/10. +[sm-8] [INFO] [1782314433.852300784] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314434.108419854] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314434.110877850] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314434.174764059] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314434.175471336] [hri]: [detect_3d.py:response_handler:121] person at (-0.13, 1.07, 1.38) +[sm-8] [INFO] [1782314434.175996036] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314434.176492452] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314434.180430591] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314434.241703265] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314434.242753470] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314434.244813361] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 106155 / 921600 +[sm-8] [INFO] [1782314434.245845235] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314434.246623968] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314434.247424520] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314434.430963287] [lasr_vision_reid]: Added face embedding for guest1, total samples: 7 +[sm-8] [INFO] [1782314434.449956815] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314434.451001680] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 7/10. +[sm-8] [INFO] [1782314434.451481663] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314434.708120590] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314434.709094186] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314434.774433658] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314434.775065205] [hri]: [detect_3d.py:response_handler:121] person at (-0.13, 1.06, 1.38) +[sm-8] [INFO] [1782314434.775633103] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314434.776321337] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314434.777224456] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314434.814401882] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314434.815313070] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314434.816793539] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 106908 / 921600 +[sm-8] [INFO] [1782314434.817686691] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314434.818410062] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314434.840564297] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314435.025327657] [lasr_vision_reid]: Added face embedding for guest1, total samples: 8 +[sm-8] [INFO] [1782314435.072071154] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314435.077576310] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 8/10. +[sm-8] [INFO] [1782314435.078144838] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314435.347625925] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314435.348496820] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314435.377674005] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314435.378102362] [hri]: [detect_3d.py:response_handler:121] person at (-0.13, 1.05, 1.38) +[sm-8] [INFO] [1782314435.378530754] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314435.379079896] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314435.379864908] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314435.438940027] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314435.440503457] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314435.442202376] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 108201 / 921600 +[sm-8] [INFO] [1782314435.443154567] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314435.443907042] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314435.444622812] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314435.623515888] [lasr_vision_reid]: Added face embedding for guest1, total samples: 9 +[sm-8] [INFO] [1782314435.647110275] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314435.648006061] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 9/10. +[sm-8] [INFO] [1782314435.648461847] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314435.906494421] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314435.907823671] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314435.948677703] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314435.949107579] [hri]: [detect_3d.py:response_handler:121] person at (-0.11, 1.05, 1.38) +[sm-8] [INFO] [1782314435.949510607] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314435.950023399] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314435.972291886] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314436.044079604] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314436.045224389] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314436.047141138] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 110460 / 921600 +[sm-8] [INFO] [1782314436.048060008] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314436.048768283] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314436.049477839] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314436.230825321] [lasr_vision_reid]: Added face embedding for guest1, total samples: 10 +[sm-8] [INFO] [1782314436.246568633] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [INFO] [1782314436.248236470] [hri]: [hri_learn_faces.py:execute:116] Collected enough images for the guest. +[sm-8] [INFO] [1782314436.248697476] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314436.249137260] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[hri_task_service-7] [INFO] [1782314438.041799527] [llm]: LLM output: Name: Jeff +[hri_task_service-7] [INFO] [1782314438.042272487] [llm]: Returning response: lasr_llm_interfaces.msg.ReceptionistResponse(name='Jeff', favourite_drink='', interests='', interest_commonality='', llm_response='Name: Jeff') +[sm-8] [INFO] [1782314438.073463862] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PARSE_NAME' : 'succeeded' --> 'PARSE_DRINK' +[sm-8] [INFO] [1782314438.074117025] [hri]: [service_state.py:execute:138] Waiting for service '/hri_task/query_llm' +[sm-8] [INFO] [1782314438.074875715] [hri]: [service_state.py:execute:153] Sending request to service '/hri_task/query_llm' +[hri_task_service-7] [INFO] [1782314438.075535530] [llm]: Received query: Hi Tiago, my name is Jeff. My favourite drink is vodka., and task is drink +[hri_task_service-7] [INFO] [1782314438.587218621] [llm]: LLM output: Favourite drink: vodka +[hri_task_service-7] [INFO] [1782314438.587533297] [llm]: Returning response: lasr_llm_interfaces.msg.ReceptionistResponse(name='', favourite_drink='vodka', interests='', interest_commonality='', llm_response='Favourite drink: vodka') +[sm-8] [INFO] [1782314438.609464197] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PARSE_DRINK' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314438.610500665] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314438.611033850] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_NAME_DRINK_FACE' : 'succeeded' --> 'SAY_WELCOME' +[sm-8] [INFO] [1782314438.611722201] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314438.635061371] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314443.897263113] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_WELCOME' : 'succeeded' --> 'STOP_EYE_TRACKING_1' +[sm-8] [INFO] [1782314443.897728177] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'STOP_EYE_TRACKING_1' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314443.898055222] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314444.180554694] [hri]: [eye_tracker.py:handle_feedback:24] Cancelling current eye tracker +[eye_tracker_action_server-5] [INFO] [1782314444.186564303] [eye_tracker_action_server]: Eye Tracker Action Server cancellation requested +[sm-8] [INFO] [1782314444.194460561] [hri]: [state.hpp:cancel_state:173] Canceling state 'StartEyeTracker' +[sm-8] [INFO] [1782314444.429422889] [hri]: [eye_tracker.py:handle_feedback:24] Cancelling current eye tracker +[sm-8] [INFO] [1782314444.432461323] [hri]: [state.hpp:cancel_state:173] Canceling state 'StartEyeTracker' +[eye_tracker_action_server-5] [INFO] [1782314444.903215497] [eye_tracker_action_server]: Eye Tracker Action Server canceled, stopping tracking. +[eye_tracker_action_server-5] [INFO] [1782314445.408029528] [eye_tracker_action_server]: Canceled EYE TRACKER +[sm-8] [INFO] [1782314445.445390963] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AND_GREET' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314445.445704834] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314445.445986830] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GREET' : 'succeeded' --> 'GUIDE_TO_SEAT' +[sm-8] [INFO] [1782314445.446270434] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PRE_NAV' +[sm-8] [INFO] [1782314445.446540815] [hri]: GIVING GOAL of pre_navigation +[sm-8] [INFO] [1782314445.446896671] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314445.447816922] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314448.209816231] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314448.210119608] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PRE_NAV' : 'succeeded' --> 'GO_TO_SEAT_POSE' +[sm-8] [INFO] [1782314448.211101356] [hri]: Navigating to goal: 0.9241805428867255 -0.37066992922907555... +[sm-8] [INFO] [1782314496.803613492] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_SEAT_POSE' : 'succeeded' --> 'POST_NAV' +[sm-8] [INFO] [1782314496.805730947] [hri]: GIVING GOAL of post_navigation +[sm-8] [INFO] [1782314496.806072498] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314496.806925397] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314499.565752034] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314499.566151292] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'POST_NAV' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314499.566448814] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314499.566735715] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GUIDE_TO_SEAT' : 'succeeded' --> 'SEAT_GUEST' +[sm-8] [INFO] [1782314499.567105141] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'SAY_FINDING_SEAT' +[sm-8] [INFO] [1782314499.567565610] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314499.569721504] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314501.965598401] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_FINDING_SEAT' : 'succeeded' --> 'RESET_HEAD_1' +[sm-8] [INFO] [1782314501.965897162] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314501.966274524] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314501.967342144] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314503.492120997] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314503.492427068] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_1' : 'succeeded' --> 'DETECT_ALL_PEOPLE_SEATS' +[sm-8] [INFO] [1782314503.492694850] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'INIT_BLACKBOARD' +[sm-8] [INFO] [1782314503.493023355] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'INIT_BLACKBOARD' : 'succeeded' --> 'CALCULATE_SWEEP_POINTS' +[sm-8] [INFO] [1782314503.493356200] [hri]: [detect_all_in_polygon.py:_calculate_sweep_points:300] Waiting for camera info and TF to map frame... +[sm-8] [INFO] [1782314503.537370375] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 1.57%, score: 0.04 +[sm-8] [INFO] [1782314503.551226771] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 3.10%, score: 0.04 +[sm-8] [INFO] [1782314503.565932856] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 4.62%, score: 0.04 +[sm-8] [INFO] [1782314503.567354881] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 6.03%, score: 0.03 +[sm-8] [INFO] [1782314503.570110312] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 7.44%, score: 0.03 +[sm-8] [INFO] [1782314503.571248886] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 8.81%, score: 0.03 +[sm-8] [INFO] [1782314503.589883432] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 10.09%, score: 0.03 +[sm-8] [INFO] [1782314503.590816717] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 11.32%, score: 0.03 +[sm-8] [INFO] [1782314503.591353121] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 12.26%, score: 0.02 +[sm-8] [INFO] [1782314503.591775561] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 13.07%, score: 0.02 +[sm-8] [INFO] [1782314503.632196966] [hri]: [detect_all_in_polygon.py:_calculate_sweep_points:349] Calculated 10 sweep points. +[sm-8] [INFO] [1782314503.632667126] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CALCULATE_SWEEP_POINTS' : 'succeeded' --> 'LOOK_AND_DETECT' +[sm-8] [INFO] [1782314503.633054050] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314503.633531392] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 0 +[sm-8] [INFO] [1782314503.633906069] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314503.634471773] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.170, -2.091, 0.700) +[sm-8] [INFO] [1782314503.634793972] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314503.636097897] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314504.691562179] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'succeeded' --> 'SLEEP' +[sm-8] [INFO] [1782314504.692084372] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314506.694211597] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314506.694485807] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314506.950927550] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314506.951760181] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314507.017338319] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314507.017677691] [hri]: [detect_3d.py:response_handler:121] chair at (-0.25, -1.65, 0.49) +[sm-8] [INFO] [1782314507.018038905] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.59, 0.62) +[sm-8] [INFO] [1782314507.018329347] [hri]: [detect_3d.py:response_handler:121] chair at (-0.01, -2.67, 0.47) +[sm-8] [INFO] [1782314507.018629511] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314507.054929653] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.25209418205868694, y:-1.6531467610787582, z:0.4872168065776056 +[sm-8] [INFO] [1782314507.055381248] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5385083821161619, y:-2.5922757041932405, z:0.6247916595294938 +[sm-8] [INFO] [1782314507.055790584] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.014379232045992452, y:-2.67464577225065, z:0.46622707207590297 +[sm-8] [INFO] [1782314507.056364199] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314507.056618237] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314507.056893047] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314507.057459902] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314507.057657552] [hri]: Detected objects: +[sm-8] [INFO] [1782314507.057919271] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314507.058135067] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314507.058327076] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314507.058638722] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314507.059104702] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 1 +[sm-8] [INFO] [1782314507.059501593] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314507.060047005] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(-0.017, -2.284, 0.700) +[sm-8] [INFO] [1782314507.060381551] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314507.061268253] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [WARN] [1782314512.095119305] [hri]: [action_state.py:execute:205] Timeout reached while waiting for response from action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314512.107286370] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'timeout' --> 'SLEEP' +[sm-8] [INFO] [1782314512.110512049] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314514.113052574] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314514.113482681] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314514.381820778] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314514.382740528] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314514.439888316] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314514.440729356] [hri]: [detect_3d.py:response_handler:121] chair at (-0.31, -1.61, 0.50) +[sm-8] [INFO] [1782314514.441117763] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.62, 0.61) +[sm-8] [INFO] [1782314514.441449413] [hri]: [detect_3d.py:response_handler:121] chair at (-0.03, -2.73, 0.46) +[sm-8] [INFO] [1782314514.441823505] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314514.476613807] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.3056239218455188, y:-1.6119756072163294, z:0.5037601247251005 +[sm-8] [INFO] [1782314514.477107952] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5422432904088664, y:-2.6231152700597695, z:0.6129247652003355 +[sm-8] [INFO] [1782314514.477506736] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.030954601232241585, y:-2.7291086666301396, z:0.4611574059305624 +[sm-8] [INFO] [1782314514.485594629] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314514.485913033] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314514.486207079] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314514.486670173] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314514.486948921] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314514.487231320] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314514.487542808] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314514.487776327] [hri]: Detected objects: +[sm-8] [INFO] [1782314514.496584098] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314514.504779583] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314514.505994713] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314514.506319521] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314514.506777202] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 2 +[sm-8] [INFO] [1782314514.507136718] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314514.507608032] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.085, -1.826, 0.700) +[sm-8] [INFO] [1782314514.507977149] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314514.510034474] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314514.514771109] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314514.515224047] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314516.518277299] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314516.540360648] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314516.813756176] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314516.814653694] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314516.872012584] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314516.874272513] [hri]: [detect_3d.py:response_handler:121] chair at (-0.30, -1.61, 0.50) +[sm-8] [INFO] [1782314516.874551880] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.61, 0.62) +[sm-8] [INFO] [1782314516.874821440] [hri]: [detect_3d.py:response_handler:121] chair at (-0.04, -2.73, 0.45) +[sm-8] [INFO] [1782314516.875098977] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314516.907819456] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.3047551475442867, y:-1.6084712450714096, z:0.5002625631274525 +[sm-8] [INFO] [1782314516.908296940] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5415599037215428, y:-2.6086463838081784, z:0.6204043280578496 +[sm-8] [INFO] [1782314516.908660150] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.0389145973488042, y:-2.7338368690143664, z:0.4532160217901322 +[sm-8] [INFO] [1782314516.909196136] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314516.909447364] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314516.909657870] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314516.910102019] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314516.910405207] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314516.910696710] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314516.911047868] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314516.911224298] [hri]: Detected objects: +[sm-8] [INFO] [1782314516.911465064] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314516.911716531] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314516.911935192] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314516.912247307] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314516.912719705] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 3 +[sm-8] [INFO] [1782314516.913148271] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314516.913656873] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.443, -2.163, 0.700) +[sm-8] [INFO] [1782314516.914008692] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314516.914930098] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [WARN] [1782314521.933182350] [hri]: [action_state.py:execute:205] Timeout reached while waiting for response from action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314521.934404603] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'timeout' --> 'SLEEP' +[sm-8] [INFO] [1782314521.934752468] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314523.937086431] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314523.937409958] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314524.194281433] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314524.195224226] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314524.229137308] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314524.229995811] [hri]: [detect_3d.py:response_handler:121] person at (0.55, -2.55, 0.63) +[sm-8] [INFO] [1782314524.230283883] [hri]: [detect_3d.py:response_handler:121] chair at (0.01, -2.58, 0.53) +[sm-8] [INFO] [1782314524.230585757] [hri]: [detect_3d.py:response_handler:121] chair at (-0.08, -1.72, 0.47) +[sm-8] [INFO] [1782314524.230923887] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314524.270848189] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5470645583161777, y:-2.5450350005980287, z:0.6303077921823138 +[sm-8] [INFO] [1782314524.271425829] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:0.009756133472979323, y:-2.5753124596863413, z:0.5340026670226391 +[sm-8] [INFO] [1782314524.271830783] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.08082877455466853, y:-1.7249992692420997, z:0.472650426604313 +[sm-8] [INFO] [1782314524.272574123] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314524.272896793] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314524.273160400] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314524.290124765] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314524.290471060] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314524.290737688] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314524.291046308] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314524.291231089] [hri]: Detected objects: +[sm-8] [INFO] [1782314524.291461185] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314524.291665393] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314524.291865259] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314524.292166247] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314524.292560882] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 4 +[sm-8] [INFO] [1782314524.292895906] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314524.293330110] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.051, -2.757, 0.700) +[sm-8] [INFO] [1782314524.293602060] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314524.294365284] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314524.299889311] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314524.300300104] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314526.321035180] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314526.322178713] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314526.586517250] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314526.589897817] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314526.625126191] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314526.625447976] [hri]: [detect_3d.py:response_handler:121] chair at (-0.16, -1.69, 0.47) +[sm-8] [INFO] [1782314526.625735740] [hri]: [detect_3d.py:response_handler:121] chair at (-0.01, -2.65, 0.47) +[sm-8] [INFO] [1782314526.626090444] [hri]: [detect_3d.py:response_handler:121] person at (0.52, -2.66, 0.63) +[sm-8] [INFO] [1782314526.626456193] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314526.666684951] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.15514888051987796, y:-1.689654009286174, z:0.47398795183323017 +[sm-8] [INFO] [1782314526.669512890] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.0051828193436621595, y:-2.65142085618084, z:0.46778712366078223 +[sm-8] [INFO] [1782314526.686662590] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5246011835652983, y:-2.655417230014539, z:0.6287310880109291 +[sm-8] [INFO] [1782314526.687310132] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314526.689076281] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314526.689405346] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314526.689843895] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314526.690142790] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314526.690407885] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314526.690746270] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314526.690963949] [hri]: Detected objects: +[sm-8] [INFO] [1782314526.691194271] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314526.691392929] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314526.691594709] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314526.691868269] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314526.692307223] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 5 +[sm-8] [INFO] [1782314526.692642906] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314526.693142662] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(-0.243, -2.485, 0.700) +[sm-8] [INFO] [1782314526.693440658] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314526.694276041] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314526.702186481] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314526.702587869] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314528.710884290] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314528.715823960] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314528.984198977] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314528.985099363] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314529.021868314] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314529.022197961] [hri]: [detect_3d.py:response_handler:121] chair at (-0.30, -1.63, 0.49) +[sm-8] [INFO] [1782314529.022468309] [hri]: [detect_3d.py:response_handler:121] person at (0.53, -2.66, 0.62) +[sm-8] [INFO] [1782314529.022786551] [hri]: [detect_3d.py:response_handler:121] chair at (-0.04, -2.77, 0.42) +[sm-8] [INFO] [1782314529.023118689] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314529.054482263] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.2990152029644403, y:-1.6282368703247216, z:0.49309862287237005 +[sm-8] [INFO] [1782314529.054965638] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5308949113886658, y:-2.6636714406060613, z:0.6163369804246929 +[sm-8] [INFO] [1782314529.055341463] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.044670673099659663, y:-2.7718244891759074, z:0.42222459057309814 +[sm-8] [INFO] [1782314529.055912122] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314529.056185706] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314529.056443197] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314529.056820730] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314529.057145858] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314529.057480129] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314529.065230645] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314529.080532408] [hri]: Detected objects: +[sm-8] [INFO] [1782314529.084295429] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314529.085016579] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314529.085265065] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314529.085605971] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314529.086086325] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 6 +[sm-8] [INFO] [1782314529.086471699] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314529.087048423] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.464, -2.621, 0.700) +[sm-8] [INFO] [1782314529.087333384] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314529.089836807] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314529.097489389] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314529.097885813] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314531.114763715] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314531.117456882] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314531.394433877] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314531.395608941] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314531.428526800] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314531.428902920] [hri]: [detect_3d.py:response_handler:121] person at (0.55, -2.61, 0.64) +[sm-8] [INFO] [1782314531.445371443] [hri]: [detect_3d.py:response_handler:121] chair at (-0.03, -1.75, 0.48) +[sm-8] [INFO] [1782314531.446263610] [hri]: [detect_3d.py:response_handler:121] chair at (0.02, -2.56, 0.49) +[sm-8] [INFO] [1782314531.446585163] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314531.482240085] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5503916195981067, y:-2.6091213997288354, z:0.6354540756361748 +[sm-8] [INFO] [1782314531.482691572] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.033678736372550144, y:-1.7462530525428173, z:0.4847532310482078 +[sm-8] [INFO] [1782314531.483134586] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:0.01930195393220957, y:-2.5597499447913177, z:0.48768873698076043 +[sm-8] [INFO] [1782314531.483696711] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314531.483967150] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314531.484240376] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314531.484697418] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314531.484982283] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314531.485280152] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314531.485610560] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314531.485827627] [hri]: Detected objects: +[sm-8] [INFO] [1782314531.486092380] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314531.486291640] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314531.486492331] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314531.486781298] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314531.487227261] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 7 +[sm-8] [INFO] [1782314531.487607061] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314531.488147247] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.584, -2.396, 0.700) +[sm-8] [INFO] [1782314531.488480438] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314531.489586607] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314532.487017510] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'succeeded' --> 'SLEEP' +[sm-8] [INFO] [1782314532.487459645] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314534.489379779] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314534.489826239] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314534.742024885] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314534.742717953] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314534.779052231] [hri]: [detect_3d.py:response_handler:119] Got 4 detections +[sm-8] [INFO] [1782314534.779363146] [hri]: [detect_3d.py:response_handler:121] person at (0.56, -2.57, 0.64) +[sm-8] [INFO] [1782314534.779632132] [hri]: [detect_3d.py:response_handler:121] chair at (0.01, -2.53, 0.49) +[sm-8] [INFO] [1782314534.779926717] [hri]: [detect_3d.py:response_handler:121] chair at (-0.03, -1.79, 0.47) +[sm-8] [INFO] [1782314534.780205707] [hri]: [detect_3d.py:response_handler:121] chair at (2.00, -3.62, 0.45) +[sm-8] [INFO] [1782314534.780501387] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314534.812098252] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5555911074722822, y:-2.565772882402859, z:0.6365989463993885 +[sm-8] [INFO] [1782314534.812538600] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:0.010655570983387985, y:-2.5299737879158033, z:0.490211669505762 +[sm-8] [INFO] [1782314534.812898431] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.02835617091186693, y:-1.7898496307289609, z:0.4671796515722655 +[sm-8] [INFO] [1782314534.813243457] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:1.9954101701315063, y:-3.6212017095608364, z:0.4483863752294778 +[sm-8] [INFO] [1782314534.813753727] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314534.813973768] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314534.814191763] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314534.814577102] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314534.814820448] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314534.815081498] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314534.815368807] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314534.815544596] [hri]: Detected objects: +[sm-8] [INFO] [1782314534.815768134] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314534.815970301] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314534.816176131] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314534.816421637] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314534.816843319] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 8 +[sm-8] [INFO] [1782314534.817182186] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314534.817605234] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.920, -1.718, 0.700) +[sm-8] [INFO] [1782314534.817920927] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314534.839766010] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314535.912010666] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'succeeded' --> 'SLEEP' +[sm-8] [INFO] [1782314535.912366423] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314537.914204544] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314537.914562805] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314538.168757229] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314538.169527127] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314538.206692434] [hri]: [detect_3d.py:response_handler:119] Got 4 detections +[sm-8] [INFO] [1782314538.207071760] [hri]: [detect_3d.py:response_handler:121] person at (0.55, -2.45, 0.68) +[sm-8] [INFO] [1782314538.207506391] [hri]: [detect_3d.py:response_handler:121] chair at (0.17, -2.54, 0.49) +[sm-8] [INFO] [1782314538.207872186] [hri]: [detect_3d.py:response_handler:121] chair at (1.95, -3.46, 0.43) +[sm-8] [INFO] [1782314538.208183711] [hri]: [detect_3d.py:response_handler:121] person at (1.99, -2.55, 0.68) +[sm-8] [INFO] [1782314538.208466390] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314538.243511095] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.547258531965884, y:-2.4519040156385152, z:0.6834171091536979 +[sm-8] [INFO] [1782314538.243985392] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:0.16837243964019266, y:-2.540950009116014, z:0.4865445903414336 +[sm-8] [INFO] [1782314538.244396602] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:1.9530945734135345, y:-3.463180824116707, z:0.4348811665503526 +[sm-8] [INFO] [1782314538.244872677] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:1.9878669075508482, y:-2.549264857168186, z:0.682310483527154 +[sm-8] [INFO] [1782314538.245488752] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314538.245762486] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314538.246066150] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314538.246478768] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314538.246739450] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314538.247111894] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314538.247351485] [hri]: Detected objects: +[sm-8] [INFO] [1782314538.247630123] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314538.247819350] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314538.248078257] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314538.248384923] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314538.248865379] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 9 +[sm-8] [INFO] [1782314538.249279602] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314538.249797015] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.348, -2.912, 0.700) +[sm-8] [INFO] [1782314538.250157183] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314538.250981480] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [WARN] [1782314543.265569240] [hri]: [action_state.py:execute:205] Timeout reached while waiting for response from action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314543.269736831] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'timeout' --> 'SLEEP' +[sm-8] [INFO] [1782314543.270164292] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314545.272132475] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314545.272412654] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314545.526139562] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314545.526797201] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314545.564840987] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314545.565176034] [hri]: [detect_3d.py:response_handler:121] chair at (0.01, -2.57, 0.49) +[sm-8] [INFO] [1782314545.565448123] [hri]: [detect_3d.py:response_handler:121] person at (0.53, -2.62, 0.63) +[sm-8] [INFO] [1782314545.565732000] [hri]: [detect_3d.py:response_handler:121] chair at (-0.06, -1.74, 0.48) +[sm-8] [INFO] [1782314545.566029098] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314545.602368852] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:0.009844900797363887, y:-2.5654199313133454, z:0.4887657968300644 +[sm-8] [INFO] [1782314545.602963828] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.5286957573586943, y:-2.6241653512518264, z:0.6283272278909247 +[sm-8] [INFO] [1782314545.603442174] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.05984870521414132, y:-1.7440766856481247, z:0.4842115150836901 +[sm-8] [INFO] [1782314545.604285248] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314545.604539353] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314545.604832377] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314545.605356614] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314545.605614146] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314545.605968677] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314545.606361587] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314545.606628326] [hri]: Detected objects: +[sm-8] [INFO] [1782314545.606958348] [hri]: - chair at (-0.25209418205868694, -1.6531467610787582, 0.4872168065776056) +[sm-8] [INFO] [1782314545.607203504] [hri]: - person at (0.5385083821161619, -2.5922757041932405, 0.6247916595294938) +[sm-8] [INFO] [1782314545.607459597] [hri]: - chair at (-0.014379232045992452, -2.67464577225065, 0.46622707207590297) +[sm-8] [INFO] [1782314545.615875714] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314545.632645091] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 10 +[sm-8] [INFO] [1782314545.634390192] [hri]: [detect_all_in_polygon.py:_get_look_point:454] Finished iterating through sweep points. +[sm-8] [INFO] [1782314545.634667628] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314545.634945746] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314545.635203613] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AND_DETECT' : 'succeeded' --> 'PUBLISH_DETECTED_OBJECTS' +[sm-8] [INFO] [1782314545.635632481] [hri]: Processing 3 detections for debug image. +[sm-8] [INFO] [1782314545.638014472] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.638236204] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.640217848] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.640445363] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.640672309] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.640915541] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.641220656] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.651416347] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.659525640] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314545.659868446] [hri]: Created debug image with detections. +[sm-8] [INFO] [1782314545.754054952] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PUBLISH_DETECTED_OBJECTS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314545.789040044] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314545.791809882] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_ALL_PEOPLE_SEATS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [WARN] [1782314545.792315035] [hri]: [seat_guest.py:execute:77] Finding seat in seat guest +[sm-8] [INFO] [1782314545.792905191] [hri]: [seat_guest.py:execute:103] Detected this many people in sweep: 1 +[sm-8] [INFO] [1782314545.793247405] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'LOOK_HOST' +[sm-8] [INFO] [1782314545.793663280] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.539, -2.592, 0.625) +[sm-8] [INFO] [1782314545.793963350] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314545.794728010] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314546.837291345] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_HOST' : 'succeeded' --> 'SAY_HOST' +[sm-8] [INFO] [1782314546.837800861] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314546.838850949] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314550.153423552] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_HOST' : 'succeeded' --> 'LEARN_HOST' +[sm-8] [INFO] [1782314550.153719364] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_3D' +[sm-8] [INFO] [1782314550.421515896] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314550.423024027] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314550.451329205] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314550.451782764] [hri]: [detect_3d.py:response_handler:121] person at (0.55, -2.55, 0.63) +[sm-8] [INFO] [1782314550.452156380] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314550.452653893] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314550.453356369] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314550.486841775] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314550.487677511] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314550.489198077] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62679 / 921600 +[sm-8] [INFO] [1782314550.490617264] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314550.491106616] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314550.491765818] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314550.651441332] [lasr_vision_reid]: Added face embedding for host, total samples: 1 +[sm-8] [INFO] [1782314550.653536325] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314550.654034342] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 0/10. +[sm-8] [INFO] [1782314550.654254630] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314550.919064285] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314550.919755277] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314550.950178233] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314550.950492874] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.63) +[sm-8] [INFO] [1782314550.950796318] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314550.951187246] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314550.951743465] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314550.984028566] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314550.984892119] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314550.987641358] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62976 / 921600 +[sm-8] [INFO] [1782314550.988521972] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314550.989217912] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314550.989964188] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314551.141644758] [lasr_vision_reid]: Added face embedding for host, total samples: 2 +[sm-8] [INFO] [1782314551.152948969] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314551.153465285] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 1/10. +[sm-8] [INFO] [1782314551.153717730] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314551.416868574] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314551.417646655] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314551.453230846] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314551.453550336] [hri]: [detect_3d.py:response_handler:121] person at (0.55, -2.55, 0.63) +[sm-8] [INFO] [1782314551.453853192] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314551.454305758] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314551.454972250] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314551.483497403] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314551.484222963] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314551.486503080] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62922 / 921600 +[sm-8] [INFO] [1782314551.487317831] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314551.487870635] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314551.488563131] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314551.634166268] [lasr_vision_reid]: Added face embedding for host, total samples: 3 +[sm-8] [INFO] [1782314551.648562517] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314551.649024216] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 2/10. +[sm-8] [INFO] [1782314551.649229351] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314551.929184276] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314551.950094498] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314551.987489822] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314551.987813373] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.54, 0.64) +[sm-8] [INFO] [1782314551.988168198] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314551.988587457] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314551.989374660] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314552.016498048] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314552.017429587] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314552.019069690] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62847 / 921600 +[sm-8] [INFO] [1782314552.019850586] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314552.020496676] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314552.021109536] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314552.170561781] [lasr_vision_reid]: Added face embedding for host, total samples: 4 +[sm-8] [INFO] [1782314552.181072132] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314552.181518484] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 3/10. +[sm-8] [INFO] [1782314552.181739649] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314552.447537369] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314552.448414622] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314552.482722078] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314552.483091671] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.64) +[sm-8] [INFO] [1782314552.483481121] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314552.484098560] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314552.484754222] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314552.515544569] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314552.516314090] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314552.517562425] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 61404 / 921600 +[sm-8] [INFO] [1782314552.522691495] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314552.523424046] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314552.524156740] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314552.671667966] [lasr_vision_reid]: Added face embedding for host, total samples: 5 +[sm-8] [INFO] [1782314552.680162032] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314552.680640527] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 4/10. +[sm-8] [INFO] [1782314552.680899521] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314552.942820609] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314552.948203020] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314552.980905326] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314552.981256094] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.63) +[sm-8] [INFO] [1782314552.981552002] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314552.982011917] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314552.982645538] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314553.019813908] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314553.020575766] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314553.022203150] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62409 / 921600 +[sm-8] [INFO] [1782314553.023017824] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314553.023563702] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314553.024264550] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314553.169830663] [lasr_vision_reid]: Added face embedding for host, total samples: 6 +[sm-8] [INFO] [1782314553.184241587] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314553.184869363] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 5/10. +[sm-8] [INFO] [1782314553.185113063] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314553.446382246] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314553.447326317] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314553.485453975] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314553.485857724] [hri]: [detect_3d.py:response_handler:121] person at (0.55, -2.55, 0.63) +[sm-8] [INFO] [1782314553.486799082] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314553.487265355] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314553.487943713] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314553.513861961] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314553.514763933] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314553.516093790] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62781 / 921600 +[sm-8] [INFO] [1782314553.516721878] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314553.517229329] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314553.517786484] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314553.667840094] [lasr_vision_reid]: Added face embedding for host, total samples: 7 +[sm-8] [INFO] [1782314553.680288170] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314553.680855810] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 6/10. +[sm-8] [INFO] [1782314553.681101354] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314553.945325508] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314553.948227303] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314553.979566865] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314553.979906489] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.64) +[sm-8] [INFO] [1782314553.980208233] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314553.980623644] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314553.981284638] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314554.014033892] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314554.014754118] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314554.016075584] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62277 / 921600 +[sm-8] [INFO] [1782314554.016760374] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314554.017296504] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314554.022329401] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314554.175364285] [lasr_vision_reid]: Added face embedding for host, total samples: 8 +[sm-8] [INFO] [1782314554.179125038] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314554.179574131] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 7/10. +[sm-8] [INFO] [1782314554.179820921] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314554.457184629] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314554.458051424] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314554.482368087] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314554.482714223] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.63) +[sm-8] [INFO] [1782314554.483085073] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314554.483508507] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314554.484296524] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314554.514328286] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314554.515042588] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314554.516755885] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 61878 / 921600 +[sm-8] [INFO] [1782314554.517576496] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314554.518215796] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314554.518902529] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314554.667921133] [lasr_vision_reid]: Added face embedding for host, total samples: 9 +[sm-8] [INFO] [1782314554.683884047] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314554.684342523] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 8/10. +[sm-8] [INFO] [1782314554.684556137] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314554.944261048] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314554.945025393] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314554.977836684] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314554.983008059] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.64) +[sm-8] [INFO] [1782314554.983324956] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314554.983752262] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314554.984359854] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314555.012019468] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314555.012715398] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314555.013931927] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62211 / 921600 +[sm-8] [INFO] [1782314555.014570261] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314555.015126286] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314555.015715351] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314555.165673081] [lasr_vision_reid]: Added face embedding for host, total samples: 10 +[sm-8] [INFO] [1782314555.179575569] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314555.180140045] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 9/10. +[sm-8] [INFO] [1782314555.180378783] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314555.444579116] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314555.445270774] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314555.480479272] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314555.480819168] [hri]: [detect_3d.py:response_handler:121] person at (0.54, -2.55, 0.63) +[sm-8] [INFO] [1782314555.481178165] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314555.481610157] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314555.482216839] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314555.510032456] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314555.510898227] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314555.512267242] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 62616 / 921600 +[sm-8] [INFO] [1782314555.512985160] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314555.513505153] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314555.514192166] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314555.665638634] [lasr_vision_reid]: Added face embedding for host, total samples: 11 +[sm-8] [INFO] [1782314555.679431338] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [INFO] [1782314555.679880324] [hri]: [hri_learn_faces.py:execute:116] Collected enough images for the guest. +[sm-8] [INFO] [1782314555.680122097] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314555.680360894] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314555.680582524] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_HOST' : 'succeeded' --> 'LOOK_TO_SEAT' +[sm-8] [INFO] [1782314555.681072600] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.311, -2.717, 0.500) +[sm-8] [INFO] [1782314555.681326976] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314555.682136453] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314556.710241428] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_TO_SEAT' : 'succeeded' --> 'SAY_SEAT_GUEST' +[sm-8] [INFO] [1782314556.710681805] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314556.711535364] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314564.968227024] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_SEAT_GUEST' : 'succeeded' --> 'WAIT_FOR_GUEST_TO_SEAT' +[sm-8] [INFO] [1782314564.968667033] [hri]: [wait.py:execute:21] Waiting for 5.0 seconds. +[sm-8] [INFO] [1782314569.992201243] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT_FOR_GUEST_TO_SEAT' : 'succeeded' --> 'RESET_HEAD_2' +[sm-8] [INFO] [1782314570.002005168] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314570.021230078] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314570.022501585] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314571.566319499] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314571.566619486] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_2' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314571.566826755] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314571.567038767] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SEAT_GUEST' : 'succeeded' --> 'CHECK' +[sm-8] [INFO] [1782314571.567450316] [hri]: [state_machine.py:check:166] 1 +[sm-8] [INFO] [1782314571.567748717] [hri]: [state_machine.py:check:179] Guest1: +[sm-8] [INFO] [1782314571.568049278] [hri]: [state_machine.py:check:182] name: Jeff +[sm-8] [INFO] [1782314571.571451600] [hri]: [state_machine.py:check:182] drink: vodka +[sm-8] [INFO] [1782314571.584610246] [hri]: [state_machine.py:check:182] detection: True +[sm-8] [INFO] [1782314571.584925616] [hri]: [state_machine.py:check:182] seating_detection: False +[sm-8] [INFO] [1782314571.585197664] [hri]: [state_machine.py:check:182] attributes: {'hair_color': 'black', 'hair_length': 'shoulderlength', 'glasses': True, 'hat': True, 'shirt_color': 'black'} +[sm-8] [INFO] [1782314571.585446828] [hri]: [state_machine.py:check:182] seated_point: None +[sm-8] [INFO] [1782314571.585677230] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK' : 'continue' --> 'GO_TO_DOOR_2' +[sm-8] [INFO] [1782314571.585900563] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PRE_NAV' +[sm-8] [INFO] [1782314571.586125167] [hri]: GIVING GOAL of pre_navigation +[sm-8] [INFO] [1782314571.586388365] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314571.587147191] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314574.353960816] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314574.354456297] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PRE_NAV' : 'succeeded' --> 'GO_TO_DOOR_POSE' +[sm-8] [INFO] [1782314574.355876319] [hri]: Navigating to goal: 0.9737232865224763 0.6644210227864706... +[sm-8] [INFO] [1782314607.562678143] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_DOOR_POSE' : 'succeeded' --> 'POST_NAV' +[sm-8] [INFO] [1782314607.562963137] [hri]: GIVING GOAL of post_navigation +[sm-8] [INFO] [1782314607.563289283] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314607.564177009] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314610.392871295] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314610.393222204] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'POST_NAV' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314610.393435646] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314610.393693100] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_DOOR_2' : 'succeeded' --> 'GREET_2' +[sm-8] [INFO] [1782314610.394035830] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'SAY_WAITING_FOR_GUEST' +[sm-8] [INFO] [1782314610.394465084] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314610.395485265] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314612.491857641] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_WAITING_FOR_GUEST' : 'succeeded' --> 'WAIT_FOR_GUEST' +[sm-8] [INFO] [1782314612.492140827] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314612.492373310] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314612.755997465] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314612.767400103] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314612.801482019] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314612.801824498] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314612.834870183] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314612.835211945] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314612.835479648] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314612.835870983] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314612.836122883] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314613.093548211] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314613.094469801] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314613.126164319] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314613.127143014] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314613.158980867] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314613.159816924] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314613.160071905] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314613.160523206] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314613.160851668] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314613.423880040] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314613.424614968] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314613.455050212] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314613.455384440] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314613.485907024] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314613.491844023] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314613.492124673] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314613.492495901] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314613.492757602] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314613.757863281] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314613.758587220] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314613.792455711] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314613.793543728] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314613.825532360] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314613.825823656] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314613.826065140] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314613.826391100] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314613.826613209] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314614.089753949] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314614.090593011] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314614.120556186] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314614.121484905] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314614.158097179] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314614.158396823] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314614.158626363] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314614.158992422] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314614.159231649] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314614.421599145] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314614.431313399] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314614.457898543] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314614.458233051] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314614.490328535] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314614.490661736] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314614.490941262] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314614.491319789] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314614.491566696] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314614.758040863] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314614.758757000] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314614.786672694] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314614.787014453] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314614.817258305] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314614.822547100] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314614.822819657] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314614.823170919] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314614.823382906] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314615.083418022] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314615.085124458] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314615.117362656] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314615.118511700] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314615.152734669] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314615.153067158] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314615.153297232] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314615.153631317] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314615.153905042] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314615.420116038] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314615.420811310] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314615.451398228] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314615.451774365] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314615.488860039] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314615.489655467] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314615.489901698] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314615.490230903] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314615.490458572] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314615.749858753] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314615.750517162] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314615.784102178] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314615.784454536] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314615.817629512] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314615.822867885] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314615.823116210] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314615.823444716] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314615.823682323] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314616.079624715] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314616.083442267] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314616.122107291] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314616.123362984] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314616.154706516] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314616.155010603] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314616.155253114] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314616.155645545] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314616.155874739] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314616.434346346] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314616.453078981] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314616.487011135] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314616.487340903] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314616.517401552] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314616.517688326] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314616.517924906] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314616.518260906] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314616.518488548] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314616.789391772] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314616.790229675] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314616.817998201] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314616.818931998] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314616.852569584] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314616.852913782] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314616.853189676] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314616.853539422] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314616.853768681] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314617.115969336] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314617.116746576] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314617.151316743] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314617.151680014] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314617.182495394] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314617.183499047] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314617.183786626] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314617.184223991] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314617.184482395] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314617.450570588] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314617.451274751] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314617.482628591] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314617.483055834] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314617.518912568] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314617.519229125] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314617.519487155] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314617.519829147] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314617.520063944] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314617.775561449] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314617.779607287] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314617.795024089] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314617.795372104] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314617.833352033] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314617.848225110] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314617.848503973] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314617.848890256] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314617.849127273] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314618.120092509] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314618.121780198] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314618.149226029] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314618.149586371] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314618.184414349] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314618.184700547] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314618.184949628] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314618.185291852] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314618.185512999] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314618.446060575] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314618.446742502] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314618.481574541] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314618.481950425] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314618.514388108] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314618.514682665] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314618.514949374] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314618.515348746] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314618.515623847] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314618.778197300] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314618.778911249] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314618.794967715] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314618.795320889] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314618.828485483] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314618.838118529] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314618.841890970] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314618.848343904] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314618.848617335] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314619.112584876] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314619.116525354] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314619.147469081] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314619.147823261] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314619.184933038] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314619.185231301] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314619.185460081] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314619.185853372] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314619.186105017] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314619.448140956] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314619.448960097] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314619.483755354] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314619.484103739] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314619.514512091] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314619.514852372] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314619.515116574] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314619.515501462] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314619.515782410] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314619.781385451] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314619.785131945] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314619.813735571] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314619.814169096] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314619.837452607] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314619.850833169] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314619.851251892] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314619.851694580] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314619.852031043] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314620.110055682] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314620.112117440] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314620.145782763] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314620.150279400] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314620.182855983] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314620.183374105] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314620.183724837] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314620.184187102] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314620.184487595] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314620.448214079] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314620.448871475] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314620.478870249] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314620.479289833] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314620.514866022] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314620.516479542] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314620.516759761] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314620.517141346] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314620.517412609] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314620.776235989] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314620.776948773] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314620.792351850] [hri]: [detect_3d.py:response_handler:119] Got 0 detections +[sm-8] [INFO] [1782314620.792816042] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314620.822033588] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314620.840884186] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314620.848199388] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314620.848719667] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314620.849017984] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314621.119091956] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314621.121350361] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314621.145923740] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314621.146249983] [hri]: [detect_3d.py:response_handler:121] person at (-0.65, 0.15, 1.57) +[sm-8] [INFO] [1782314621.146614617] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314621.179656822] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.6464012746261173, y:0.15432992339275858, z:1.565025196475478 +[sm-8] [INFO] [1782314621.180288658] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314621.180534013] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314621.180740557] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314621.181084390] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314621.181290785] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314621.437975990] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314621.443169897] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314621.481112955] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314621.482933278] [hri]: [detect_3d.py:response_handler:121] person at (-0.54, 0.30, 1.17) +[sm-8] [INFO] [1782314621.483203876] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314621.514532009] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.5372319853007532, y:0.29707506793757477, z:1.174778033724748 +[sm-8] [INFO] [1782314621.515096516] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314621.515321018] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314621.515532804] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314621.515874823] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314621.516138557] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314621.790771835] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314621.814084354] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314621.846138532] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314621.846499860] [hri]: [detect_3d.py:response_handler:121] person at (-0.52, 0.46, 1.17) +[sm-8] [INFO] [1782314621.847854662] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314621.885378961] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.5208812876911361, y:0.4642845707176593, z:1.1666597692807943 +[sm-8] [INFO] [1782314621.885965014] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314621.886348844] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314621.886724559] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314621.887172656] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314621.887498996] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314622.143086212] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314622.143819706] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314622.176101215] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314622.176426030] [hri]: [detect_3d.py:response_handler:121] person at (-0.45, 0.72, 1.15) +[sm-8] [INFO] [1782314622.177398352] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314622.215859877] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.45435553612008295, y:0.7172188699020331, z:1.1491240598070962 +[sm-8] [INFO] [1782314622.216438995] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314622.216669661] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314622.216922813] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314622.217255985] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314622.217488851] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314622.483431521] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314622.503774556] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314622.556307246] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314622.556671450] [hri]: [detect_3d.py:response_handler:121] person at (-0.35, 0.90, 1.17) +[sm-8] [INFO] [1782314622.562837817] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314622.588145237] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.35495940375451907, y:0.9000778754817584, z:1.171071568265971 +[sm-8] [INFO] [1782314622.588705891] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314622.589035045] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314622.589266265] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314622.589606542] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314622.589908116] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314622.844444604] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314622.845102348] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314622.878556866] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314622.878891990] [hri]: [detect_3d.py:response_handler:121] person at (-0.17, 1.08, 1.21) +[sm-8] [INFO] [1782314622.879190235] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314622.913053252] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.1748315664970238, y:1.0826080554759048, z:1.2068920388641595 +[sm-8] [INFO] [1782314622.913645204] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314622.913895901] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314622.914130810] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314622.914447568] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314622.914664046] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314623.172123847] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314623.172762733] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314623.213830825] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314623.214196607] [hri]: [detect_3d.py:response_handler:121] person at (-0.16, 1.16, 1.13) +[sm-8] [INFO] [1782314623.215293735] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314623.244829317] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.15810316582080508, y:1.161360066404817, z:1.125632086517693 +[sm-8] [INFO] [1782314623.245411540] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314623.245644239] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314623.245910412] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314623.246261969] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314623.246529113] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314623.508535120] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314623.509351899] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314623.541094579] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314623.541433628] [hri]: [detect_3d.py:response_handler:121] person at (-0.26, 1.28, 1.04) +[sm-8] [INFO] [1782314623.542497492] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314623.577306400] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.25927891596940467, y:1.2756353339284188, z:1.0440256581109146 +[sm-8] [INFO] [1782314623.577779592] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314623.578047023] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314623.578268012] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314623.578578505] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314623.578818245] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314623.838814637] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314623.840333868] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314623.878932250] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314623.879281637] [hri]: [detect_3d.py:response_handler:121] person at (-0.29, 1.31, 1.00) +[sm-8] [INFO] [1782314623.879642678] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314623.911423514] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.29146107594566817, y:1.3076744003758705, z:1.0034226703050166 +[sm-8] [INFO] [1782314623.911901735] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314623.912153048] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314623.912373547] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314623.912726222] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314623.912962639] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314624.173200739] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314624.174913770] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314624.208538672] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314624.208858590] [hri]: [detect_3d.py:response_handler:121] person at (-0.30, 1.31, 1.00) +[sm-8] [INFO] [1782314624.209193724] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314624.243985839] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.3034046010289967, y:1.3090350336714904, z:0.9968509522024498 +[sm-8] [INFO] [1782314624.244497457] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314624.244718805] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314624.244930462] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314624.245263289] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314624.245490381] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314624.505268256] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314624.505860073] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314624.540909623] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314624.541309838] [hri]: [detect_3d.py:response_handler:121] person at (-0.29, 1.29, 1.03) +[sm-8] [INFO] [1782314624.541650458] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314624.572593460] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.2923297479902547, y:1.2870572972806351, z:1.0254125963179312 +[sm-8] [INFO] [1782314624.573157004] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314624.573386240] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314624.573611420] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314624.573962675] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314624.574242375] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314624.836921225] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314624.837570789] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314624.875900708] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314624.876309513] [hri]: [detect_3d.py:response_handler:121] person at (-0.27, 1.22, 1.16) +[sm-8] [INFO] [1782314624.876649918] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314624.909848948] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.2682151006738669, y:1.2248088549915073, z:1.1565833118281894 +[sm-8] [INFO] [1782314624.910377474] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314624.910617219] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314624.910854681] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314624.911257371] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314624.911496111] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314625.176110883] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314625.203451837] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314625.236945801] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314625.237284534] [hri]: [detect_3d.py:response_handler:121] person at (-0.31, 1.12, 1.14) +[sm-8] [INFO] [1782314625.237587011] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314625.276009927] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.3104917192148795, y:1.1234991110187118, z:1.1433096621821344 +[sm-8] [INFO] [1782314625.276471037] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314625.276695577] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314625.276899995] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314625.277195010] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314625.277415632] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314625.545619580] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314625.559962289] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314625.604341107] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314625.609820846] [hri]: [detect_3d.py:response_handler:121] person at (-0.26, 1.07, 1.16) +[sm-8] [INFO] [1782314625.610116907] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314625.639906996] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.2603595509479777, y:1.0654611268582719, z:1.1646785421613097 +[sm-8] [INFO] [1782314625.640444592] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314625.640690806] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314625.640954503] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314625.641325668] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'not_done' --> 'DETECT_PEOPLE_3D' +[sm-8] [INFO] [1782314625.641580851] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314625.910225190] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314625.911134877] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314625.942298576] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314625.942609372] [hri]: [detect_3d.py:response_handler:121] person at (-0.13, 0.98, 1.19) +[sm-8] [INFO] [1782314625.942962975] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314625.976003545] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:-0.1330882746383899, y:0.9819475239175945, z:1.1931859943726002 +[sm-8] [INFO] [1782314625.976838846] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314625.977076217] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314625.977345360] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_PEOPLE_3D' : 'succeeded' --> 'CHECK_FOR_PERSON' +[sm-8] [INFO] [1782314625.977792258] [hri]: [wait_for_person_in_area.py:execute:22] Found 1 people in wait area. +[sm-8] [INFO] [1782314625.978044179] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_FOR_PERSON' : 'done' --> 'succeeded' +[sm-8] [INFO] [1782314625.984304254] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314625.984529170] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT_FOR_GUEST' : 'succeeded' --> 'GET_PERSON_POINT' +[sm-8] [INFO] [1782314625.984923047] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_PERSON_POINT' : 'succeeded' --> 'LOOK_AND_GREET' +[sm-8] [INFO] [1782314626.006232781] [hri]: [action_state.py:execute:165] Waiting for action '/lasr_vision_eye_tracker/track_eyes' +[sm-8] [INFO] [1782314626.007669062] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'GREET_AND_ASK_GUEST' +[sm-8] [INFO] [1782314626.008312995] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'SAY' +[sm-8] [INFO] [1782314626.008460545] [hri]: [action_state.py:execute:189] Sending goal to action '/lasr_vision_eye_tracker/track_eyes' +[sm-8] [INFO] [1782314626.008937539] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[eye_tracker_action_server-5] [INFO] [1782314626.009616355] [eye_tracker_action_server]: Received eye tracker goal +[sm-8] [INFO] [1782314626.009694876] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[eye_tracker_action_server-5] [INFO] [1782314626.010668725] [eye_tracker_action_server]: Beginning eye tracking... +[sm-8] [INFO] [1782314632.215844471] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY' : 'succeeded' --> 'LISTEN' +[sm-8] [INFO] [1782314632.216400512] [hri]: [action_state.py:execute:165] Waiting for action 'transcribe_speech' +[sm-8] [INFO] [1782314632.226896901] [hri]: [action_state.py:execute:189] Sending goal to action 'transcribe_speech' +[transcribe_microphone_server-6] [INFO] [1782314632.228604296] [whisper_mic_server]: Request Received +[transcribe_microphone_server-6] [INFO] [1782314640.191914966] [whisper_mic_server]: Transcribing phrase with Whisper... +[transcribe_microphone_server-6] [INFO] [1782314640.730958380] [whisper_mic_server]: Transcription finished! +[transcribe_microphone_server-6] [INFO] [1782314640.731307259] [whisper_mic_server]: Time taken: 0.54s +[transcribe_microphone_server-6] [INFO] [1782314640.731788377] [whisper_mic_server]: Transcribed phrase: Hi Tiago, my name is Fadi and my favourite drink is peach iced tea. +[transcribe_microphone_server-6] [INFO] [1782314640.732186534] [whisper_mic_server]: transcribe_speech has succeeded +[sm-8] [INFO] [1782314640.759094330] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LISTEN' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314640.759452915] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314640.759803030] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GREET_AND_ASK_GUEST' : 'succeeded' --> 'GET_NAME_DRINK_FACE' +[sm-8] [INFO] [1782314640.763034479] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_3D' +[sm-8] [INFO] [1782314640.763291447] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'INITIALISE_DETECTION_FLAG' +[sm-8] [INFO] [1782314640.763973799] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PARSE_NAME' +[sm-8] [INFO] [1782314640.764201501] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'INITIALISE_DETECTION_FLAG' : 'succeeded' --> 'GET_GUEST_ATTRIBUTES' +[sm-8] [INFO] [1782314640.764860127] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'GET_IMAGE' +[sm-8] [INFO] [1782314640.765480709] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_IMAGE' : 'succeeded' --> 'GET_ATTRIBUTES' +[sm-8] [INFO] [1782314640.765921796] [hri]: [service_state.py:execute:138] Waiting for service '/hri_task/query_llm' +[sm-8] [INFO] [1782314640.766940554] [hri]: [service_state.py:execute:138] Waiting for service '/vlm/describe_people' +[sm-8] [INFO] [1782314640.767577317] [hri]: [service_state.py:execute:153] Sending request to service '/hri_task/query_llm' +[sm-8] [INFO] [1782314640.767859484] [hri]: [service_state.py:execute:153] Sending request to service '/vlm/describe_people' +[hri_task_service-7] [INFO] [1782314640.768335203] [llm]: Received query: Hi Tiago, my name is Fadi and my favourite drink is peach iced tea., and task is name +[vlm_service-4] [INFO] [1782314640.773548046] [lasr_vlm_service]: Received request to describe person +[sm-8] [INFO] [1782314641.027216269] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314641.029511642] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314641.060096178] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314641.060628686] [hri]: [detect_3d.py:response_handler:121] person at (-0.17, 0.79, 1.39) +[sm-8] [INFO] [1782314641.061055480] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314641.066998423] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314641.067885452] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314641.104050516] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314641.125845504] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314641.127656898] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 106146 / 921600 +[sm-8] [INFO] [1782314641.133732044] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314641.134569782] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314641.135330090] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314641.315358363] [lasr_vision_reid]: Added face embedding for guest2, total samples: 1 +[sm-8] [INFO] [1782314641.337015615] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314641.362595092] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 0/10. +[sm-8] [INFO] [1782314641.363999794] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314641.635511183] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314641.656937329] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314641.694478427] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314641.694852226] [hri]: [detect_3d.py:response_handler:121] person at (-0.18, 0.79, 1.37) +[sm-8] [INFO] [1782314641.695228145] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314641.695696219] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314641.696436935] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314641.725318604] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314641.726133535] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314641.727903386] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 107505 / 921600 +[sm-8] [INFO] [1782314641.735684924] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314641.736426123] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314641.761942493] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314641.935106224] [lasr_vision_reid]: Added face embedding for guest2, total samples: 2 +[sm-8] [INFO] [1782314641.958701824] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314641.959623833] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 1/10. +[sm-8] [INFO] [1782314641.960103136] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314642.227640824] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314642.229840015] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314642.257228103] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314642.257595778] [hri]: [detect_3d.py:response_handler:121] person at (-0.18, 0.77, 1.38) +[sm-8] [INFO] [1782314642.257959462] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314642.259973153] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314642.260572495] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314642.293681643] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314642.294373171] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314642.295881646] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 108780 / 921600 +[sm-8] [INFO] [1782314642.296686453] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314642.297248882] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314642.297872691] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314642.470314206] [lasr_vision_reid]: Added face embedding for guest2, total samples: 3 +[sm-8] [INFO] [1782314642.491780612] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314642.492677364] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 2/10. +[sm-8] [INFO] [1782314642.493065699] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314642.780444297] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314642.793430607] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314642.833409543] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314642.833790491] [hri]: [detect_3d.py:response_handler:121] person at (-0.19, 0.75, 1.38) +[sm-8] [INFO] [1782314642.834164870] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314642.846151387] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314642.856584224] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314642.904823844] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314642.924009978] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314642.925482188] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 111084 / 921600 +[sm-8] [INFO] [1782314642.926386736] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314642.927078963] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314642.927752650] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314643.105883871] [lasr_vision_reid]: Added face embedding for guest2, total samples: 4 +[sm-8] [INFO] [1782314643.128040037] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314643.160042660] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 3/10. +[sm-8] [INFO] [1782314643.160458255] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314643.427546560] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314643.430225503] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314643.461040207] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314643.461423829] [hri]: [detect_3d.py:response_handler:121] person at (-0.18, 0.72, 1.38) +[sm-8] [INFO] [1782314643.461832814] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314643.462306725] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314643.462943621] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314643.523054150] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314643.524284827] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314643.525947807] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 111579 / 921600 +[sm-8] [INFO] [1782314643.526733831] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314643.527395513] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314643.560908592] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314643.737798542] [lasr_vision_reid]: Added face embedding for guest2, total samples: 5 +[sm-8] [INFO] [1782314643.757536708] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314643.759735766] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 4/10. +[sm-8] [INFO] [1782314643.760114387] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314644.030815995] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314644.034965670] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314644.062114260] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314644.062440605] [hri]: [detect_3d.py:response_handler:121] person at (-0.16, 0.76, 1.38) +[sm-8] [INFO] [1782314644.074959294] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314644.092902796] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314644.093687633] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314644.122049314] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314644.122837495] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314644.124324484] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 109299 / 921600 +[sm-8] [INFO] [1782314644.125164453] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314644.125772846] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314644.126418401] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314644.304898204] [lasr_vision_reid]: Added face embedding for guest2, total samples: 6 +[sm-8] [INFO] [1782314644.327541529] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314644.346301422] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 5/10. +[sm-8] [INFO] [1782314644.355107862] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314644.629694267] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314644.630378191] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314644.660291751] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314644.660637472] [hri]: [detect_3d.py:response_handler:121] person at (-0.18, 0.97, 1.37) +[sm-8] [INFO] [1782314644.661014447] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314644.661455598] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314644.683859074] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314644.724816102] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314644.725647653] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314644.726990837] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 96336 / 921600 +[sm-8] [INFO] [1782314644.727791903] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314644.741324149] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314644.761804498] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314644.939130543] [lasr_vision_reid]: Added face embedding for guest2, total samples: 7 +[sm-8] [INFO] [1782314644.955904118] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314644.956472573] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 6/10. +[sm-8] [INFO] [1782314644.956773960] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314645.230938225] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314645.254534109] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314645.292779954] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314645.293160754] [hri]: [detect_3d.py:response_handler:121] person at (-0.20, 1.17, 1.39) +[sm-8] [INFO] [1782314645.293538688] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314645.294012813] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314645.294693636] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314645.361345284] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314645.362360292] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314645.363900624] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 88296 / 921600 +[sm-8] [INFO] [1782314645.386457892] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314645.387936129] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314645.390485296] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314645.574634274] [lasr_vision_reid]: Added face embedding for guest2, total samples: 8 +[sm-8] [INFO] [1782314645.592386947] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314645.593303406] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 7/10. +[sm-8] [INFO] [1782314645.593735288] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314645.851882388] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314645.852605759] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[yolo_service_node-2] /home/rexy/fadi_ws/install/lasr_vision_yolo/share/lasr_vision_yolo/venv/lib/python3.10/site-packages/numpy/core/fromnumeric.py:3464: RuntimeWarning: Mean of empty slice. +[yolo_service_node-2] return _methods._mean(a, axis=axis, dtype=dtype, +[yolo_service_node-2] /home/rexy/fadi_ws/install/lasr_vision_yolo/share/lasr_vision_yolo/venv/lib/python3.10/site-packages/numpy/core/_methods.py:184: RuntimeWarning: invalid value encountered in divide +[yolo_service_node-2] ret = um.true_divide( +[sm-8] [INFO] [1782314645.895809165] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314645.896219869] [hri]: [detect_3d.py:response_handler:121] person at (nan, nan, nan) +[sm-8] [INFO] [1782314645.909032528] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314645.920106021] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314645.921041016] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314645.960469010] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314645.961383250] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314645.962839321] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 30339 / 921600 +[sm-8] [INFO] [1782314645.984285270] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314645.985203467] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314645.985962058] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314646.174642651] [lasr_vision_reid]: Added face embedding for guest2, total samples: 9 +[sm-8] [INFO] [1782314646.194094018] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314646.194819691] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 8/10. +[sm-8] [INFO] [1782314646.219912825] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314646.492885375] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314646.493660488] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314646.519584126] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314646.519918717] [hri]: [detect_3d.py:response_handler:121] person at (nan, nan, nan) +[sm-8] [INFO] [1782314646.520450494] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314646.521001274] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314646.521661406] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314646.554404791] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314646.819729031] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314646.820456296] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314646.857068981] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314646.857649577] [hri]: [detect_3d.py:response_handler:121] person at (nan, nan, nan) +[sm-8] [INFO] [1782314646.858204676] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314646.862487821] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314646.887136061] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314646.921386965] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314647.182708336] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314647.183580625] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314647.251088038] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314647.252002330] [hri]: [detect_3d.py:response_handler:121] person at (-1.29, 1.68, 2.06) +[sm-8] [INFO] [1782314647.252421004] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314647.252856831] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314647.253516791] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314647.291940778] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314647.553170036] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314647.553915282] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314647.625244663] [hri]: [detect_3d.py:response_handler:119] Got 1 detections +[sm-8] [INFO] [1782314647.625869081] [hri]: [detect_3d.py:response_handler:121] person at (-0.19, 0.92, 1.59) +[sm-8] [INFO] [1782314647.626356016] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314647.626929733] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314647.650370752] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314647.717075185] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314647.717870332] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314647.719822945] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 28014 / 921600 +[sm-8] [INFO] [1782314647.720689788] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314647.721526850] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314647.722328403] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314647.893497942] [lasr_vision_reid]: Added face embedding for guest2, total samples: 10 +[sm-8] [INFO] [1782314647.917140610] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [WARN] [1782314647.917791233] [hri]: [hri_learn_faces.py:execute:120] Not enough images collected for the guest: 9/10. +[sm-8] [INFO] [1782314647.929642377] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'failed' --> 'DETECT_3D' +[sm-8] [INFO] [1782314648.184694675] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314648.185605725] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314648.216659708] [hri]: [detect_3d.py:response_handler:119] Got 2 detections +[sm-8] [INFO] [1782314648.217060069] [hri]: [detect_3d.py:response_handler:121] person at (-0.20, 0.78, 1.45) +[sm-8] [INFO] [1782314648.217380633] [hri]: [detect_3d.py:response_handler:121] person at (-2.14, -0.38, 1.43) +[sm-8] [INFO] [1782314648.217766119] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_3D' : 'succeeded' --> 'CHECK_EYES' +[sm-8] [INFO] [1782314648.247883190] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect_pose' +[sm-8] [INFO] [1782314648.251473642] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect_pose' +[sm-8] [INFO] [1782314648.292577743] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_EYES' : 'succeeded' --> 'CROP_IMAGE_3D' +[sm-8] [INFO] [1782314648.321779520] [hri]: [crop_image_3d.py:execute:162] Processing person: +[sm-8] [INFO] [1782314648.323391656] [hri]: [crop_image_3d.py:execute:174] stencil filled pixels: 7548 / 921600 +[sm-8] [INFO] [1782314648.324375899] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CROP_IMAGE_3D' : 'succeeded' --> 'LEARN_FACE' +[sm-8] [INFO] [1782314648.326526819] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/add_face' +[sm-8] [INFO] [1782314648.355914734] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/add_face' +[service-3] [INFO] [1782314648.536055419] [lasr_vision_reid]: Added face embedding for guest2, total samples: 11 +[sm-8] [INFO] [1782314648.552340355] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LEARN_FACE' : 'succeeded' --> 'CHECK_DONE' +[sm-8] [INFO] [1782314648.553296022] [hri]: [hri_learn_faces.py:execute:116] Collected enough images for the guest. +[sm-8] [INFO] [1782314648.553790855] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK_DONE' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314648.554201791] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[vlm_service-4] [INFO] [1782314655.857521850] [lasr_vlm_service]: VLM result: {'hair_color': ['brown'], 'hair_length': ['short'], 'glasses': [True], 'hat': [True], 'shirt color': ['beige']} +[sm-8] [INFO] [1782314655.877937615] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_ATTRIBUTES' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314655.878280900] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314655.878540807] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_GUEST_ATTRIBUTES' : 'succeeded' --> 'HANDLE_GUEST_ATTRIBUTES' +[sm-8] [INFO] [1782314655.878996857] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'HANDLE_GUEST_ATTRIBUTES' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314655.879245838] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[hri_task_service-7] [INFO] [1782314665.431810703] [llm]: LLM output: Name: Fadi +[hri_task_service-7] [INFO] [1782314665.432142688] [llm]: Returning response: lasr_llm_interfaces.msg.ReceptionistResponse(name='Fadi', favourite_drink='', interests='', interest_commonality='', llm_response='Name: Fadi') +[sm-8] [INFO] [1782314665.458876787] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PARSE_NAME' : 'succeeded' --> 'PARSE_DRINK' +[sm-8] [INFO] [1782314665.460001435] [hri]: [service_state.py:execute:138] Waiting for service '/hri_task/query_llm' +[sm-8] [INFO] [1782314665.460782251] [hri]: [service_state.py:execute:153] Sending request to service '/hri_task/query_llm' +[hri_task_service-7] [INFO] [1782314665.461716075] [llm]: Received query: Hi Tiago, my name is Fadi and my favourite drink is peach iced tea., and task is drink +[hri_task_service-7] [INFO] [1782314666.038225424] [llm]: LLM output: Favourite drink: peach iced tea +[hri_task_service-7] [INFO] [1782314666.038517132] [llm]: Returning response: lasr_llm_interfaces.msg.ReceptionistResponse(name='', favourite_drink='peach iced tea', interests='', interest_commonality='', llm_response='Favourite drink: peach iced tea') +[sm-8] [INFO] [1782314666.057928853] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PARSE_DRINK' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314666.058403762] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314666.059198456] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_NAME_DRINK_FACE' : 'succeeded' --> 'GET_ATTRIBUTE_STR' +[sm-8] [INFO] [1782314666.059942917] [hri]: [greet.py:get_guest1_attributes:277] Attribute string: Hello Fadi, welcome to the party! Jeff has already arrived and is sitting down. They have black coloured hair. have shoulderlength hair. are wearing glasses. are wearing a hat. are wearing a black coloured shirt. +[sm-8] [INFO] [1782314666.060292518] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_ATTRIBUTE_STR' : 'succeeded' --> 'SAY_ATTRIBUTE' +[sm-8] [INFO] [1782314666.060745718] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314666.061662393] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314684.473998082] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_ATTRIBUTE' : 'succeeded' --> 'STOP_EYE_TRACKING_2' +[sm-8] [INFO] [1782314684.501283235] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'STOP_EYE_TRACKING_2' : 'succeeded' --> 'WAIT' +[sm-8] [INFO] [1782314684.501920333] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314684.971959524] [hri]: [eye_tracker.py:handle_feedback:24] Cancelling current eye tracker +[eye_tracker_action_server-5] [INFO] [1782314684.973451125] [eye_tracker_action_server]: Eye Tracker Action Server cancellation requested +[sm-8] [INFO] [1782314685.033959461] [hri]: [state.hpp:cancel_state:173] Canceling state 'StartEyeTracker' +[eye_tracker_action_server-5] [INFO] [1782314685.425461999] [eye_tracker_action_server]: Eye Tracker Action Server canceled, stopping tracking. +[eye_tracker_action_server-5] [INFO] [1782314685.935831601] [eye_tracker_action_server]: Canceled EYE TRACKER +[sm-8] [INFO] [1782314686.517505703] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT' : 'succeeded' --> 'GRAB_BAG' +[sm-8] [INFO] [1782314686.519402372] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'CLEAR_OCTOMAP' +[sm-8] [INFO] [1782314686.519810019] [hri]: [service_state.py:execute:138] Waiting for service '/clear_octomap' +[sm-8] [INFO] [1782314686.520533403] [hri]: [service_state.py:execute:153] Sending request to service '/clear_octomap' +[sm-8] [INFO] [1782314686.523651385] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CLEAR_OCTOMAP' : 'succeeded' --> 'LOOK_AROUND' +[sm-8] [INFO] [1782314686.523959716] [hri]: GIVING GOAL of head_tour +[sm-8] [INFO] [1782314686.524276042] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314686.525160378] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314701.553988281] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314701.554296896] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AROUND' : 'succeeded' --> 'SAY_REACH_ARM' +[sm-8] [INFO] [1782314701.554610100] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314701.555497250] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314707.880800984] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_REACH_ARM' : 'succeeded' --> 'REACH_ARM' +[sm-8] [INFO] [1782314707.883639393] [hri]: GIVING GOAL of reach_arm_vertical_gripper +[sm-8] [INFO] [1782314707.885310405] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314707.886193264] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314711.246385374] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314711.246722711] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'REACH_ARM' : 'succeeded' --> 'OPEN_GRIPPER' +[sm-8] [INFO] [1782314711.246968857] [hri]: GIVING GOAL of open_gripper +[sm-8] [INFO] [1782314711.247241461] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314711.248144338] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314712.275580385] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314712.275904699] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'OPEN_GRIPPER' : 'succeeded' --> 'SAY_PLACE' +[sm-8] [INFO] [1782314712.276231321] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314712.277147819] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314718.162399897] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_PLACE' : 'succeeded' --> 'WAIT_5' +[sm-8] [INFO] [1782314718.162836441] [hri]: [wait.py:execute:21] Waiting for 5 seconds. +[sm-8] [INFO] [1782314723.168260444] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT_5' : 'succeeded' --> 'CLOSE_HALF_GRIPPER' +[sm-8] [INFO] [1782314723.169841727] [hri]: GIVING GOAL of close_half +[sm-8] [INFO] [1782314723.170179995] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314723.178340560] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314723.722560863] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314723.722911730] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CLOSE_HALF_GRIPPER' : 'succeeded' --> 'FOLD_ARM' +[sm-8] [INFO] [1782314723.723177988] [hri]: GIVING GOAL of cml_arm_away +[sm-8] [INFO] [1782314723.723469234] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314723.724587364] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314726.614175379] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314726.615601023] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FOLD_ARM' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314726.615828159] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314726.616027403] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GRAB_BAG' : 'succeeded' --> 'SAY_WELCOME_2' +[sm-8] [INFO] [1782314726.616321796] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314726.617068211] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314729.315162849] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_WELCOME_2' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314729.315512520] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314729.316268275] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AND_GREET' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314729.316692914] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314729.317070993] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GREET_2' : 'succeeded' --> 'GUIDE_TO_SEAT_2' +[sm-8] [INFO] [1782314729.317916445] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'PRE_NAV' +[sm-8] [INFO] [1782314729.319184170] [hri]: GIVING GOAL of pre_navigation +[sm-8] [INFO] [1782314729.319479600] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314729.320314686] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314731.212686057] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314731.221484874] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PRE_NAV' : 'succeeded' --> 'GO_TO_SEAT_POSE' +[sm-8] [INFO] [1782314731.223214705] [hri]: Navigating to goal: 0.9241805428867255 -0.37066992922907555... +[sm-8] [INFO] [1782314748.955732591] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GO_TO_SEAT_POSE' : 'succeeded' --> 'POST_NAV' +[sm-8] [INFO] [1782314748.956056080] [hri]: GIVING GOAL of post_navigation +[sm-8] [INFO] [1782314748.956443263] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314748.957414900] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314750.740932151] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314750.745536226] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'POST_NAV' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314750.748705274] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314750.749084035] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GUIDE_TO_SEAT_2' : 'succeeded' --> 'SEAT_GUEST_2' +[sm-8] [INFO] [1782314750.749472973] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'SAY_FINDING_SEAT' +[sm-8] [INFO] [1782314750.749912040] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314750.750797355] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314753.146213758] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_FINDING_SEAT' : 'succeeded' --> 'RESET_HEAD_1' +[sm-8] [INFO] [1782314753.146492001] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314753.146811020] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314753.148847828] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314754.682586911] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314754.684283182] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_1' : 'succeeded' --> 'DETECT_ALL_PEOPLE_SEATS' +[sm-8] [INFO] [1782314754.684577775] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'INIT_BLACKBOARD' +[sm-8] [INFO] [1782314754.687567849] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'INIT_BLACKBOARD' : 'succeeded' --> 'CALCULATE_SWEEP_POINTS' +[sm-8] [INFO] [1782314754.687961413] [hri]: [detect_all_in_polygon.py:_calculate_sweep_points:300] Waiting for camera info and TF to map frame... +[sm-8] [INFO] [1782314754.750947396] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 0.28%, score: 0.01 +[sm-8] [INFO] [1782314754.752294906] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 0.56%, score: 0.01 +[sm-8] [INFO] [1782314754.753373424] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 0.83%, score: 0.01 +[sm-8] [INFO] [1782314754.754394250] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 1.09%, score: 0.01 +[sm-8] [INFO] [1782314754.755421138] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 1.34%, score: 0.01 +[sm-8] [INFO] [1782314754.756368926] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 1.58%, score: 0.01 +[sm-8] [INFO] [1782314754.784948514] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 1.77%, score: 0.00 +[sm-8] [INFO] [1782314754.785970891] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 1.96%, score: 0.00 +[sm-8] [INFO] [1782314754.786667015] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 2.14%, score: 0.00 +[sm-8] [INFO] [1782314754.787248435] [hri]: [detect_all_in_polygon.py:_greedy_coverage_min_overlap:287] Selected new footprint, total coverage: 2.32%, score: 0.00 +[sm-8] [INFO] [1782314754.835656263] [hri]: [detect_all_in_polygon.py:_calculate_sweep_points:349] Calculated 10 sweep points. +[sm-8] [INFO] [1782314754.838323630] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CALCULATE_SWEEP_POINTS' : 'succeeded' --> 'LOOK_AND_DETECT' +[sm-8] [INFO] [1782314754.846469617] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314754.847010351] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 0 +[sm-8] [INFO] [1782314754.847464260] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314754.848007332] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.216, -1.923, 0.700) +[sm-8] [INFO] [1782314754.849213064] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314754.851233035] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314755.909084546] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'succeeded' --> 'SLEEP' +[sm-8] [INFO] [1782314755.909482282] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314757.915090574] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314757.915474249] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314758.166634037] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314758.167445147] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314758.211987496] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314758.212316426] [hri]: [detect_3d.py:response_handler:121] person at (0.62, -2.73, 0.58) +[sm-8] [INFO] [1782314758.212657401] [hri]: [detect_3d.py:response_handler:121] chair at (-0.24, -1.67, 0.50) +[sm-8] [INFO] [1782314758.212978355] [hri]: [detect_3d.py:response_handler:121] person at (0.20, -2.30, 0.50) +[sm-8] [INFO] [1782314758.213305828] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314758.247570967] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.6193406740954449, y:-2.730382238197708, z:0.5803075375904453 +[sm-8] [INFO] [1782314758.248074858] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.2352875350233149, y:-1.6654367004297261, z:0.49727366850470844 +[sm-8] [INFO] [1782314758.248481484] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.2029386573330394, y:-2.301679510603871, z:0.5039309202759044 +[sm-8] [INFO] [1782314758.249100691] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314758.249346548] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314758.249602468] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314758.250195267] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314758.250377498] [hri]: Detected objects: +[sm-8] [INFO] [1782314758.251488873] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314758.271567699] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314758.274398002] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314758.274694105] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314758.275169621] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 1 +[sm-8] [INFO] [1782314758.275509434] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314758.275996053] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(-0.008, -2.560, 0.700) +[sm-8] [INFO] [1782314758.276281546] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314758.277095874] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [WARN] [1782314763.294854306] [hri]: [action_state.py:execute:205] Timeout reached while waiting for response from action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314763.295188226] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'timeout' --> 'SLEEP' +[sm-8] [INFO] [1782314763.295584694] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314765.299067420] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314765.299400201] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314765.556154189] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314765.556795988] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314765.603180226] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314765.603503869] [hri]: [detect_3d.py:response_handler:121] chair at (-0.19, -1.75, 0.47) +[sm-8] [INFO] [1782314765.603839186] [hri]: [detect_3d.py:response_handler:121] person at (0.61, -2.76, 0.59) +[sm-8] [INFO] [1782314765.604154936] [hri]: [detect_3d.py:response_handler:121] person at (0.13, -2.49, 0.68) +[sm-8] [INFO] [1782314765.604465094] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314765.636939270] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.19499963590159608, y:-1.7524832865094861, z:0.46965134003622877 +[sm-8] [INFO] [1782314765.637427294] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.6069791540043993, y:-2.7629716449820894, z:0.5906246529624255 +[sm-8] [INFO] [1782314765.637828198] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.13489937047857548, y:-2.4945818158541537, z:0.6807242030588793 +[sm-8] [INFO] [1782314765.660340836] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314765.662391909] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314765.664804083] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314765.666551218] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314765.666859775] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314765.667128885] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314765.667419116] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314765.667593680] [hri]: Detected objects: +[sm-8] [INFO] [1782314765.667813484] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314765.667993971] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314765.668166641] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314765.668402776] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314765.669918965] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 2 +[sm-8] [INFO] [1782314765.670293523] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314765.670738324] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.435, -1.724, 0.700) +[sm-8] [INFO] [1782314765.671044344] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314765.671859244] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314765.695904661] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314765.696788869] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314767.699216268] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314767.699632015] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314767.951437592] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314767.952320347] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314767.990904932] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314767.991306505] [hri]: [detect_3d.py:response_handler:121] chair at (-0.12, -1.77, 0.45) +[sm-8] [INFO] [1782314767.991718101] [hri]: [detect_3d.py:response_handler:121] person at (0.62, -2.71, 0.57) +[sm-8] [INFO] [1782314767.992101609] [hri]: [detect_3d.py:response_handler:121] person at (0.22, -2.26, 0.48) +[sm-8] [INFO] [1782314767.992431474] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314768.025748497] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.11879501187750585, y:-1.7688946509903127, z:0.45339158872571383 +[sm-8] [INFO] [1782314768.026240569] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.622078968422792, y:-2.711968346563429, z:0.5733567938350658 +[sm-8] [INFO] [1782314768.026593389] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.2173912171536757, y:-2.2596572571166624, z:0.484072073526326 +[sm-8] [INFO] [1782314768.027240037] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314768.027463081] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314768.027680195] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314768.028106362] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314768.028372168] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314768.028661186] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314768.028999735] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314768.029168515] [hri]: Detected objects: +[sm-8] [INFO] [1782314768.029377051] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314768.029698303] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314768.029887664] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314768.030129756] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314768.030472394] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 3 +[sm-8] [INFO] [1782314768.030779386] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314768.031189002] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.415, -1.620, 0.700) +[sm-8] [INFO] [1782314768.031467315] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314768.032293170] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314769.024420612] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'succeeded' --> 'SLEEP' +[sm-8] [INFO] [1782314769.024857095] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314771.026314539] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314771.026692987] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314771.285275815] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314771.285931884] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314771.316143461] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314771.316487077] [hri]: [detect_3d.py:response_handler:121] chair at (-0.17, -1.73, 0.48) +[sm-8] [INFO] [1782314771.316781040] [hri]: [detect_3d.py:response_handler:121] person at (0.61, -2.73, 0.56) +[sm-8] [INFO] [1782314771.317051062] [hri]: [detect_3d.py:response_handler:121] person at (0.14, -2.42, 0.64) +[sm-8] [INFO] [1782314771.317347297] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314771.354817932] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.1731672638959224, y:-1.7251520101109095, z:0.4818593643113742 +[sm-8] [INFO] [1782314771.355231981] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.6104313443534934, y:-2.7261472036088223, z:0.5582490170953512 +[sm-8] [INFO] [1782314771.355557862] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.13592449385926575, y:-2.422750813020721, z:0.642262441839883 +[sm-8] [INFO] [1782314771.356086460] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314771.356316429] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314771.356546992] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314771.356930225] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314771.357201361] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314771.357450427] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314771.357739047] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314771.357926745] [hri]: Detected objects: +[sm-8] [INFO] [1782314771.358155677] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314771.358332610] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314771.358514896] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314771.358746511] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314771.359134689] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 4 +[sm-8] [INFO] [1782314771.359461616] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314771.359895698] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.324, -2.502, 0.700) +[sm-8] [INFO] [1782314771.360193252] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314771.361145783] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [WARN] [1782314776.373635089] [hri]: [action_state.py:execute:205] Timeout reached while waiting for response from action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314776.373930140] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'timeout' --> 'SLEEP' +[sm-8] [INFO] [1782314776.374285817] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314778.376123855] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314778.376390700] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314778.634817724] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314778.636326694] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314778.680621915] [hri]: [detect_3d.py:response_handler:119] Got 4 detections +[sm-8] [INFO] [1782314778.680969918] [hri]: [detect_3d.py:response_handler:121] person at (0.62, -2.71, 0.57) +[sm-8] [INFO] [1782314778.681259196] [hri]: [detect_3d.py:response_handler:121] person at (0.12, -2.52, 0.67) +[sm-8] [INFO] [1782314778.681547322] [hri]: [detect_3d.py:response_handler:121] chair at (-1.14, -3.07, 0.10) +[sm-8] [INFO] [1782314778.681847378] [hri]: [detect_3d.py:response_handler:121] chair at (-0.03, -1.79, 0.47) +[sm-8] [INFO] [1782314778.682183879] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314778.714276438] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.6154921185260351, y:-2.7074095212928344, z:0.570150122446445 +[sm-8] [INFO] [1782314778.714707777] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.12489200344368745, y:-2.519219707714123, z:0.674373636622188 +[sm-8] [INFO] [1782314778.715106707] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-1.1397049410986115, y:-3.071779997329116, z:0.10353092054997104 +[sm-8] [INFO] [1782314778.715443454] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.030496223431798697, y:-1.7942899646074606, z:0.4664364655376454 +[sm-8] [INFO] [1782314778.716002905] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314778.716215251] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314778.716446460] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314778.716856704] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314778.717127168] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314778.717384662] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314778.717699786] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314778.742531623] [hri]: Detected objects: +[sm-8] [INFO] [1782314778.742851493] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314778.743044649] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314778.743234665] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314778.743527782] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314778.743929970] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 5 +[sm-8] [INFO] [1782314778.744257653] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314778.744713962] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.403, -2.608, 0.700) +[sm-8] [INFO] [1782314778.745006155] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314778.745817018] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314778.765229632] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314778.771164477] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314780.773769682] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314780.774239420] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314781.040671037] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314781.041406519] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314781.076464232] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314781.076837611] [hri]: [detect_3d.py:response_handler:121] person at (0.61, -2.71, 0.57) +[sm-8] [INFO] [1782314781.077203559] [hri]: [detect_3d.py:response_handler:121] person at (0.14, -2.50, 0.67) +[sm-8] [INFO] [1782314781.077523787] [hri]: [detect_3d.py:response_handler:121] chair at (0.00, -1.85, 0.46) +[sm-8] [INFO] [1782314781.077826645] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314781.110522608] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.613086385463015, y:-2.7058319701658022, z:0.5742179920180621 +[sm-8] [INFO] [1782314781.111007884] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.1384372099528045, y:-2.4998267608364553, z:0.672177747037668 +[sm-8] [INFO] [1782314781.111400112] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:0.004740497414783329, y:-1.8521691891653527, z:0.45767899498772413 +[sm-8] [INFO] [1782314781.112006291] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314781.112225858] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314781.112461277] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314781.113010709] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314781.113281747] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314781.113547462] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314781.113826648] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314781.113990333] [hri]: Detected objects: +[sm-8] [INFO] [1782314781.114194869] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314781.114362632] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314781.114526256] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314781.114752294] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314781.115086638] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 6 +[sm-8] [INFO] [1782314781.133931076] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314781.141764450] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.638, -2.903, 0.700) +[sm-8] [INFO] [1782314781.142319952] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314781.143379467] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314781.166454760] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314781.166893769] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314783.171019646] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314783.171341809] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314783.427896684] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314783.430185636] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314783.472125494] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314783.475979112] [hri]: [detect_3d.py:response_handler:121] person at (0.62, -2.69, 0.60) +[sm-8] [INFO] [1782314783.476320938] [hri]: [detect_3d.py:response_handler:121] person at (0.14, -2.52, 0.76) +[sm-8] [INFO] [1782314783.476650089] [hri]: [detect_3d.py:response_handler:121] chair at (nan, nan, nan) +[sm-8] [INFO] [1782314783.477029273] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314783.509660310] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.6178677246649334, y:-2.6885216642505894, z:0.598656074039917 +[sm-8] [INFO] [1782314783.510200710] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.13719134128552213, y:-2.5214008748742227, z:0.7612709664201134 +[sm-8] [INFO] [1782314783.510820233] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314783.511076588] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314783.511356947] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314783.521493637] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314783.535540506] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314783.535894587] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314783.536103238] [hri]: Detected objects: +[sm-8] [INFO] [1782314783.536348325] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314783.536541212] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314783.536745792] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314783.537205068] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314783.538906815] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 7 +[sm-8] [INFO] [1782314783.539722782] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314783.540318096] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(-0.466, -1.859, 0.700) +[sm-8] [INFO] [1782314783.540747678] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314783.541699157] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314783.565145328] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314783.566193362] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314785.568691360] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314785.569128191] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314785.825856199] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314785.826579204] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314785.867952381] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314785.868300222] [hri]: [detect_3d.py:response_handler:121] person at (0.06, -2.51, 0.63) +[sm-8] [INFO] [1782314785.868583337] [hri]: [detect_3d.py:response_handler:121] chair at (-0.23, -1.73, 0.45) +[sm-8] [INFO] [1782314785.868862042] [hri]: [detect_3d.py:response_handler:121] chair at (nan, nan, nan) +[sm-8] [INFO] [1782314785.869147164] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314785.899724381] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.0626246430328059, y:-2.511395637266447, z:0.6250686060050291 +[sm-8] [INFO] [1782314785.900311201] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.2270418285246243, y:-1.7320761324164469, z:0.4506950292752745 +[sm-8] [INFO] [1782314785.900930351] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314785.901163113] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314785.901435142] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314785.901853330] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314785.902140562] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314785.902474470] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314785.902663092] [hri]: Detected objects: +[sm-8] [INFO] [1782314785.902914734] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314785.903108009] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314785.903317016] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314785.903577805] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314785.904000202] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 8 +[sm-8] [INFO] [1782314785.904381942] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314785.904876999] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(-0.702, -2.606, 0.700) +[sm-8] [INFO] [1782314785.905204232] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314785.906124971] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [WARN] [1782314790.923764433] [hri]: [action_state.py:execute:205] Timeout reached while waiting for response from action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314790.925109501] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'timeout' --> 'SLEEP' +[sm-8] [INFO] [1782314790.925582646] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314792.927481059] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314792.944213342] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314793.215224600] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314793.216011119] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314793.253657638] [hri]: [detect_3d.py:response_handler:119] Got 4 detections +[sm-8] [INFO] [1782314793.253983681] [hri]: [detect_3d.py:response_handler:121] person at (0.39, -2.89, 0.60) +[sm-8] [INFO] [1782314793.254278820] [hri]: [detect_3d.py:response_handler:121] person at (0.07, -2.63, 0.67) +[sm-8] [INFO] [1782314793.254568160] [hri]: [detect_3d.py:response_handler:121] chair at (-0.24, -1.70, 0.47) +[sm-8] [INFO] [1782314793.254840642] [hri]: [detect_3d.py:response_handler:121] chair at (-0.31, -1.66, 0.61) +[sm-8] [INFO] [1782314793.255154514] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314793.290264151] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.3931153614770337, y:-2.891063418791336, z:0.6017311674355308 +[sm-8] [INFO] [1782314793.290850688] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.06556227126836045, y:-2.6333078258916047, z:0.6730924485732879 +[sm-8] [INFO] [1782314793.291630405] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.23787177396038062, y:-1.6994283999174096, z:0.4689823660994773 +[sm-8] [INFO] [1782314793.291988738] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.3050189773000358, y:-1.6629102222450536, z:0.6132031503319433 +[sm-8] [INFO] [1782314793.292589426] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314793.292840543] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314793.293062968] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314793.293483285] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314793.293767330] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314793.294039296] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314793.294288539] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314793.294575097] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314793.294752968] [hri]: Detected objects: +[sm-8] [INFO] [1782314793.294985600] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314793.295183662] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314793.295360717] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314793.295629859] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314793.296013284] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 9 +[sm-8] [INFO] [1782314793.296356364] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'continue' --> 'LOOK_POINT' +[sm-8] [INFO] [1782314793.315225778] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(-0.570, -2.050, 0.700) +[sm-8] [INFO] [1782314793.318284279] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314793.319829393] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314793.326635564] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_POINT' : 'aborted' --> 'SLEEP' +[sm-8] [INFO] [1782314793.327054418] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314795.345141100] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SLEEP' : 'succeeded' --> 'DETECT_OBJECTS' +[sm-8] [INFO] [1782314795.351401827] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'DETECT_OBJECTS_3D' +[sm-8] [INFO] [1782314795.612906534] [hri]: [service_state.py:execute:138] Waiting for service '/yolo/detect3d' +[sm-8] [INFO] [1782314795.613619560] [hri]: [service_state.py:execute:153] Sending request to service '/yolo/detect3d' +[sm-8] [INFO] [1782314795.650269398] [hri]: [detect_3d.py:response_handler:119] Got 3 detections +[sm-8] [INFO] [1782314795.650825227] [hri]: [detect_3d.py:response_handler:121] chair at (-0.23, -1.72, 0.45) +[sm-8] [INFO] [1782314795.651232380] [hri]: [detect_3d.py:response_handler:121] person at (0.07, -2.59, 0.66) +[sm-8] [INFO] [1782314795.651588667] [hri]: [detect_3d.py:response_handler:121] chair at (-1.41, -2.96, 0.64) +[sm-8] [INFO] [1782314795.651951524] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS_3D' : 'succeeded' --> 'FILTER_DETECTIONS' +[sm-8] [INFO] [1782314795.686126783] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-0.228313572839151, y:-1.7241017624719208, z:0.45217512883602984 +[sm-8] [INFO] [1782314795.686583720] [hri]: [detect_3d_in_area.py:execute:85] Detected a person at x:0.06509594183150047, y:-2.5906925798583904, z:0.6609030476436922 +[sm-8] [INFO] [1782314795.686946117] [hri]: [detect_3d_in_area.py:execute:85] Detected a chair at x:-1.4068475813333872, y:-2.9558192009463053, z:0.6431754986523208 +[sm-8] [INFO] [1782314795.687497791] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'FILTER_DETECTIONS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314795.687727649] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314795.687986225] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_OBJECTS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [INFO] [1782314795.688381841] [hri]: Detected object chair is too close to existing object chair. Not counting as new. +[sm-8] [INFO] [1782314795.688652670] [hri]: Detected object person is too close to existing object person. Not counting as new. +[sm-8] [INFO] [1782314795.689008757] [hri]: Processed detections. Total detected objects: 3 +[sm-8] [INFO] [1782314795.689202812] [hri]: Detected objects: +[sm-8] [INFO] [1782314795.689443174] [hri]: - person at (0.6193406740954449, -2.730382238197708, 0.5803075375904453) +[sm-8] [INFO] [1782314795.689640137] [hri]: - chair at (-0.2352875350233149, -1.6654367004297261, 0.49727366850470844) +[sm-8] [INFO] [1782314795.689860263] [hri]: - person at (0.2029386573330394, -2.301679510603871, 0.5039309202759044) +[sm-8] [INFO] [1782314795.690145392] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'GET_LOOK_POINT' +[sm-8] [INFO] [1782314795.690579120] [hri]: [detect_all_in_polygon.py:_get_look_point:448] 10 +[sm-8] [INFO] [1782314795.690967112] [hri]: [detect_all_in_polygon.py:_get_look_point:454] Finished iterating through sweep points. +[sm-8] [INFO] [1782314795.691229916] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_LOOK_POINT' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314795.691485085] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314795.691746932] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AND_DETECT' : 'succeeded' --> 'PUBLISH_DETECTED_OBJECTS' +[sm-8] [INFO] [1782314795.692156989] [hri]: Processing 3 detections for debug image. +[sm-8] [INFO] [1782314795.714377021] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.714605530] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.714805648] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.714988432] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.715182591] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.715374042] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.715562939] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.715769613] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.716002448] [hri]: Processing 0 detections for debug image. +[sm-8] [INFO] [1782314795.716380004] [hri]: Created debug image with detections. +[sm-8] [INFO] [1782314795.815303384] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PUBLISH_DETECTED_OBJECTS' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314795.816057038] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314795.817282642] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'DETECT_ALL_PEOPLE_SEATS' : 'succeeded' --> 'PROCESS_DETECTIONS' +[sm-8] [WARN] [1782314795.822117756] [hri]: [seat_guest.py:execute:77] Finding seat in seat guest +[sm-8] [INFO] [1782314795.824273361] [hri]: [seat_guest.py:execute:103] Detected this many people in sweep: 2 +[sm-8] [INFO] [1782314795.824700973] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'PROCESS_DETECTIONS' : 'succeeded' --> 'LOOK_TO_SEAT' +[sm-8] [INFO] [1782314795.825263731] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.311, -2.717, 0.500) +[sm-8] [INFO] [1782314795.825648506] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314795.845352551] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314796.915569098] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_TO_SEAT' : 'succeeded' --> 'SAY_SEAT_GUEST' +[sm-8] [INFO] [1782314796.918817133] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314796.920035007] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314805.203267156] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_SEAT_GUEST' : 'succeeded' --> 'WAIT_FOR_GUEST_TO_SEAT' +[sm-8] [INFO] [1782314805.210866423] [hri]: [wait.py:execute:21] Waiting for 5.0 seconds. +[sm-8] [INFO] [1782314810.221334628] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT_FOR_GUEST_TO_SEAT' : 'succeeded' --> 'RESET_HEAD_2' +[sm-8] [INFO] [1782314810.222965944] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314810.226574301] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314810.227945074] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314811.752656615] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314811.753089492] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_2' : 'succeeded' --> 'succeeded' +[sm-8] [INFO] [1782314811.753446245] [hri]: [state_machine.cpp:execute:496] State machine ends with outcome 'succeeded' +[sm-8] [INFO] [1782314811.753760312] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SEAT_GUEST_2' : 'succeeded' --> 'CHECK' +[sm-8] [INFO] [1782314811.754211480] [hri]: [state_machine.py:check:166] 2 +[sm-8] [INFO] [1782314811.754646459] [hri]: [state_machine.py:check:170] Guest1: +[sm-8] [INFO] [1782314811.755034637] [hri]: [state_machine.py:check:173] name: Jeff +[sm-8] [INFO] [1782314811.755448209] [hri]: [state_machine.py:check:173] drink: vodka +[sm-8] [INFO] [1782314811.755846929] [hri]: [state_machine.py:check:173] detection: True +[sm-8] [INFO] [1782314811.756205955] [hri]: [state_machine.py:check:173] seating_detection: False +[sm-8] [INFO] [1782314811.756579195] [hri]: [state_machine.py:check:173] attributes: {'hair_color': 'black', 'hair_length': 'shoulderlength', 'glasses': True, 'hat': True, 'shirt_color': 'black'} +[sm-8] [INFO] [1782314811.757047526] [hri]: [state_machine.py:check:173] seated_point: None +[sm-8] [INFO] [1782314811.757431009] [hri]: [state_machine.py:check:174] Guest2: +[sm-8] [INFO] [1782314811.757742607] [hri]: [state_machine.py:check:177] name: Fadi +[sm-8] [INFO] [1782314811.758017818] [hri]: [state_machine.py:check:177] drink: peach iced tea +[sm-8] [INFO] [1782314811.758280164] [hri]: [state_machine.py:check:177] detection: True +[sm-8] [INFO] [1782314811.758554015] [hri]: [state_machine.py:check:177] seating_detection: False +[sm-8] [INFO] [1782314811.758823626] [hri]: [state_machine.py:check:177] attributes: {'hair_color': 'brown', 'hair_length': 'short', 'glasses': True, 'hat': True, 'shirt_color': 'beige'} +[sm-8] [INFO] [1782314811.759096066] [hri]: [state_machine.py:check:177] seated_point: None +[sm-8] [INFO] [1782314811.759309304] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'CHECK' : 'succeeded' --> 'INTRODUCE' +[sm-8] [INFO] [1782314811.759552911] [hri]: [state_machine.cpp:execute:423] Executing state machine with initial state 'RESET_SEATING_DETECTIONS' +[sm-8] [INFO] [1782314811.759931761] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_SEATING_DETECTIONS' : 'succeeded' --> 'LOOP_PERSON_STATE' +[sm-8] [INFO] [1782314811.760350544] [hri]: [introduce.py:_loop_person_index:246] 0 +[sm-8] [INFO] [1782314811.760614613] [hri]: [introduce.py:_loop_person_index:247] Guest1 point: None +[sm-8] [INFO] [1782314811.760906967] [hri]: [introduce.py:_loop_person_index:248] Guest2 point: None +[sm-8] [INFO] [1782314811.761194386] [hri]: [introduce.py:_loop_person_index:249] Host point: None +[sm-8] [INFO] [1782314811.761455390] [hri]: [introduce.py:_loop_person_index:250] Total detections (seats + people): 2 +[sm-8] [INFO] [1782314811.761785918] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOP_PERSON_STATE' : 'continue' --> 'LOOK_AT_PERSON' +[sm-8] [INFO] [1782314811.762272251] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.619, -2.730, 0.580) +[sm-8] [INFO] [1782314811.762566630] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314811.763423612] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314812.826455138] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AT_PERSON' : 'succeeded' --> 'WAIT' +[sm-8] [INFO] [1782314812.826866070] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314814.829400736] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT' : 'succeeded' --> 'RECOGNISE' +[sm-8] [INFO] [1782314814.830040009] [hri]: [recognise.py:_create_request:83] Waiting for synced rgb and depth frames +[sm-8] [INFO] [1782314815.846235480] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/recognise' +[sm-8] [INFO] [1782314815.846919234] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/recognise' +[service-3] 2026-06-24 16:26:57.366904: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 438.19MiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] 2026-06-24 16:26:57.366950: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 408.19MiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] 2026-06-24 16:26:57.366968: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 408.19MiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] 2026-06-24 16:26:57.366983: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 408.19MiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] 2026-06-24 16:26:57.366998: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 408.19MiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[service-3] 2026-06-24 16:26:57.367013: W external/local_tsl/tsl/framework/bfc_allocator.cc:296] Allocator (GPU_0_bfc) ran out of memory trying to allocate 408.19MiB with freed_by_count=0. The caller indicates that this is not a failure, but this may mean that there could be performance gains if more memory were available. +[sm-8] [INFO] [1782314817.456993939] [hri]: [recognise.py:_handle_resp:106] guest1 +[sm-8] [INFO] [1782314817.457463120] [hri]: [recognise.py:_handle_resp:107] geometry_msgs.msg.Point(x=0.1272925716897485, y=-2.5621232127688796, z=1.100714093020688) +[sm-8] [INFO] [1782314817.458102305] [hri]: [recognise.py:_handle_resp:106] host +[sm-8] [INFO] [1782314817.458470289] [hri]: [recognise.py:_handle_resp:107] geometry_msgs.msg.Point(x=0.5652780457356624, y=-2.821163961564225, z=1.1150141099009405) +[sm-8] [INFO] [1782314817.458907386] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RECOGNISE' : 'succeeded' --> 'RESET_HEAD_1' +[sm-8] [INFO] [1782314817.459218082] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314817.459562133] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314817.465074064] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314818.984768854] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314818.985108770] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_1' : 'succeeded' --> 'LOOP_PERSON_STATE' +[sm-8] [INFO] [1782314818.985650518] [hri]: [introduce.py:_loop_person_index:246] 1 +[sm-8] [INFO] [1782314818.985933057] [hri]: [introduce.py:_loop_person_index:247] Guest1 point: geometry_msgs.msg.Point(x=0.1272925716897485, y=-2.5621232127688796, z=1.100714093020688) +[sm-8] [INFO] [1782314818.986209898] [hri]: [introduce.py:_loop_person_index:248] Guest2 point: None +[sm-8] [INFO] [1782314818.986479431] [hri]: [introduce.py:_loop_person_index:249] Host point: geometry_msgs.msg.Point(x=0.5652780457356624, y=-2.821163961564225, z=1.1150141099009405) +[sm-8] [INFO] [1782314818.986801878] [hri]: [introduce.py:_loop_person_index:250] Total detections (seats + people): 2 +[sm-8] [INFO] [1782314818.987099860] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOP_PERSON_STATE' : 'continue' --> 'LOOK_AT_PERSON' +[sm-8] [INFO] [1782314818.987515495] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.203, -2.302, 0.504) +[sm-8] [INFO] [1782314818.987800746] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314818.988572788] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314820.043969494] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AT_PERSON' : 'succeeded' --> 'WAIT' +[sm-8] [INFO] [1782314820.044335843] [hri]: [wait.py:execute:21] Waiting for 2 seconds. +[sm-8] [INFO] [1782314822.046152912] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'WAIT' : 'succeeded' --> 'RECOGNISE' +[sm-8] [INFO] [1782314822.046750799] [hri]: [recognise.py:_create_request:83] Waiting for synced rgb and depth frames +[sm-8] [INFO] [1782314823.048677356] [hri]: [service_state.py:execute:138] Waiting for service '/lasr_vision_reid/recognise' +[sm-8] [INFO] [1782314823.049754424] [hri]: [service_state.py:execute:153] Sending request to service '/lasr_vision_reid/recognise' +[sm-8] [INFO] [1782314823.307802867] [hri]: [recognise.py:_handle_resp:106] guest1 +[sm-8] [INFO] [1782314823.308149696] [hri]: [recognise.py:_handle_resp:107] geometry_msgs.msg.Point(x=0.09661599787381481, y=-2.63956179785073, z=1.0824569285559633) +[sm-8] [INFO] [1782314823.308548589] [hri]: [recognise.py:_handle_resp:106] host +[sm-8] [INFO] [1782314823.308824309] [hri]: [recognise.py:_handle_resp:107] geometry_msgs.msg.Point(x=0.5603743875304602, y=-2.872620242063255, z=1.1067359963905194) +[sm-8] [INFO] [1782314823.309108790] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RECOGNISE' : 'succeeded' --> 'RESET_HEAD_1' +[sm-8] [INFO] [1782314823.309316405] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314823.309572743] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314823.310370758] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314824.832361330] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314824.832667206] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_1' : 'succeeded' --> 'LOOP_PERSON_STATE' +[sm-8] [INFO] [1782314824.833239733] [hri]: [introduce.py:_loop_person_index:246] 2 +[sm-8] [INFO] [1782314824.833561986] [hri]: [introduce.py:_loop_person_index:247] Guest1 point: geometry_msgs.msg.Point(x=0.09661599787381481, y=-2.63956179785073, z=1.0824569285559633) +[sm-8] [INFO] [1782314824.833847580] [hri]: [introduce.py:_loop_person_index:248] Guest2 point: None +[sm-8] [INFO] [1782314824.834160109] [hri]: [introduce.py:_loop_person_index:249] Host point: geometry_msgs.msg.Point(x=0.5603743875304602, y=-2.872620242063255, z=1.1067359963905194) +[sm-8] [INFO] [1782314824.834540077] [hri]: [introduce.py:_loop_person_index:250] Total detections (seats + people): 2 +[sm-8] [INFO] [1782314824.834970769] [hri]: [introduce.py:_loop_person_index:285] Fallback Guest2 point: geometry_msgs.msg.Point(x=0.2029386573330394, y=-2.301679510603871, z=0.5039309202759044) +[sm-8] [INFO] [1782314824.835177788] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOP_PERSON_STATE' : 'succeeded' --> 'GRAB_GUEST_POINT' +[sm-8] [INFO] [1782314824.835605926] [hri]: [introduce.py:_loop_guest:315] guest1 +[sm-8] [INFO] [1782314824.835884224] [hri]: [introduce.py:_loop_guest:316] geometry_msgs.msg.Point(x=0.09661599787381481, y=-2.63956179785073, z=1.0824569285559633) +[sm-8] [INFO] [1782314824.836196590] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GRAB_GUEST_POINT' : 'continue' --> 'GET_INTRODUCTION_STR' +[sm-8] [INFO] [1782314824.836500258] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_INTRODUCTION_STR' : 'succeeded' --> 'LOOK_AT_GUEST' +[sm-8] [INFO] [1782314824.836929432] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.097, -2.640, 1.082) +[sm-8] [INFO] [1782314824.837212267] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314824.837944084] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314825.901930326] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AT_GUEST' : 'succeeded' --> 'SAY_INTRODUCTION' +[sm-8] [INFO] [1782314825.902350348] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314825.903237617] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314831.601909062] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_INTRODUCTION' : 'succeeded' --> 'RESET_HEAD_2' +[sm-8] [INFO] [1782314831.602251249] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314831.602596941] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314831.603457958] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314833.125915847] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314833.126210809] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_2' : 'succeeded' --> 'GRAB_GUEST_POINT' +[sm-8] [INFO] [1782314833.126782161] [hri]: [introduce.py:_loop_guest:315] guest2 +[sm-8] [INFO] [1782314833.127084861] [hri]: [introduce.py:_loop_guest:316] geometry_msgs.msg.Point(x=0.2029386573330394, y=-2.301679510603871, z=0.5039309202759044) +[sm-8] [INFO] [1782314833.127421060] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GRAB_GUEST_POINT' : 'continue' --> 'GET_INTRODUCTION_STR' +[sm-8] [INFO] [1782314833.127738527] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_INTRODUCTION_STR' : 'succeeded' --> 'LOOK_AT_GUEST' +[sm-8] [INFO] [1782314833.128158832] [hri]: [look_to_point.py:_create_goal:45] Sending PointHead goal: frame=map, point=(0.203, -2.302, 0.504) +[sm-8] [INFO] [1782314833.128451646] [hri]: [action_state.py:execute:165] Waiting for action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314833.129326926] [hri]: [action_state.py:execute:189] Sending goal to action '/head_controller/point_head_action' +[sm-8] [INFO] [1782314834.184083405] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'LOOK_AT_GUEST' : 'succeeded' --> 'SAY_INTRODUCTION' +[sm-8] [INFO] [1782314834.188780704] [hri]: [action_state.py:execute:165] Waiting for action '/tts_engine/tts' +[sm-8] [INFO] [1782314834.189612552] [hri]: [action_state.py:execute:189] Sending goal to action '/tts_engine/tts' +[sm-8] [INFO] [1782314839.412322579] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'SAY_INTRODUCTION' : 'succeeded' --> 'RESET_HEAD_2' +[sm-8] [INFO] [1782314839.412615542] [hri]: GIVING GOAL of look_centre +[sm-8] [INFO] [1782314839.412957319] [hri]: [action_state.py:execute:165] Waiting for action '/play_motion2' +[sm-8] [INFO] [1782314839.413769028] [hri]: [action_state.py:execute:189] Sending goal to action '/play_motion2' +[sm-8] [INFO] [1782314840.929708614] [hri]: Received result with response: play_motion2_msgs.action.PlayMotion2_Result(success=True, error='') +[sm-8] [INFO] [1782314840.937091410] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'RESET_HEAD_2' : 'succeeded' --> 'GRAB_GUEST_POINT' +[sm-8] [INFO] [1782314840.947805510] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GRAB_GUEST_POINT' : 'succeeded' --> 'GET_HOST' +[sm-8] [INFO] [1782314840.948135132] [hri]: [state_machine.cpp:execute:488] State machine transitioning 'GET_HOST' : 'succeeded' --> 'LOOK_AT_HOST' +[sm-8] Traceback (most recent call last): +[sm-8] File "/home/rexy/fadi_ws/install/HRI/lib/HRI/sm-venv", line 33, in +[sm-8] sys.exit(load_entry_point('HRI==0.0.0', 'console_scripts', 'sm')()) +[sm-8] File "/home/rexy/fadi_ws/install/HRI/lib/python3.10/site-packages/HRI/state_machine.py", line 263, in main +[sm-8] outcome = sm(bb) +[sm-8] File "/opt/robocup_ws/install/yasmin_ros/local/lib/python3.10/dist-packages/yasmin_ros/action_state.py", line 162, in execute +[sm-8] goal = self._create_goal_handler(blackboard) +[sm-8] File "/home/rexy/fadi_ws/install/skills/lib/python3.10/site-packages/lasr_skills/look_to_point.py", line 43, in _create_goal +[sm-8] goal.target = target +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/control_msgs/action/_point_head.py", line 163, in target +[sm-8] assert \ +[sm-8] AssertionError: The 'target' field must be a sub message of type 'PointStamped' +[sm-8] Exception in thread Thread-1 (spin): +[sm-8] Exception in thread Thread-2 (spin): +[sm-8] Traceback (most recent call last): +[sm-8] Traceback (most recent call last): +[sm-8] File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner +[sm-8] File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner +[sm-8] self.run() +[sm-8] File "/usr/lib/python3.10/threading.py", line 953, in run +[sm-8] self.run() +[sm-8] File "/usr/lib/python3.10/threading.py", line 953, in run +[sm-8] self._target(*self._args, **self._kwargs) +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 323, in spin +[sm-8] self._target(*self._args, **self._kwargs) +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 323, in spin +[sm-8] self.spin_once() +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 863, in spin_once +[sm-8] self.spin_once() +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 863, in spin_once +[sm-8] self._spin_once_impl(timeout_sec) +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 855, in _spin_once_impl +[sm-8] self._spin_once_impl(timeout_sec) +[sm-8] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 855, in _spin_once_impl +[sm-8] self._executor.submit(handler) +[sm-8] File "/usr/lib/python3.10/concurrent/futures/thread.py", line 167, in submit +[sm-8] self._executor.submit(handler) +[sm-8] File "/usr/lib/python3.10/concurrent/futures/thread.py", line 167, in submit +[sm-8] raise RuntimeError('cannot schedule new futures after shutdown') +[sm-8] RuntimeError: cannot schedule new futures after shutdown +[sm-8] raise RuntimeError('cannot schedule new futures after shutdown') +[sm-8] RuntimeError: cannot schedule new futures after shutdown +[sm-8] sys:1: RuntimeWarning: coroutine 'Executor._make_handler..handler' was never awaited +[sm-8] RuntimeWarning: Enable tracemalloc to get the object allocation traceback +[sm-8] Segmentation fault (core dumped) +[ERROR] [sm-8]: process has died [pid 72290, exit code 139, cmd '/home/rexy/fadi_ws/install/HRI/lib/HRI/sm --ros-args -r __node:=hri --params-file /home/rexy/fadi_ws/install/HRI/share/HRI/config/lab.yaml']. diff --git a/skills/config/follow_debug.rviz b/skills/config/follow_debug.rviz new file mode 100644 index 000000000..c2f0b5ff5 --- /dev/null +++ b/skills/config/follow_debug.rviz @@ -0,0 +1,785 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Image1 + - /Polygon1 + - /PointCloud21 + - /PointCloud21/Topic1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 167 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_7_link: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_antenna_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_antenna_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_cover_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_dock_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_mic_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mic_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mic_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mic_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_sonar_01_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_sonar_02_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_sonar_03_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_left_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_left_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_right_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_right_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_left_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_left_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_right_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_right_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_front_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_orbbec_aux_joint_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + suspension_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + suspension_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_fixed_column_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_fixed_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_ft_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_ft_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: scan_raw + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /head_front_camera/rgb/image_raw + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /person_follow/debug/polygon + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /head_front_camera/depth/rgb/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 9.734539985656738 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.7975128889083862 + Y: -0.8598803281784058 + Z: 1.05267333984375 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6897971034049988 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 4.2492146492004395 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1131 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000415fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000123000001230000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000fb0000000a0049006d006100670065010000024c000002040000002800ffffff000000010000010f00000415fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000415000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002500000041500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 960 + X: 960 + Y: 32 diff --git a/skills/config/motions.yaml b/skills/config/motions.yaml index 0f59f0923..26f7d7b97 100644 --- a/skills/config/motions.yaml +++ b/skills/config/motions.yaml @@ -20,8 +20,8 @@ positions: [ 0.04, 0.04 ] times_from_start: [ 1.0 ] pre_navigation: - joints: [ torso_lift_joint ] - positions: [ 0.15 ] + joints: [ torso_lift_joint, head_1_joint, head_2_joint] + positions: [ 0.125, 0.0, 0.0 ] times_from_start: [ 2.0 ] post_navigation: joints: [ torso_lift_joint, head_1_joint, head_2_joint ] @@ -71,32 +71,7 @@ joints: [ arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint ] positions: [ 2.63, 0.10, -3.21, 1.61, 1.53, 0.00, 0.13 ] times_from_start: [ 5.0 ] - - u3l: - joints: [ torso_lift_joint, head_1_joint, head_2_joint ] - positions: [ 0.4, 0.35, 0.1 ] - times_from_start: [ 2.0 ] - u3m: - joints: [ torso_lift_joint, head_1_joint, head_2_joint ] - positions: [ 0.4, 0.0, 0.1 ] - times_from_start: [ 2.0 ] - u3r: - joints: [ torso_lift_joint, head_1_joint, head_2_joint ] - positions: [ 0.4, -0.35, 0.1 ] - times_from_start: [ 2.0 ] - u2l: - joints: [ torso_lift_joint, head_1_joint, head_2_joint ] - positions: [ 0.35, 0.35, 0.05 ] - times_from_start: [ 2.0 ] - u2m: - joints: [ torso_lift_joint, head_1_joint, head_2_joint ] - positions: [ 0.35, 0.0, 0.05 ] - times_from_start: [ 2.0 ] - u2r: - joints: [ torso_lift_joint, head_1_joint, head_2_joint ] - positions: [ 0.35, -0.35, 0.05 ] - times_from_start: [ 2.0 ] u1l: joints: [ torso_lift_joint, head_1_joint, head_2_joint ] positions: [ 0.3, 0.35, 0.05 ] diff --git a/skills/launch/follow_person.launch.py b/skills/launch/follow_person.launch.py new file mode 100644 index 000000000..c5e8d2ecf --- /dev/null +++ b/skills/launch/follow_person.launch.py @@ -0,0 +1,51 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import AnyLaunchDescriptionSource + + +def generate_launch_description(): + load_motions = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("skills"), + "launch", + "load_motions.launch.py", + ) + ) + ) + + yolo_service = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("lasr_vision_yolo"), + "launch", + "service_launch.xml", + ) + ) + ) + + transcribe_speech = Node( + package="lasr_speech_recognition_whisper", + executable="transcribe_microphone_server", + name="whisper_mic_server", + output="screen", + ) + + follow_person = TimerAction( + period=10.0, + actions=[ + Node( + package="skills", + executable="follow_person", + name="follow_person", + output="screen", + ) + ], + ) + + return LaunchDescription( + [load_motions, yolo_service, follow_person, transcribe_speech] + ) diff --git a/skills/setup.py b/skills/setup.py index a02c3819c..cdb377ae2 100755 --- a/skills/setup.py +++ b/skills/setup.py @@ -60,7 +60,11 @@ def run(self): "play_motion = lasr_skills.play_motion:main", "look = lasr_skills.look_to_point:main", "go_to_location = lasr_skills.go_to_location:main", - "ask_and_listen = lasr_skills.ask_and_listen:main" + "ask_and_listen = lasr_skills.ask_and_listen:main", + "receive_object = lasr_skills.receive_object:main", + "follow_person = lasr_skills.follow_person:main", + "rotate = lasr_skills.rotate:main", + "detect_keypoints_3d = lasr_skills.detect_keypoints_3d:main", ], }, ) diff --git a/skills/src/lasr_skills/__init__.py b/skills/src/lasr_skills/__init__.py index 1e4e5247b..e72225513 100755 --- a/skills/src/lasr_skills/__init__.py +++ b/skills/src/lasr_skills/__init__.py @@ -1,34 +1,24 @@ -import rclpy -from rclpy.node import Node -from rclpy.task import Future -from rclpy.qos import QoSProfile, QoSReliabilityPolicy - from .wait import Wait -# from .detect import Detect - from .detect_3d import Detect3D from .detect_3d_in_area import Detect3DInArea from .detect_all_in_polygon import DetectAllInPolygon +from .detect_keypoints_3d import DetectKeypoints3D -# from .wait_for_person import WaitForPerson from .say import Say -# from .wait_for_person_in_area import WaitForPersonInArea from .describe_people import DescribePeople from .look_to_point import LookToPoint from .play_motion import PlayMotion from .go_to_location import GoToLocation +from .rotate import Rotate +from .continuous_go_to_location import ContinuousGoToLocation from .listen import Listen from .face_person import FacePerson from .listen import Listen -# from .listen_for import ListenFor from .receive_object import ReceiveObject -from .handover_object import HandoverObject - -# from .clip_vqa import QueryImage from .detect_faces import DetectFaces from .eye_tracker import StartEyeTracker, StopEyeTracker from .wait_for_person_in_area import WaitForPersonInArea @@ -36,15 +26,17 @@ # from .recognise import Recognise from .detect_gesture import DetectGesture -# from .look_at_person import LookAtPerson -# from .wait import Wait -# from .guide import Guide from .detect_clothing import DetectClothing from .ask_and_listen import AskAndListen from .detect_door_opening import DetectDoorOpening +from .go_to_location_with_play_motion import SafeGoToLocation +from .start_task import StartDoorSM + +from .follow_person import FollowPerson + # from .detect_pose import DetectPose # from .find_person import FindPerson # from .xml_question_answer import XmlQuestionAnswer diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 9cfe4e365..ed9800fdc 100644 --- a/skills/src/lasr_skills/ask_and_listen.py +++ b/skills/src/lasr_skills/ask_and_listen.py @@ -13,7 +13,7 @@ def __init__( tts_phrase: Union[str, None] = None, tts_phrase_format_str: Union[str, None] = None, ): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("transcribed_speech") if tts_phrase is not None: self.add_state( @@ -84,15 +84,13 @@ def __init__( def main(): rclpy.init() - + yasmin_ros.set_ros_loggers() - - sm = AskAndListen('PLease say hi tiago then say your name and favourite drink') - + + sm = AskAndListen("PLease say hi tiago then say your name and favourite drink") + outcome = sm() - - yasmin.YASMIN_LOG_INFO(f'SM FINISHED WITH OUTCOME {outcome}') - + + yasmin.YASMIN_LOG_INFO(f"SM FINISHED WITH OUTCOME {outcome}") + rclpy.shutdown() - - \ No newline at end of file diff --git a/skills/src/lasr_skills/continuous_go_to_location.py b/skills/src/lasr_skills/continuous_go_to_location.py new file mode 100644 index 000000000..2836d2c8d --- /dev/null +++ b/skills/src/lasr_skills/continuous_go_to_location.py @@ -0,0 +1,121 @@ +from typing import Union +import rclpy +from rclpy.time import Time + +import yasmin +from yasmin import StateMachine, State, Blackboard +import yasmin_ros +import time + +from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from std_msgs.msg import Header + + +class ContinuousGoToLocation(State): + """ + Similar to GoToLocation, but is intend for constant navigation, concurrent to another process. + """ + + def __init__(self): + # This state only exits if the Concurrence kills it or Nav2 crashes + super().__init__(outcomes=["succeeded", "canceled", "aborted"]) + self.node = yasmin_ros.logger_node + self.navigator = BasicNavigator() + self.last_goal = None + + self.add_input_key("location") + self.add_input_key("cancel_nav") + self.add_input_key("stop_robot_requested") + + self.add_output_key("stop_robot_requested") + + def execute(self, blackboard: Blackboard): + if "stop_robot_requested" not in blackboard.keys(): + blackboard["stop_robot_requested"] = False + if "location" not in blackboard.keys(): + blackboard["location"] = None + if "cancel_nav" not in blackboard.keys(): + blackboard["cancel_nav"] = False + + while not self.is_canceled() and rclpy.ok(): + current_goal = blackboard["location"] + + if blackboard["cancel_nav"]: + blackboard["stop_robot_requested"] = True + self.cancel_state() + + # Handle Idling or stop requests + if blackboard["stop_robot_requested"]: + if not self.navigator.isTaskComplete(): + self.navigator.cancelTask() + self.node.get_logger().warn("Stop Requested") + time.sleep(0.1) + continue + + # Handle missing Goals + if current_goal is None: + time.sleep(0.1) + continue + + # Update Goal + if self.last_goal is None or self.isMoveableDistance( + current_goal, self.last_goal + ): + + self.node.get_logger().info("Nav2: Sending updated goal...") + goal_stamped = PoseStamped( + pose=current_goal, + header=Header(frame_id="map", stamp=Time().to_msg()), + ) + + self.navigator.goToPose(goal_stamped) + self.last_goal = current_goal + + time.sleep(0.1) + + # Cleanup if the Concurrence cancels this state + if not self.navigator.isTaskComplete(): + self.navigator.cancelTask() + + if self.is_canceled(): + return "canceled" + + return "succeeded" + + def isMoveableDistance( + self, new_pose: Pose, old_pose: Pose, threshold: float = 0.15 + ) -> bool: + dx = new_pose.position.x - old_pose.position.x + dy = new_pose.position.y - old_pose.position.y + return (dx**2 + dy**2) ** 0.5 > threshold + + +def main(): + rclpy.init() + + node = rclpy.create_node("hri") + yasmin_ros.set_ros_loggers(node) + + try: + sm = StateMachine(outcomes=["succeeded", "failed"]) + sm.add_state( + "GO_TO_START", + ContinuousGoToLocation(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + + bb = Blackboard() + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(outcome) + except Exception as e: + yasmin.YASMIN_LOG_WARN(e) + + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py index 633c1b1ff..96b96eeb4 100755 --- a/skills/src/lasr_skills/describe_people.py +++ b/skills/src/lasr_skills/describe_people.py @@ -1,7 +1,7 @@ import rclpy import yasmin import yasmin_ros -from lasr_vision_interfaces.srv import Vqa +from lasr_vlm_interfaces.srv import VlmDescribePeople from .vision import GetImage @@ -16,46 +16,21 @@ def __init__(self): self.add_state( "GET_IMAGE", GetImage(), - transitions={"succeeded": "LOOP_ATTR_STATE", "failed": "failed"}, - ) - - loop_state = yasmin.CbState( - outcomes=["succeeded", "continue"], callback=self._get_attr - ) - - loop_state.add_input_key("clip_index") - loop_state.add_output_key("clip_index") - - self.add_state( - "LOOP_ATTR_STATE", - loop_state, - transitions={"succeeded": "succeeded", "continue": "GET_CLIP_ATTRIBUTES"}, + transitions={"succeeded": "GET_ATTRIBUTES", "failed": "failed"}, ) self.add_state( - "GET_CLIP_ATTRIBUTES", + "GET_ATTRIBUTES", GetClipAttributes(), - transitions={"succeeded": "LOOP_ATTR_STATE", "aborted": "failed"}, + transitions={"succeeded": "succeeded", "aborted": "failed"}, ) - def _get_attr(self, blackboard): - try: - if blackboard['clip_index'] < 3: - blackboard['clip_index'] += 1 - return 'continue' - else: - return 'succeeded' - except RuntimeError: - blackboard['clip_index'] = 0 - blackboard['clip_detection_dict'] = {} - return 'continue' - class GetClipAttributes(yasmin_ros.ServiceState): def __init__(self): super().__init__( - srv_name="/clip_vqa/query_service", - srv_type=Vqa, + srv_name="/vlm/describe_people", + srv_type=VlmDescribePeople, create_request_handler=self._create_request, response_handler=self._handle_resp, ) @@ -63,67 +38,22 @@ def __init__(self): self.add_input_key("image_raw") self.add_output_key("clip_detection_dict") - self.glasses_questions = [ - "a person wearing glasses", - "a person not wearing glasses", - ] - self.hat_questions = [ - "a person wearing a hat", - "a person not wearing a hat", - ] - self.hair_questions = [ - "a person with long hair", - "a person with short hair", - ] - self.t_shirt_questions = [ - "a person wearing a short-sleeve t-shirt", - "a person wearing a long-sleeve t-shirt", - ] - def _create_request(self, blackboard): - if blackboard["clip_index"] == 0: - glasses_request = Vqa.Request() - glasses_request.possible_answers = self.glasses_questions - glasses_request.image_raw = blackboard["image_raw"] - return glasses_request - elif blackboard["clip_index"] == 1: - hat_request = Vqa.Request() - hat_request.possible_answers = self.hat_questions - hat_request.image_raw = blackboard["image_raw"] - return hat_request - elif blackboard["clip_index"] == 2: - hair_request = Vqa.Request() - hair_request.possible_answers = self.hair_questions - hair_request.image_raw = blackboard["image_raw"] - return hair_request - elif blackboard["clip_index"] == 3: - t_shirt_request = Vqa.Request() - t_shirt_request.possible_answers = self.t_shirt_questions - t_shirt_request.image_raw = blackboard["image_raw"] - return t_shirt_request + request = VlmDescribePeople.Request() + request.image_raw = blackboard["image_raw"] + + return request def _handle_resp(self, blackboard, response): - if blackboard["clip_index"] == 0: - yasmin.YASMIN_LOG_INFO(f"Glasses: {response.answer}") - glasses_bool = response.answer == "a person wearing glasses" - blackboard["clip_detection_dict"].update({"glasses": glasses_bool}) - return "succeeded" - elif blackboard["clip_index"] == 1: - yasmin.YASMIN_LOG_INFO(f"Hat: {response.answer}") - hat_bool = response.answer == "a person wearing a hat" - blackboard["clip_detection_dict"].update({"hat": hat_bool}) - return "succeeded" - elif blackboard["clip_index"] == 2: - yasmin.YASMIN_LOG_INFO(f"Hair: {response.answer}") - hair_bool = response.answer == "a person with long hair" - blackboard["clip_detection_dict"].update({"long_hair": hair_bool}) - return "succeeded" - elif blackboard["clip_index"] == 3: - yasmin.YASMIN_LOG_INFO(f"T-shirt: {response.answer}") - t_shirt_bool = response.answer == "a person wearing a short-sleeve t-shirt" - blackboard["clip_detection_dict"].update( - {"short_sleeve_t_shirt": t_shirt_bool} - ) - attributes = blackboard["clip_detection_dict"] - yasmin.YASMIN_LOG_INFO(f"Detected attributes: {attributes}") - return "succeeded" + + dict = { + "hair_color": response.hair_color, + "hair_length": response.hair_length, + "glasses": response.glasses, + "hat": response.hat, + "shirt_color": response.shirt_color, + } + + blackboard["attributes"] = dict + + return "succeeded" diff --git a/skills/src/lasr_skills/detect_3d.py b/skills/src/lasr_skills/detect_3d.py index fcc7cdc8d..58a68e666 100644 --- a/skills/src/lasr_skills/detect_3d.py +++ b/skills/src/lasr_skills/detect_3d.py @@ -33,7 +33,6 @@ def __init__( filter: Union[List[str], None] = None, confidence: float = 0.5, target_frame: str = "map", - slop=0.2, ): super().__init__( srv_type=YoloDetection3D, @@ -54,75 +53,59 @@ def __init__( self.confidence = confidence self.target_frame = target_frame - self.node = yasmin_ros.logger_node - camera_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, ) - self.cam_info = None - self.node.create_subscription( - CameraInfo, - self.depth_camera_info_topic, - self._cache_camera_info, - qos_profile=camera_qos, + self.data = None + self.image_msg = None + + cam_info = message_filters.Subscriber( + self._node, CameraInfo, self.depth_camera_info_topic, qos_profile=camera_qos ) + self.cache = message_filters.Cache(cam_info) + image_sub = message_filters.Subscriber( - self.node, Image, self.image_topic, qos_profile=camera_qos + self._node, Image, self.image_topic, qos_profile=camera_qos ) + depth_sub = message_filters.Subscriber( - self.node, Image, self.depth_image_topic, qos_profile=camera_qos + self._node, Image, self.depth_image_topic, qos_profile=camera_qos ) self.ts = message_filters.ApproximateTimeSynchronizer( - [image_sub, depth_sub], queue_size=30, slop=slop + [image_sub, depth_sub], queue_size=10, slop=0.1 ) - self.data = None - self.image_msg = None - def _cache_camera_info(self, msg: CameraInfo) -> None: - if self.cam_info is None: - self.cam_info = msg + self.ts.registerCallback(self.callback) - def _create_req(self, blackboard): - if self.cam_info is None: - deadline = time.time() + 5.0 - while self.cam_info is None and time.time() < deadline: - time.sleep(1) - if self.cam_info is None: - yasmin.YASMIN_LOG_ERROR( - f"Timed out waiting for camera info on {self.depth_camera_info_topic}" - ) - return "failed" + def callback(self, image_msg, depth_msg): + if self.data is None: + self.data = (image_msg, depth_msg) + def _create_req(self, blackboard): self.data = None - - def callback(image_msg, depth_msg): - if self.data is not None: - return - self.data = (image_msg, depth_msg, self.cam_info) - - self.ts.registerCallback(callback) + self.image_msg = None deadline = time.time() + 30.0 - while not self.data: + while self.data is None: if time.time() > deadline: - self.node.get_logger().error( + yasmin.YASMIN_LOG_ERROR( f"Timed out waiting for synced rgb/depth frames. " f"Check that {self.image_topic} and {self.depth_image_topic} are publishing and roughly synchronized." ) return "failed" - time.sleep(1) + time.sleep(0.25) - image_msg, depth_msg, cam_info_msg = self.data + image_msg, depth_msg = self.data req = YoloDetection3D.Request( image_raw=image_msg, depth_image=depth_msg, - depth_camera_info=cam_info_msg, + depth_camera_info=self.cache.getLast(), model=self.model, confidence=self.confidence, filter=self.filter, @@ -135,7 +118,7 @@ def callback(image_msg, depth_msg): def response_handler(self, blackboard, response): yasmin.YASMIN_LOG_INFO(f"Got {len(response.detected_objects)} detections") for det in response.detected_objects: - self.node.get_logger().info( + yasmin.YASMIN_LOG_INFO( f" {det.name} at ({det.point.x:.2f}, {det.point.y:.2f}, {det.point.z:.2f})" ) diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py index 6dc96f160..10bfe885e 100644 --- a/skills/src/lasr_skills/detect_3d_in_area.py +++ b/skills/src/lasr_skills/detect_3d_in_area.py @@ -14,6 +14,8 @@ from shapely.geometry import Point as ShapelyPoint from shapely.geometry.polygon import Polygon as ShapelyPolygon +import math + class Detect3DInArea(yasmin.StateMachine): class FilterDetections(yasmin.State): @@ -24,6 +26,8 @@ def __init__( z_max: Optional[float] = None, debug_publisher: str = "/skills/detect3d_in_area/debug", ): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("detections_3d") if area_polygon is None: self.add_input_key("polygon") @@ -33,7 +37,7 @@ def __init__( self.add_input_key("z_sweep_max") self.add_output_key("detections_3d") - super().__init__(outcomes=["succeeded", "failed"]) + self._z_min = z_min self._z_max = z_max self.area_polygon = area_polygon @@ -59,6 +63,11 @@ def execute(self, blackboard): else: area_polygon = self.area_polygon + assert isinstance(area_polygon, ShapelyPolygon), ( + f"Expected a Polygon but got {type(area_polygon).__name__}. " + "Check the source geometry." + ) + polygon_msg.points = [ Point32(x=point[0], y=point[1], z=0.0) for point in area_polygon.exterior.coords @@ -67,12 +76,14 @@ def execute(self, blackboard): PolygonStamped(polygon=polygon_msg, header=Header(frame_id="map")) ) - pub = yasmin_ros.logger_node.create_publisher( + pub = yasmin_ros.logger_node.create_publisher( # CHECK: New publisher each time? declare in __init__ instead? PointStamped, "objects_points", 10 ) for detection in detected_objects: - if detection.point.x == "nan": + if math.isnan( + detection.point.x + ): # CHECK: Potential broken? float vs string? continue yasmin.YASMIN_LOG_INFO( f"Detected a {detection.name} at x:{detection.point.x}, y:{detection.point.y}, z:{detection.point.z}" @@ -123,6 +134,8 @@ def __init__( z_min: Optional[float] = None, z_max: Optional[float] = None, ): + + super().__init__(outcomes=["succeeded", "failed"]) if area_polygon is None: self.add_input_key("polygon") if z_min is None and z_max is None: @@ -132,8 +145,6 @@ def __init__( self.add_output_key("detections_3d") self.add_output_key("image_raw") - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) - self.add_state( "DETECT_OBJECTS_3D", Detect3D( diff --git a/skills/src/lasr_skills/detect_all_in_polygon.py b/skills/src/lasr_skills/detect_all_in_polygon.py index c247c0dc5..30fba2a16 100644 --- a/skills/src/lasr_skills/detect_all_in_polygon.py +++ b/skills/src/lasr_skills/detect_all_in_polygon.py @@ -24,6 +24,8 @@ from threading import Thread, RLock +import message_filters + import tf2_ros from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point @@ -142,6 +144,8 @@ def __init__( ): super().__init__(outcomes=["succeeded", "failed"]) + self.node = yasmin_ros.logger_node + self.add_output_key("sweep_points") self.add_output_key("sweep_point_index") @@ -150,7 +154,21 @@ def __init__( self._z_axis = z_axis self._fov_depth = fov_depth - self.node = yasmin_ros.logger_node + qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.VOLATILE, + reliability=ReliabilityPolicy.BEST_EFFORT, + depth=10, + ) + + info_sub = message_filters.Subscriber( + self.node, + CameraInfo, + "/head_front_camera/depth/camera_info", + qos_profile=qos, + ) + + self.cache = message_filters.Cache(info_sub) self._tf_buffer = tf2_ros.Buffer(Duration(seconds=10.0)) self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self.node) @@ -163,27 +181,10 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon: ShapelyPolygon: Footprint of camera FOV in map frame. """ - qos = QoSProfile( - history=HistoryPolicy.KEEP_LAST, - durability=DurabilityPolicy.VOLATILE, - reliability=ReliabilityPolicy.BEST_EFFORT, - depth=10, - ) - - success, msg = rclpy.wait_for_message.wait_for_message( - msg_type=CameraInfo, - node=self.node, - topic="/head_front_camera/depth/camera_info", - qos_profile=qos, - time_to_wait=10, - ) - - if success is False: - yasmin.YASMIN_LOG_INFO("No camera info received, ending state") - self.cancel_state() + self.msg = self.cache.getLast() model = PinholeCameraModel() - model.fromCameraInfo(msg) + model.fromCameraInfo(self.msg) # Define pixel corners (image boundaries) corners = [ @@ -193,28 +194,22 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon: (0, model.height - 1), # bottom-left ] - qos_test = QoSProfile(history=HistoryPolicy.KEEP_ALL) - - pub = self.node.create_publisher(PointStamped, "fov_corners", qos_test) - # Transform pixel rays to map frame transformed_points = [] for u, v in corners: ray = model.projectPixelTo3dRay((u, v)) point_cam = PointStamped() - point_cam.header.frame_id = msg.header.frame_id + point_cam.header.frame_id = self.msg.header.frame_id point_cam.header.stamp = Time().to_msg() point_cam.point.x = ray[0] * self._fov_depth point_cam.point.y = ray[1] * self._fov_depth point_cam.point.z = ray[2] * self._fov_depth - pub.publish(point_cam) - # Transform to map frame try: transform = self._tf_buffer.lookup_transform( "map", - msg.header.frame_id, + self.msg.header.frame_id, Time(), timeout=Duration(seconds=5.0), ) @@ -307,9 +302,7 @@ def _calculate_sweep_points(self) -> List[PointStamped]: # Optional: visualize FOV - qos = QoSProfile( - depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL - ) + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) pub = self.node.create_publisher(PolygonStamped, "projected_fov_polygon", qos) @@ -378,7 +371,7 @@ def __init__( min_confidence: float = 0.5, min_new_object_dist: float = 0.1, ): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) get_point_state = yasmin.CbState( outcomes=["succeeded", "failed", "continue"], callback=self._get_look_point @@ -408,9 +401,9 @@ def __init__( }, ) self.add_state( - 'SLEEP', + "SLEEP", Wait(wait_time=2), - transitions={'succeeded': 'DETECT_OBJECTS', 'failed': 'failed'} + transitions={"succeeded": "DETECT_OBJECTS", "failed": "failed"}, ) self.add_state( "DETECT_OBJECTS", @@ -514,7 +507,7 @@ def __init__( prompt (Optional[str], optional): Prompt for the LangSam model, if used. """ - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detected_objects") @@ -525,11 +518,13 @@ def __init__( self._min_confidence = min_confidence self._min_new_object_dist = min_new_object_dist self._node = yasmin_ros.logger_node + image_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, ) + self._debug_publisher = self._node.create_publisher( Image, "/detect_all_in_polygon/debug", image_qos ) @@ -648,6 +643,7 @@ def _init_bb(blackboard): }, ) + class Detect_node(Node): def __init__(self): super().__init__( @@ -659,6 +655,7 @@ def __init__(self): self._spin_thread = Thread(target=self._executor.spin) self._spin_thread.start() + def main(): seat_area = [ [0.37787461280822754, -3.057680130004883], @@ -670,9 +667,9 @@ def main(): seat_polygon = ShapelyPolygon(seat_area) rclpy.init() - + node = Detect_node() - + yasmin_ros.set_ros_loggers(node) bb = Blackboard() @@ -703,7 +700,6 @@ def main(): node.destroy_node() rclpy.shutdown() - node.destroy_node() rclpy.shutdown() diff --git a/skills/src/lasr_skills/detect_door_opening.py b/skills/src/lasr_skills/detect_door_opening.py index feb01c553..e593467ac 100644 --- a/skills/src/lasr_skills/detect_door_opening.py +++ b/skills/src/lasr_skills/detect_door_opening.py @@ -26,7 +26,7 @@ class DetectDoorOpening(State): def __init__( self, - lasr_scan_topic: str = "/scan_raw", + lasr_scan_topic: str = "/scan", opened_delta: float = 0.5, timeout: float = 15.0, ): @@ -67,7 +67,6 @@ def _is_door_opened(self, msg: LaserScan) -> None: yasmin.YASMIN_LOG_WARN("Laser scan mean distance is NaN.") return - yasmin.YASMIN_LOG_INFO(f"Current mean distance: {mean_distance:.2f}") if mean_distance - self._initial_mean_distance > self._opened_delta: yasmin.YASMIN_LOG_INFO("Door has been opened.") self._door_opened = True @@ -91,7 +90,7 @@ def _capture_first_scan(msg: LaserScan) -> None: while ( rclpy.ok() and initial_scan is None and (time.time() - start_time) < timeout ): - rclpy.spin_once(self._node, timeout_sec=0.1) + time.sleep(0.1) self._node.destroy_subscription(temp_sub) return initial_scan @@ -127,7 +126,7 @@ def execute(self, blackboard): and (not self._door_opened) and ((time.time() - start_time) < self._timeout) ): - rclpy.spin_once(self._node, timeout_sec=0.1) + time.sleep(1) if self._scan_subscriber is not None: self._node.destroy_subscription(self._scan_subscriber) diff --git a/skills/src/lasr_skills/detect_keypoints_3d.py b/skills/src/lasr_skills/detect_keypoints_3d.py new file mode 100644 index 000000000..51690953b --- /dev/null +++ b/skills/src/lasr_skills/detect_keypoints_3d.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +from typing import List, Union, Optional + +import rclpy + +import yasmin +import yasmin_ros +from yasmin import Blackboard, StateMachine +from yasmin_ros import set_ros_loggers, ServiceState +from yasmin_viewer import YasminViewerPub + +import message_filters + +import time + +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from sensor_msgs.msg import Image, CameraInfo, PointCloud2 +from lasr_vision_interfaces.srv import YoloPoseDetection3D + + +class DetectKeypoints3D(ServiceState): + def __init__( + self, + image_topic: str = "/head_front_camera/rgb/image_raw", + depth_image_topic: str = "/head_front_camera/depth/image_raw", + depth_camera_info_topic: str = "/head_front_camera/depth/camera_info", + model: str = "yolo11n-pose.pt", + confidence: float = 0.5, + target_frame: str = "map", + slop=0.1, + ): + super().__init__( + srv_type=YoloPoseDetection3D, + srv_name="/yolo/detect3d_pose", + create_request_handler=self._create_req, + outcomes=["succeeded", "failed"], + response_handler=self.response_handler, + ) + + self.add_output_key("keypoint_detections_3d") + self.add_output_key("image_raw") + + self.image_topic = image_topic + self.depth_image_topic = depth_image_topic + self.depth_camera_info_topic = depth_camera_info_topic + self.model = model + self.confidence = confidence + self.target_frame = target_frame + + self.node = yasmin_ros.logger_node + + camera_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + ) + + self.cam_info = None + self.data = None + self.image_msg = None + + image_sub = message_filters.Subscriber( + self.node, Image, self.image_topic, qos_profile=camera_qos + ) + + depth_sub = message_filters.Subscriber( + self.node, Image, self.depth_image_topic, qos_profile=camera_qos + ) + cam_info_sub = message_filters.Subscriber( + self.node, CameraInfo, self.depth_camera_info_topic, qos_profile=camera_qos + ) + + self.cache = message_filters.Cache(cam_info_sub) + + self.ts = message_filters.ApproximateTimeSynchronizer( + [image_sub, depth_sub], queue_size=10, slop=slop + ) + + self.ts.registerCallback(self.callback) + + def callback(self, image_msg, depth_msg): + self.data = (image_msg, depth_msg) + + def _create_req(self, blackboard): + self.data = None + self.image_msg = None + + deadline = time.time() + 30.0 + while self.data is None: + if time.time() > deadline: + self.node.get_logger().error( + f"Timed out waiting for synced rgb/depth frames. " + f"Check that {self.image_topic} and {self.depth_image_topic} are publishing and roughly synchronized." + ) + return "failed" + time.sleep(0.25) + + image_msg, depth_msg = self.data + + req = YoloPoseDetection3D.Request( + image_raw=image_msg, + depth_image=depth_msg, + depth_camera_info=self.cache.getLast(), + model=self.model, + confidence=self.confidence, + target_frame=self.target_frame, + ) + self.image_msg = image_msg + + return req + + def response_handler(self, blackboard, response): + yasmin.YASMIN_LOG_INFO(f"Got {len(response.detections)} detections") + if len(response.detections) == 0: + return "failed" + + for x, detection in enumerate(response.detections): + yasmin.YASMIN_LOG_INFO(f"Detection {x}") + for keypoint in detection.keypoints: + yasmin.YASMIN_LOG_INFO( + f"keypoint: {keypoint.keypoint_name}, point: {keypoint.point}" + ) + + blackboard["keypoint_detections_3d"] = response + blackboard["image_raw"] = self.image_msg + + return "succeeded" + + +def main(): + rclpy.init() + set_ros_loggers() + + yasmin.YASMIN_LOG_INFO("yasmin_detect3d_pose_test") + sm = StateMachine(outcomes=["succeeded", "failed"], handle_sigint=True) + bb = Blackboard() + + # def printKeypoints(blackboard): + # if len(blackboard["keypoint_detections_3d"].detections) == 0: + # return "failed" + + # for x, detection in enumerate(blackboard["keypoint_detections_3d"].detections): + # yasmin.YASMIN_LOG_INFO(f"Detection {x}") + # for keypoint in detection.keypoints: + # yasmin.YASMIN_LOG_INFO(f"keypoint: {keypoint.keypoint_name}, point: {keypoint.point}") + # return "succeeded" + + sm.add_state( + "DETECT3D_POSE", + DetectKeypoints3D(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + # sm.add_state( + # "PROCESS_RESPONSE", + # yasmin.CbState( + # outcomes=["succeeded", "failed"], + # callback=printKeypoints), + # transitions={ + # "succeeded": "succeeded", + # "failed": "failed", + # }, + # ) + outcome = sm(bb) + yasmin.YASMIN_LOG_INFO(outcome) + + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/skills/src/lasr_skills/eye_tracker.py b/skills/src/lasr_skills/eye_tracker.py index 39bd4916e..0c6b24a5c 100644 --- a/skills/src/lasr_skills/eye_tracker.py +++ b/skills/src/lasr_skills/eye_tracker.py @@ -1,8 +1,10 @@ import rclpy import yasmin_ros +import yasmin from rclpy.action import ActionClient from lasr_vision_interfaces.action import EyeTracker as EyeTrackerAction +from rclpy.callback_groups import ReentrantCallbackGroup class StartEyeTracker(yasmin_ros.ActionState): @@ -11,24 +13,36 @@ def __init__(self): action_name="/lasr_vision_eye_tracker/track_eyes", action_type=EyeTrackerAction, create_goal_handler=self.create_goal, - response_timeout=1.0, - maximum_retry=0, + feedback_handler=self.handle_feedback, + callback_group=ReentrantCallbackGroup(), ) + self.add_input_key("person_point") + + def handle_feedback(self, blackboard, feedback): + if feedback.running and blackboard["cancel_eye_tracker"]: + yasmin.YASMIN_LOG_INFO("Cancelling current eye tracker") + self.cancel_state() + def create_goal(self, blackboard): goal_msg = EyeTrackerAction.Goal() goal_msg.person_point = blackboard["person_point"] + blackboard["cancel_eye_tracker"] = False return goal_msg -class StopEyeTracker(yasmin_ros.ActionState): +class StopEyeTracker(yasmin.CbState): def __init__(self): super().__init__( - action_name="/lasr_vision_eye_tracker/track_eyes", - action_spec=EyeTrackerAction, - goal_cb=self.create_goal, - create_goal_handler=self.cancel_goal, + outcomes=["succeeded", "failed"], + callback=self.cancel_goal, ) - super().cancel_state() + def cancel_goal(self, blackboard): + try: + blackboard["cancel_eye_tracker"] = True + return "succeeded" + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Error raised: {e}") + return "failed" diff --git a/skills/src/lasr_skills/follow_person.py b/skills/src/lasr_skills/follow_person.py new file mode 100644 index 000000000..1d7e6bd9f --- /dev/null +++ b/skills/src/lasr_skills/follow_person.py @@ -0,0 +1,712 @@ +import math +import rclpy +from rclpy.time import Time +import time +import traceback + +import yasmin +from yasmin import State, Blackboard, StateMachine, Concurrence +import yasmin_ros +from yasmin_viewer import YasminViewerPub + +import tf2_ros + +from std_msgs.msg import Header +from geometry_msgs.msg import ( + PointStamped, + PoseWithCovarianceStamped, + Pose, + PolygonStamped, + Point32, +) +from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point +from shapely.geometry import Polygon as ShapelyPolygon + +from lasr_skills import ( + Detect3DInArea, + Wait, + PlayMotion, + Say, + ContinuousGoToLocation, + WaitForPersonInArea, + LookToPoint, + AskAndListen, +) + +# ----------------------------------------------- +""" + TRACKER LOGIC + +""" + + +class UpdateDetectionPolygon(State): + """ + Transforms baselink coordinates into new map polygon after the robot moves. + Writes: blackboard["polygon"] + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("polygon") + + self.node = yasmin_ros.logger_node + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + + self.base_footprint_polygon = [ + [3.5, 1.25], # Top Left + [3.5, -1.25], # Top Right + [-0.2, -1.75], # Bottom Right + [-0.2, 1.75], # Bottom Left + ] + + self.debug_pub = self.node.create_publisher( + PolygonStamped, "/person_follow/debug/polygon", 10 + ) + + def execute(self, blackboard): + + try: + transform = self.tf_buffer.lookup_transform( + "map", # Target frame + "base_footprint", # Source frame + rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + + debug_polygon = PolygonStamped() + debug_polygon.header.frame_id = "map" + debug_polygon.header.stamp = Time().to_msg() + + transformed_polygon = [] + for pt in self.base_footprint_polygon: + point_stamped = PointStamped() + point_stamped.header.frame_id = "base_footprint" + point_stamped.header.stamp = Time().to_msg() + point_stamped.point.x = pt[0] + point_stamped.point.y = pt[1] + point_stamped.point.z = 0.0 + + # Multiply the point by the transform matrix to get map coordinates + mapped_point = do_transform_point(point_stamped, transform) + transformed_polygon.append([mapped_point.point.x, mapped_point.point.y]) + + p = Point32(x=mapped_point.point.x, y=mapped_point.point.y, z=0.0) + debug_polygon.polygon.points.append(p) + + blackboard["polygon"] = ShapelyPolygon(transformed_polygon).buffer(0.05) + + self.debug_pub.publish(debug_polygon) + return "succeeded" + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"TF transform failed: {e}") + return "failed" + + +class EvaluateDetections(State): + """ + Evaluates people detected in the polygon frame. + Handles data association matching, stationary counting, and updating Nav2 blackboard targets. + """ + + def __init__(self, safe_distance=1, threshold=0.25, max_stationary=5): + # Outcomes mapping perfectly back to your TrackPerson state machine + super().__init__( + outcomes=["updated", "paused", "person_stationary", "person_lost"] + ) + + # Pulls detections from the blackboard populated by Detect3DInArea + self.add_input_key("detections_3d") + self.add_input_key("last_known") + # if using getpersonpoint after wait for person in area pass and remap + + self.add_output_key("location") + self.add_output_key("cancel_nav") + + self.node = yasmin_ros.logger_node + self.safe_distance = safe_distance + self.threshold = threshold + self.max_stationary = max_stationary + + # Internal Loop Memory Tracking + self.stationary_count = 0 + self.last_known = ( + None # Stores the last known (x, y) map coordinate of the person + ) + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + self.current_robot_point = None + + def create_goal_pose( + self, rx: float, ry: float, tx: float, ty: float, offset: bool = False + ) -> Pose: + # Calculate directional delta vectors + dx = tx - rx + dy = ty - ry + distance = math.sqrt(dx**2 + dy**2) + + # Goal Point + goal_x = tx + goal_y = ty + # If moving to a person stop before reaching thier point. + if offset and distance > self.safe_distance: + goal_x = tx - (dx / distance) * self.safe_distance + goal_y = ty - (dy / distance) * self.safe_distance + + # Calculate 2D planar heading angle (yaw) so the robot faces the target point + theta = math.atan2(dy, dx) + + # Build and populate the standard ROS 2 Pose message + goal_pose = Pose() + goal_pose.position.x = goal_x + goal_pose.position.y = goal_y + goal_pose.orientation.z = math.sin(theta / 2.0) + goal_pose.orientation.w = math.cos(theta / 2.0) + + return goal_pose + + def robot_point_cb(self, msg: PoseWithCovarianceStamped): + """Stores the raw underlying Pose data on message arrival""" + self.current_robot_point = msg.pose.pose.position + + def calc_distance_between_points(self, pointOne, pointTwo): + """Calculates Eculidian distance between 2 given point""" + return math.sqrt( + (float(pointOne.x) - float(pointTwo.x)) ** 2 + + (float(pointOne.y) - float(pointTwo.y)) ** 2 + ) + + def get_closest_person(self, detections): + """Iterates through points and finds best person to go to.""" + best_person = None + closest_d = float("inf") + + for person in detections: + distance = self.calc_distance_between_points(person.point, self.last_known) + if distance < closest_d: + closest_d = distance + best_person = person.point + return best_person + + def execute(self, blackboard: Blackboard): + # retrive robot's location in map + try: + transform = self.tf_buffer.lookup_transform( + "map", + "base_footprint", + rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + self.current_robot_point = transform.transform.translation + except Exception as e: + self.node.get_logger().warn(f"Waiting for map-base_footprint TF: {e}") + return "paused" + + # Handle blackboard data + if ( + "last_known" in blackboard.keys() + and self.last_known != blackboard["last_known"] + ): + self.last_known = blackboard["last_known"] + + detections = blackboard["detections_3d"] + if self.last_known is None: + if len(detections) > 0: + self.node.get_logger().info( + "First detection found. Initializing last_known." + ) + self.last_known = detections[0].point + else: + self.node.get_logger().warn("Waiting for first person detection...") + return "paused" + + if "cancel_nav" not in blackboard.keys(): + blackboard["cancel_nav"] = False + + # People found in frame + if len(detections) > 0: + personPoint = self.get_closest_person(detections) + + if len(detections) > 1: + self.node.get_logger().warn( + "Multiple people in polygon. Tracking closest to last known." + ) + else: + self.node.get_logger().info("Target locked.") + + distance_person_moved = self.calc_distance_between_points( + personPoint, self.last_known + ) + distance_robot_from_person = self.calc_distance_between_points( + personPoint, self.current_robot_point + ) + + if distance_person_moved > self.threshold: + # Person is actively moving — reset stationary counter and chase + self.last_known = personPoint + blackboard["last_known"] = personPoint + self.stationary_count = 0 + + if distance_robot_from_person > (self.safe_distance + 0.3): + blackboard["location"] = self.create_goal_pose( + self.current_robot_point.x, + self.current_robot_point.y, + self.last_known.x, + self.last_known.y, + offset=True, + ) + blackboard["stop_robot_requested"] = False + self.node.get_logger().warn("PERSON FOUND: UPDATING NAV GOAL") + return "updated" + else: + # Robot is already within safe_distance of the person + blackboard["stop_robot_requested"] = True + self.node.get_logger().warn("PERSON TOO CLOSE TO NAVIGATE") + return "paused" + else: + # Person has not moved significantly this tick + self.last_known = personPoint + blackboard["last_known"] = personPoint + + if distance_robot_from_person <= (self.safe_distance + 0.3): + # Robot is close and person is stationary → count up + self.stationary_count += 1 + blackboard["stop_robot_requested"] = True + self.node.get_logger().warn( + f"PERSON STATIONARY: {self.stationary_count}/{self.max_stationary}" + ) + else: + # Person hasn't moved but robot hasn't caught up yet — keep following + self.stationary_count = 0 + blackboard["location"] = self.create_goal_pose( + self.current_robot_point.x, + self.current_robot_point.y, + self.last_known.x, + self.last_known.y, + offset=True, + ) + blackboard["stop_robot_requested"] = False + + if self.stationary_count >= self.max_stationary: + self.node.get_logger().warn( + f"PERSON CONFIRMED STATIONARY AFTER {self.stationary_count} TICKS" + ) + blackboard["cancel_nav"] = True + return "person_stationary" + + return "paused" + + distance_old_from_robot = self.calc_distance_between_points( + self.last_known, self.current_robot_point + ) + if distance_old_from_robot > self.threshold + 0.3: + # Robot hasn't reached the last known position yet — keep driving + blackboard["location"] = self.create_goal_pose( + self.current_robot_point.x, + self.current_robot_point.y, + self.last_known.x, + self.last_known.y, + offset=False, + ) + blackboard["stop_robot_requested"] = False + self.node.get_logger().warn("NO PERSON: MOVING TO LAST KNOWN POSITION.") + return "updated" + else: + self.node.get_logger().warn("NO PERSON FOUND. ALREADY AT LAST KNOWN.") + blackboard["stop_robot_requested"] = True + return "person_lost" + + +class InitialRecovery(StateMachine): + class ScanForPerson(StateMachine): + def __init__(self, direction: str = "centre"): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("last_known") + + self.add_state( + "PLAYMOTION", + PlayMotion(f"look_{direction}"), + transitions={ + "succeeded": "DETECT_3D", + "aborted": "failed", + "canceled": "failed", + }, + ) + self.add_state( + "DETECT_3D", + Detect3DInArea(filter=["person"]), + transitions={ + "succeeded": "GET_PERSON_POINT", + "failed": "failed", + }, + ) + self.add_state( + "GET_PERSON_POINT", + GetPersonPoint(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("last_known") + self.add_output_key("cancel_nav") + + self.add_state( + "SAY_RECOVERING", + Say(text="I can't see you."), + transitions={ + "succeeded": "DETECT_3D_CENTER", + "aborted": "DETECT_3D_CENTER", + "canceled": "DETECT_3D_CENTER", + }, + ) + + self.add_state( + "DETECT_3D_CENTER", + self.ScanForPerson("center"), + transitions={ + "succeeded": "succeeded", + "failed": "DETECT_3D_LEFT", + }, + ) + + self.add_state( + "DETECT_3D_LEFT", + self.ScanForPerson("left"), + transitions={ + "succeeded": "succeeded", + "failed": "DETECT_3D_RIGHT", + }, + ) + + self.add_state( + "DETECT_3D_RIGHT", + self.ScanForPerson("right"), + transitions={ + "succeeded": "succeeded", + "failed": "CANCEL_NAV", + }, + ) + + self.add_state( + "CANCEL_NAV", + yasmin.CbState(outcomes=["done"], callback=self.cancel_nav), + transitions={ + "done": "failed", + }, + ) + + def cancel_nav(self, blackboard): + blackboard["cancel_nav"] = True + return "done" + + +class TrackPerson(StateMachine): + def __init__(self): + # Outcomes align perfectly with your main locate_and_follow_host.py plan + super().__init__(outcomes=["person_stationary", "person_lost", "failed"]) + + # 1. Update the Map Area + self.add_state( + "UPDATE_POLYGON", + UpdateDetectionPolygon(), + transitions={ + "succeeded": "DETECT_3D", + "failed": "failed", + }, + ) + self.add_state( + "DETECT_3D", + Detect3DInArea(filter=["person"]), + transitions={ + "succeeded": "GET_PERSON_POINT", + "failed": "failed", + }, + ) + self.add_state( + "GET_PERSON_POINT", + GetPersonPoint(), + transitions={ + "succeeded": "LOOK_AT_LAST_KNOWN", + "failed": "EVALUATE_DETECTIONS", + }, + ) + self.add_state( + "LOOK_AT_LAST_KNOWN", + LookToPoint(), + transitions={ + "succeeded": "EVALUATE_DETECTIONS", + "aborted": "EVALUATE_DETECTIONS", + "canceled": "EVALUATE_DETECTIONS", + "timeout": "EVALUATE_DETECTIONS", + }, + remappings={"pointstamped": "last_known_stamped"}, + ) + + # 3. Process Math & Blackboard Updates + self.add_state( + "EVALUATE_DETECTIONS", + EvaluateDetections(), + transitions={ + "updated": "WAIT", # Goal changed, pause briefly + "paused": "WAIT", # Too close, pause briefly + "person_stationary": "person_stationary", # Breakout: Reached destination + "person_lost": "BASIC_RECOVERY", # Breakout: Host vanished + }, + ) + + # 4. Short Loop Buffer + self.add_state( + "WAIT", + Wait(0.2), + transitions={"succeeded": "UPDATE_POLYGON", "failed": "failed"}, + ) + + self.add_state( + "BASIC_RECOVERY", + InitialRecovery(), + transitions={"succeeded": "EVALUATE_DETECTIONS", "failed": "person_lost"}, + ) + + +# ----------------------------------------------- +""" + Overall Following Logic +""" + + +class GetPersonPoint(State): + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("detections_3d") + self.add_input_key("last_known") + + self.add_output_key("last_known") + self.add_output_key("last_known_stamped") + + self.node = yasmin_ros.logger_node + + def calc_distance_between_points(self, pointOne, pointTwo): + """Calculates Eculidian distance between 2 given point""" + return math.sqrt( + (float(pointOne.x) - float(pointTwo.x)) ** 2 + + (float(pointOne.y) - float(pointTwo.y)) ** 2 + ) + + def execute(self, blackboard): + + try: + if not blackboard["detections_3d"]: + return "failed" + + if ( + "last_known" in blackboard.keys() + and blackboard["last_known"] is not None + ): + last_known = blackboard["last_known"] + else: + last_known = blackboard["detections_3d"][0].point + + closest_distance = float("inf") + for person in blackboard["detections_3d"]: + distance = self.calc_distance_between_points(person.point, last_known) + if distance < closest_distance: + closest_distance = distance + last_known = person.point + + yasmin.YASMIN_LOG_WARN( + f"DETECTIONS: {[detection.point for detection in blackboard['detections_3d']]} -- LAST_KNOWN: {last_known}" + ) + blackboard["last_known"] = last_known + blackboard["last_known_stamped"] = PointStamped( + header=Header( + frame_id="map", + stamp=Time().to_msg(), + ), + point=last_known, + ) + + return "succeeded" + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"The following error occured: {e}") + return "failed" + + +class FollowPerson(StateMachine): + def __init__(self): + # Outcomes align perfectly with your main locate_and_follow_host.py plan + super().__init__(outcomes=["succeeded", "failed"]) + + # Start of following + self.add_state( + "POST_NAV_1", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "UPDATE_POLYGON", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "UPDATE_POLYGON", + UpdateDetectionPolygon(), + transitions={ + "succeeded": "WAIT_FOR_HOST", + "failed": "failed", + }, + ) + self.add_state( + "WAIT_FOR_HOST", + WaitForPersonInArea(), # Empty to use blackboard polygon + transitions={ + "succeeded": "GET_PERSON_POINT", # Host is infront of the robot + "failed": "failed", # Still waiting on host + }, + ) + self.add_state( + "GET_PERSON_POINT", + GetPersonPoint(), + transitions={"succeeded": "PRE_NAV_1", "failed": "WAIT_FOR_HOST"}, + ) + + self.add_state( + "PRE_NAV_1", + PlayMotion("pre_navigation"), + transitions={ + "succeeded": "SAY_FOLLOW", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( # Do PRE_NAV_1 Before + "SAY_FOLLOW", + Say(text="I will now follow you. Lead the way slowly. "), + transitions={ + "succeeded": "TRACK_AND_NAVIGATE", + "aborted": "TRACK_AND_NAVIGATE", + "canceled": "TRACK_AND_NAVIGATE", + }, + ) + + # Tracking and Navigate + self.add_state( + "TRACK_AND_NAVIGATE", + Concurrence( + states={ + "tracker": TrackPerson(), + "navigator": ContinuousGoToLocation(), + }, + default_outcome="failed", + outcome_map={ + "person_stationary": { + "tracker": "person_stationary", + "navigator": "canceled", + }, + "person_lost": { + "tracker": "person_lost", + "navigator": "canceled", + }, + }, + ), + transitions={ + "person_stationary": "POST_NAV_2", + "person_lost": "CALL_LOST_PERSON_BACK", + "failed": "failed", + }, + ) + + self.add_state( + "CALL_LOST_PERSON_BACK", + Say( + text="I seam to have lost track of you. I will wait until you are back infront of me. " + ), + transitions={ + "succeeded": "POST_NAV_1", + "aborted": "POST_NAV_1", + "canceled": "POST_NAV_1", + }, + ) + + # BEFORE ASKING DO POSTNAV AND LOOK AT PERSON + self.add_state( + "POST_NAV_2", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "ASK_IF_ARRIVED", + "aborted": "failed", + "canceled": "failed", + }, + ) + + # Stationay Person + self.add_state( + "ASK_IF_ARRIVED", + AskAndListen( + tts_phrase="Say YES if we have arrived. NO if we have not.", + ), + transitions={ + "succeeded": "PROCESS_RESPONSE", + "failed": "ASK_IF_ARRIVED", + }, + ) + self.add_state( + "PROCESS_RESPONSE", + yasmin.CbState( + outcomes=["yes", "unknown", "no"], + callback=self.parse_arrival_confirmation, + ), + transitions={ + "yes": "succeeded", + "unknown": "FEEDBACK_RESPONSE", + "no": "PRE_NAV_1", + }, + ) + self.add_state( + "FEEDBACK_RESPONSE", + Say(text="I didn't quite understand that. "), + transitions={ + "succeeded": "ASK_IF_ARRIVED", + "aborted": "ASK_IF_ARRIVED", + "canceled": "ASK_IF_ARRIVED", + }, + ) + + def parse_arrival_confirmation(self, blackboard): + response = str(blackboard["transcribed_speech"]).lower() + yasmin.YASMIN_LOG_INFO(f"Recieved response: {response}") + + if "yes" in response: + return "yes" + elif "no" in response: + return "no" + else: + return "unknown" + + +def main(): + rclpy.init() + + yasmin_ros.set_ros_loggers() + + sm = FollowPerson() + sm.set_sigint_handler(True) + bb = Blackboard() + bb["z_sweep_min"] = -10 + bb["z_sweep_max"] = 50 + + YasminViewerPub(sm, "Follow_Person") + + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(outcome) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index 4ac04c95c..c665720e2 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -4,7 +4,7 @@ import yasmin from yasmin import StateMachine, State, Blackboard import yasmin_ros - +import time from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult @@ -37,12 +37,6 @@ def execute(self, blackboard): node = yasmin_ros.logger_node - for _c in ("position.x", "position.y", "position.z", - "orientation.x", "orientation.y", "orientation.z", "orientation.w"): - _p = f"{self.location_param}.{_c}" - if not node.has_parameter(_p): - node.declare_parameter(_p, 0.0) - goal_pose = Pose( position=Point( x=float( @@ -81,7 +75,7 @@ def execute(self, blackboard): self.navigator.goToPose(goal_stamped) while not self.navigator.isTaskComplete(): - rclpy.spin_once(self.navigator) + time.sleep(1) return ( "succeeded" @@ -118,4 +112,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/skills/src/lasr_skills/go_to_location_with_play_motion.py b/skills/src/lasr_skills/go_to_location_with_play_motion.py new file mode 100644 index 000000000..247aa3f74 --- /dev/null +++ b/skills/src/lasr_skills/go_to_location_with_play_motion.py @@ -0,0 +1,41 @@ +from lasr_skills import GoToLocation, PlayMotion + +import yasmin + + +class SafeGoToLocation(yasmin.StateMachine): + def __init__(self, location_pose=None, location_param=None): + super().__init__(outcomes=["succeeded", "failed"]) + + self.add_input_key("location") + self.add_input_key("motion_name") + + location_param = location_param.upper() + + state_name = f"GO_TO_{location_param}" + + self.add_state( + "PRE_NAV", + PlayMotion("pre_navigation"), + transitions={ + "succeeded": state_name, + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + state_name, + GoToLocation(location_param=location_param.lower()), + transitions={"succeeded": "POST_NAV", "failed": "failed"}, + ) + + self.add_state( + "POST_NAV", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "canceled": "failed", + }, + ) diff --git a/skills/src/lasr_skills/handover_object.py b/skills/src/lasr_skills/handover_object.py deleted file mode 100755 index b7b510191..000000000 --- a/skills/src/lasr_skills/handover_object.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python3 -import smach -import smach_ros - -import rclpy -from rclpy.node import Node -from std_srvs.srv import Empty - -from lasr_skills import Say, PlayMotion, Wait - -from ament_index_python.packages import get_package_share_directory -import yaml -import os - -from typing import Union - - -class HandoverObject(smach.StateMachine): - def __init__( - self, node: Node, object_name: Union[str, None] = None, vertical: bool = True - ): - - if object_name is not None: - super(HandoverObject, self).__init__(outcomes=["succeeded", "failed"]) - else: - smach.StateMachine.__init__( - self, outcomes=["succeeded", "failed"], input_keys=["object_name"] - ) - self.node = node - self.load_motion_params() - - with self: - smach.StateMachine.add( - "CLEAR_OCTOMAP", - smach_ros.ServiceState("clear_octomap", Empty), - transitions={ - "succeeded": "LOOK_LEFT", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "LOOK_LEFT", - PlayMotion(node=Node, motion_name="look_left"), - transitions={ - "succeeded": "LOOK_DOWN_LEFT", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "LOOK_DOWN_LEFT", - PlayMotion(node=Node, motion_name="look_down_left"), - transitions={ - "succeeded": "LOOK_RIGHT", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "LOOK_RIGHT", - PlayMotion(node=Node, motion_name="look_right"), - transitions={ - "succeeded": "LOOK_DOWN_RIGHT", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "LOOK_DOWN_RIGHT", - PlayMotion(node=Node, motion_name="look_down_right"), - transitions={ - "succeeded": "LOOK_DOWN_CENTRE", - "aborted": "failed", - "preempted": "failed", - }, - ) - # TODO: check whether the motion name for state LOOK_DOWN_CENTRE in ROS1 was look_centre is not look_down_centre on purpose and not just a mistake - smach.StateMachine.add( - "LOOK_DOWN_CENTRE", - PlayMotion(node=Node, motion_name="look_centre"), - transitions={ - "succeeded": "LOOK_CENTRE", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "LOOK_CENTRE", - PlayMotion(node=Node, motion_name="look_centre"), - transitions={ - "succeeded": "SAY_REACH_ARM", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "SAY_REACH_ARM", - Say( - node=Node, text="Please step back, I am going to reach my arm out." - ), - transitions={ - "succeeded": "REACH_ARM", - "aborted": "REACH_ARM", - "preempted": "REACH_ARM", - }, - ) - if vertical: - smach.StateMachine.add( - "REACH_ARM", - PlayMotion(node=Node, motion_name="reach_arm_vertical_gripper"), - transitions={ - "succeeded": "SAY_TAKE", - "aborted": "failed", - "preempted": "failed", - }, - ) - else: - smach.StateMachine.add( - "REACH_ARM", - PlayMotion(node=Node, motion_name="reach_arm_horizontal_gripper"), - transitions={ - "succeeded": "SAY_TAKE", - "aborted": "failed", - "preempted": "failed", - }, - ) - - if object_name is not None: - smach.StateMachine.add( - "SAY_TAKE", - Say( - node=Node, - text=f"Please grab the {object_name} in my hand. I will wait for a few seconds.", - ), - transitions={ - "succeeded": "OPEN_GRIPPER", - "aborted": "failed", - "preempted": "failed", - }, - ) - else: - smach.StateMachine.add( - "SAY_TAKE", - Say( - node=Node, - format_str="Please take the {} from my hand. I will wait for a few seconds.", - ), - transitions={ - "succeeded": "OPEN_GRIPPER", - "aborted": "failed", - "preempted": "failed", - }, - remapping={"placeholders": "object_name"}, - ) - - smach.StateMachine.add( - "WAIT_5", - Wait(5), - transitions={"succeeded": "FOLD_ARM", "failed": "OPEN_GRIPPER"}, - ) - - smach.StateMachine.add( - "OPEN_GRIPPER", - PlayMotion(node=Node, motion_name="open_gripper"), - transitions={ - "succeeded": "WAIT_5", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "FOLD_ARM", - PlayMotion(node=Node, motion_name="home"), - transitions={ - "succeeded": "succeeded", - "aborted": "failed", - "preempted": "failed", - }, - ) - - def load_motion_params(self): - package_path = get_package_share_directory("lasr_skills") - config_path = os.path.join(package_path, "config", "motion.yaml") - if os.path.exists(config_path): - with open(config_path, "r") as f: - params = yaml.safe_load(f) - for key, value in params.items(): - self.node.declare_parameters(key, value) - - -if __name__ == "__main__": - - rclpy.init() - node = rclpy.create_node("handover_object") - sm = HandoverObject(node=node, object_name="cola", vertical=True) - outcome = sm.execute() - node.get_logger().info(f"Outcome: {outcome}") - rclpy.shutdown() diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py index c24ca0f45..ce0b7fd24 100755 --- a/skills/src/lasr_skills/receive_object.py +++ b/skills/src/lasr_skills/receive_object.py @@ -1,225 +1,192 @@ #!/usr/bin/env python3 -import smach -import smach_ros import rclpy -from rclpy.node import Node + +import yasmin +from yasmin import StateMachine, Blackboard +import yasmin_ros +from yasmin_ros import ServiceState, ActionState + + +import yasmin +from yasmin import StateMachine, Blackboard +import yasmin_ros +from yasmin_ros import ServiceState, ActionState + from std_srvs.srv import Empty from lasr_skills import Say, PlayMotion, Wait -from ament_index_python.packages import get_package_share_directory -import yaml -import os - from typing import Union -class ReceiveObject(smach.StateMachine): - def __init__( - self, node: Node, object_name: Union[str, None] = None, vertical: bool = True - ): - - if object_name is not None: - super(ReceiveObject, self).__init__(outcomes=["succeeded", "failed"]) - else: - smach.StateMachine.__init__( - self, outcomes=["succeeded", "failed"], input_keys=["object_name"] - ) - self.node = node - self.load_motion_params() - """ - r = rospkg.RosPack() - els = rosparam.load_file( - os.path.join(r.get_path("lasr_skills"), "config", "motions.yaml") +class ClearOctomap(ServiceState): + def __init__(self): + super().__init__( + srv_type=Empty, + srv_name="/clear_octomap", + create_request_handler=self._create_request, ) - for param, ns in els: - rosparam.upload_params(ns, param) - """ - with self: - - smach.StateMachine.add( - "CLEAR_OCTOMAP", - smach_ros.ServiceState("clear_octomap", Empty), - transitions={ - "succeeded": "LOOK_LEFT", - "aborted": "failed", - "preempted": "failed", - }, - ) - smach.StateMachine.add( - "LOOK_LEFT", - PlayMotion(node=Node, motion_name="look_left"), - transitions={ - "succeeded": "LOOK_DOWN_LEFT", - "aborted": "failed", - "preempted": "failed", - }, - ) + def _create_request(self, blackboard): + return Empty.Request() - smach.StateMachine.add( - "LOOK_DOWN_LEFT", - PlayMotion(node=Node, motion_name="look_down_left"), - transitions={ - "succeeded": "LOOK_RIGHT", - "aborted": "failed", - "preempted": "failed", - }, - ) - smach.StateMachine.add( - "LOOK_RIGHT", - PlayMotion(motion_name="look_right"), - transitions={ - "succeeded": "LOOK_DOWN_RIGHT", - "aborted": "failed", - "preempted": "failed", - }, - ) +class ReceiveObject(StateMachine): + def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): - smach.StateMachine.add( - "LOOK_DOWN_RIGHT", - PlayMotion(node=Node, motion_name="look_down_right"), + super().__init__(outcomes=["succeeded", "failed"]) + if object_name is None: + self.add_input_key("object_name") + + self.add_state( + "CLEAR_OCTOMAP", + ClearOctomap(), + transitions={"succeeded": "LOOK_AROUND", "aborted": "failed"}, + ) + + self.add_state( + "LOOK_AROUND", + PlayMotion(motion_name="head_tour"), + transitions={ + "succeeded": "SAY_REACH_ARM", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "SAY_REACH_ARM", + Say( + text="I see you have a bag. Please step back, I am going to reach my arm out." + ), + transitions={ + "succeeded": "REACH_ARM", + "aborted": "REACH_ARM", + "canceled": "REACH_ARM", + }, + ) + + if vertical: + self.add_state( + "REACH_ARM", + PlayMotion(motion_name="reach_arm_vertical_gripper"), transitions={ - "succeeded": "LOOK_DOWN_CENTRE", + "succeeded": "OPEN_GRIPPER", "aborted": "failed", - "preempted": "failed", + "canceled": "failed", }, ) - # TODO: check whether the motion name for state LOOK_DOWN_CENTRE in ROS1 was look_centre is not look_down_centre on purpose and not just a mistake - smach.StateMachine.add( - "LOOK_DOWN_CENTRE", - PlayMotion(node=Node, motion_name="look_centre"), + else: + self.add_state( + "REACH_ARM", + PlayMotion(motion_name="reach_arm_horizontal_gripper"), transitions={ - "succeeded": "LOOK_CENTRE", + "succeeded": "OPEN_GRIPPER", "aborted": "failed", - "preempted": "failed", + "canceled": "failed", }, ) - smach.StateMachine.add( - "LOOK_CENTRE", - PlayMotion(node=Node, motion_name="look_centre"), + self.add_state( + "OPEN_GRIPPER", + PlayMotion(motion_name="open_gripper"), + transitions={ + "succeeded": "SAY_PLACE", + "aborted": "failed", + "canceled": "failed", + }, + ) + if object_name is not None: + self.add_state( + "SAY_PLACE", + Say( + text=f"I am ready to recieve the {object_name} in my hand. I will wait for a few seconds.", + ), transitions={ - "succeeded": "SAY_REACH_ARM", + "succeeded": "WAIT_5", "aborted": "failed", - "preempted": "failed", + "canceled": "failed", }, ) - - smach.StateMachine.add( - "SAY_REACH_ARM", + else: + self.add_state( + "SAY_PLACE", Say( - node=Node, text="Please step back, I am going to reach my arm out." + format_str="I am ready to recieve the {} in my hand. I will wait for a few seconds.", ), transitions={ - "succeeded": "REACH_ARM", - "aborted": "REACH_ARM", - "preempted": "REACH_ARM", - }, - ) - - if vertical: - smach.StateMachine.add( - "REACH_ARM", - PlayMotion(node=Node, motion_name="reach_arm_vertical_gripper"), - transitions={ - "succeeded": "OPEN_GRIPPER", - "aborted": "failed", - "preempted": "failed", - }, - ) - else: - smach.StateMachine.add( - "REACH_ARM", - PlayMotion(node=Node, motion_name="reach_arm_horizontal_gripper"), - transitions={ - "succeeded": "OPEN_GRIPPER", - "aborted": "failed", - "preempted": "failed", - }, - ) - - smach.StateMachine.add( - "OPEN_GRIPPER", - PlayMotion(node=Node, motion_name="open_gripper"), - transitions={ - "succeeded": "SAY_PLACE", + "succeeded": "WAIT_5", "aborted": "failed", - "preempted": "failed", + "canceled": "failed", }, + remapping={"placeholders": "object_name"}, ) + self.add_state( + "WAIT_5", + Wait(5), + transitions={ + "succeeded": "CLOSE_HALF_GRIPPER", + "failed": "CLOSE_HALF_GRIPPER", + }, + ) - if object_name is not None: - smach.StateMachine.add( - "SAY_PLACE", - Say( - node=Node, - text=f"Please place the {object_name} in my hand. I will wait for a few seconds.", - ), - transitions={ - "succeeded": "WAIT_5", - "aborted": "failed", - "preempted": "failed", - }, - ) - else: - smach.StateMachine.add( - "SAY_PLACE", - Say( - node=Node, - format_str="Please place the {} in my hand. I will wait for a few seconds.", - ), - transitions={ - "succeeded": "WAIT_5", - "aborted": "failed", - "preempted": "failed", - }, - remapping={"placeholders": "object_name"}, - ) - - smach.StateMachine.add( - "WAIT_5", - Wait(5), - transitions={ - "succeeded": "CLOSE_GRIPPER", - "failed": "CLOSE_GRIPPER", - }, - ) + # TODO: No longer a gripper server for this smach_ros.ServiceState("/parallel_gripper_controller/grasp", Empty) + # Alternatively: + # 1. https://docs.pal-robotics.com/sdk/24.09/actions/advanced_grasping-grasp.html but verify Fruity has the action server + # 2. /gripper_controller/incrementer service or + # 3. /gripper_controller/ action server - NOT AVAILABLE | use lasr_manipulation + + # self.add_state( + # "CLOSE_GRIPPER", + # smach_ros.ServiceState("parallel_gripper_controller/grasp", Empty), + # transitions={ + # "succeeded": "FOLD_ARM", + # "aborted": "failed", + # "canceled": "failed", + # }, + # ) + self.add_state( + "CLOSE_HALF_GRIPPER", # TEMPORARY REPLACEMENT + PlayMotion(motion_name="close_half"), + transitions={ + "succeeded": "FOLD_ARM", + "aborted": "failed", + "canceled": "failed", + }, + ) - smach.StateMachine.add( - "CLOSE_GRIPPER", - smach_ros.ServiceState("parallel_gripper_controller/grasp", Empty), - transitions={ - "succeeded": "FOLD_ARM", - "aborted": "failed", - "preempted": "failed", - }, - ) - smach.StateMachine.add( - "FOLD_ARM", - PlayMotion(node=Node, motion_name="cml_arm_away"), - transitions={ - "succeeded": "succeeded", - "aborted": "failed", - "preempted": "failed", - }, - ) + self.add_state( + "FOLD_ARM", + PlayMotion(motion_name="cml_arm_away"), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "canceled": "failed", + }, + ) - def load_motion_params(self): - package_path = get_package_share_directory("lasr_skills") - config_path = os.path.join(package_path, "config", "motion.yaml") - if os.path.exists(config_path): - with open(config_path, "r") as f: - params = yaml.safe_load(f) - for key, value in params.items(): - self.node.declare_parameters(key, value) +def main(): -if __name__ == "__main__": rclpy.init() - node = rclpy.create_node("receive_object") - sm = ReceiveObject(node=node, object_name="cola", vertical=True) - outcome = sm.execute() - node.get_logger().info(f"Outcome: {outcome}") - rclpy.shutdown() + + yasmin_ros.set_ros_loggers() + + try: + sm = ReceiveObject(object_name="bag") + bb = Blackboard() + + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(outcome) + except Exception as e: + yasmin.YASMIN_LOG_WARN(e) + + # Shutdown ROS 2 if it's running + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() + main() diff --git a/skills/src/lasr_skills/rotate.py b/skills/src/lasr_skills/rotate.py new file mode 100644 index 000000000..7bde3816c --- /dev/null +++ b/skills/src/lasr_skills/rotate.py @@ -0,0 +1,200 @@ +from typing import List, Union, Optional + +import rclpy +from rclpy.wait_for_message import wait_for_message + +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy + + +import yasmin +import yasmin_ros +from yasmin import Blackboard, StateMachine, State +from yasmin_ros import set_ros_loggers, ServiceState +from yasmin_viewer import YasminViewerPub + +from geometry_msgs.msg import ( + Pose, + PoseStamped, + PoseWithCovarianceStamped, + Quaternion, + Point, + PointStamped, +) +from lasr_skills import GoToLocation +from scipy.spatial.transform import Rotation as R +import numpy as np +import math + + +class Rotate(StateMachine): + class GetRotatedPose(State): + def __init__( + self, + angle: Optional[float] = None, + target_point: Optional[Point] = None, + mode: Optional[str] = None, # "angle" or "point" + ): + + super().__init__(outcomes=["succeeded", "failed"]) + + if angle is None: + self.add_input_key("angle") + self.angle = angle + + if target_point is None: + self.add_input_key("target_point") + self.target_point = target_point + + self.mode = mode + + self.add_output_key("target_pose") + + self.robot_pose = None + self.robot_pose_sub = yasmin_ros.logger_node.create_subscription( + PoseWithCovarianceStamped, + "/amcl_pose", + self.robot_point_cb, + QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ), + ) + + def robot_point_cb(self, msg: PoseWithCovarianceStamped): + self.robot_pose = msg + + def calcuate_pose_from_angle(self): + + current_orientation = np.array( + [ + self.robot_pose.pose.pose.orientation.x, + self.robot_pose.pose.pose.orientation.y, + self.robot_pose.pose.pose.orientation.z, + self.robot_pose.pose.pose.orientation.w, + ] + ) + + rot_matrix = R.from_quat(current_orientation) + new_rot_matrix = rot_matrix * R.from_euler("z", self.angle, degrees=True) + + matrix = new_rot_matrix.as_quat() + return Pose( + position=self.robot_pose.pose.pose.position, + orientation=Quaternion( + x=matrix[0], + y=matrix[1], + z=matrix[2], + w=matrix[3], + ), + ) + + def calculate_pose_from_point(self): + rx = self.robot_pose.pose.pose.position.x + ry = self.robot_pose.pose.pose.position.y + + dx = self.target_point.x - rx + dy = self.target_point.y - ry + target_yaw = math.atan2(dy, dx) + + new_rot_matrix = R.from_euler("z", target_yaw, degrees=False) + matrix = new_rot_matrix.as_quat() + + return Pose( + position=self.robot_pose.pose.pose.position, + orientation=Quaternion( + x=matrix[0], + y=matrix[1], + z=matrix[2], + w=matrix[3], + ), + ) + + def execute(self, blackboard): + + if ( + "angle" in blackboard.keys() + and blackboard["angle"] is not None + and self.angle is None + ): + self.angle = blackboard["angle"] + + if ( + "target_point" in blackboard.keys() + and blackboard["target_point"] is not None + and self.target_point is None + ): + if isinstance(blackboard["target_point"], PointStamped): + self.target_point = blackboard["target_point"].point + else: + self.target_point = blackboard["target_point"] + + goal = None + + if self.mode == "angle" and self.angle is not None: # Rotate using angle + goal = self.calcuate_pose_from_angle() + elif ( + self.mode == "point" and self.target_point is not None + ): # Rotate to face point + goal = self.calculate_pose_from_point() + else: # Not Specified + if self.angle is not None: + goal = self.calcuate_pose_from_angle() + elif self.target_point is not None: + goal = self.calculate_pose_from_point() + else: + yasmin.YASMIN_LOG_INFO( + "Rotation angle or target point not Specified" + ) + return "failed" + + if goal is not None: + blackboard["target_pose"] = goal + return "succeeded" + else: + yasmin.YASMIN_LOG_ERROR(f"Rotation Failed") + return "failed" + + def __init__( + self, + angle: Optional[float] = None, + target_point: Optional[Point] = None, + mode: Optional[str] = None, + ): + super().__init__(outcomes=["succeeded", "failed"]) + + self.add_input_key("target_point") + self.add_input_key("angle") + + self.add_state( + "GET_ROTATED_POSE", + self.GetRotatedPose(angle=angle, target_point=target_point, mode=mode), + transitions={"succeeded": "ROTATE", "failed": "failed"}, + ) + self.add_state( + "ROTATE", + GoToLocation(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + remappings={"location": "target_pose"}, + ) + + +def main(): + rclpy.init() + + yasmin_ros.set_ros_loggers() + + sm = Rotate(angle=180) + sm.set_sigint_handler(True) + bb = Blackboard() + + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(outcome) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/skills/src/lasr_skills/say.py b/skills/src/lasr_skills/say.py index ee70941e4..b0f927b43 100755 --- a/skills/src/lasr_skills/say.py +++ b/skills/src/lasr_skills/say.py @@ -5,7 +5,7 @@ import rclpy import os -HAS_TTS_MSGS: bool = True +HAS_TTS_MSGS: bool = False try: diff --git a/tasks/HRI/HRI/states/start_door_sm.py b/skills/src/lasr_skills/start_task.py similarity index 86% rename from tasks/HRI/HRI/states/start_door_sm.py rename to skills/src/lasr_skills/start_task.py index 193165cd6..53f12aa79 100644 --- a/tasks/HRI/HRI/states/start_door_sm.py +++ b/skills/src/lasr_skills/start_task.py @@ -10,7 +10,7 @@ from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped -from lasr_skills import DetectDoorOpening, GoToLocation +from lasr_skills import DetectDoorOpening, SafeGoToLocation, PlayMotion class StartDoorSM(StateMachine): # TODO: Rename to start_task and move to Skills @@ -20,19 +20,18 @@ def __init__( location: Union[Pose, None] = None, location_param: Union[str, None] = "start_pose", ): - super().__init__( - outcomes=["succeeded", "failed"], - ) + super().__init__(outcomes=["succeeded", "failed"]) self.add_state( "DETECT_DOOR_OPENING", DetectDoorOpening(), transitions={"door_opened": "GO_TO_START", "failed": "failed"}, ) + self.add_state( "GO_TO_START", - GoToLocation( - location=location, + SafeGoToLocation( + location_pose=location, location_param=location_param, ), transitions={"succeeded": "succeeded", "failed": "failed"}, diff --git a/skills/src/lasr_skills/vision/crop_image_3d.py b/skills/src/lasr_skills/vision/crop_image_3d.py index 4ece5b3f5..958ffc6c7 100644 --- a/skills/src/lasr_skills/vision/crop_image_3d.py +++ b/skills/src/lasr_skills/vision/crop_image_3d.py @@ -1,6 +1,5 @@ import rclpy from rclpy.node import Node -from rclpy.wait_for_message import wait_for_message from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy import yasmin @@ -14,6 +13,8 @@ from typing import Optional, List +import time + from geometry_msgs.msg import PoseWithCovarianceStamped from cv_bridge import CvBridge @@ -64,7 +65,7 @@ def __init__( self.crop_logic = crop_logic self.crop_type = crop_type self._bridge = CvBridge() - + self.node = yasmin_ros.logger_node self.debug_publisher = self.node.create_publisher( @@ -77,6 +78,19 @@ def __init__( ), ) + amcl_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) + + self.robot_pose_msg = None + + self.node.create_subscription( + PoseWithCovarianceStamped, "amcl_pose", self.pose_cb, qos_profile=amcl_qos + ) + if self.crop_type not in ["masked", "bbox"]: raise ValueError( f"Invalid crop_type: {self.crop_type}. Must be 'masked' or 'bbox'." @@ -86,33 +100,29 @@ def __init__( f"Invalid crop_logic: {self.crop_logic}. Must be 'nearest' or 'farthest'." ) + def pose_cb(self, msg): + self.robot_pose_msg = msg + def execute(self, blackboard): - yasmin.YASMIN_LOG_INFO('CROPPING OUR 3D HEHEHE') detections = blackboard["detections_3d"].detected_objects if not detections: yasmin.YASMIN_LOG_WARN("No 3D detections found.") return "failed" - # From: https://github.com/ros2/rclpy/blob/humble/rclpy/rclpy/wait_for_message.py - yasmin.YASMIN_LOG_INFO('WAITING FOR MSG') - qos = QoSProfile(depth=1, history=HistoryPolicy.KEEP_LAST, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL) - success, robot_pose_msg = wait_for_message( - msg_type=PoseWithCovarianceStamped, - node=self.node, - topic="/amcl_pose", - qos_profile=qos, - time_to_wait=10, - ) - yasmin.YASMIN_LOG_INFO('MSG RECEIVED') - if not success: - yasmin.YASMIN_LOG_WARN("Timed out waiting for robot pose.") - return "failed" + attempt = 0 + while self.robot_pose_msg is None: + if attempt > 5: + time.sleep(0.5) + attempt += 0.5 + else: + yasmin.YASMIN_LOG_WARN("Timed out waiting for robot pose.") + return "failed" # Pose in map frame, same as detected objects robot_x, robot_y, robot_z = ( - robot_pose_msg.pose.pose.position.x, - robot_pose_msg.pose.pose.position.y, - robot_pose_msg.pose.pose.position.z, + self.robot_pose_msg.pose.pose.position.x, + self.robot_pose_msg.pose.pose.position.y, + self.robot_pose_msg.pose.pose.position.z, ) rgb_image = self._bridge.imgmsg_to_cv2( diff --git a/skills/src/lasr_skills/vision/get_image.py b/skills/src/lasr_skills/vision/get_image.py index 3c4dcc855..35e61e982 100755 --- a/skills/src/lasr_skills/vision/get_image.py +++ b/skills/src/lasr_skills/vision/get_image.py @@ -5,57 +5,48 @@ import rclpy from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy -from rclpy.wait_for_message import wait_for_message - +import message_filters from typing import Optional from sensor_msgs.msg import Image, PointCloud2 +import time + class GetImage(State): """ State for reading an sensor_msgs Image message """ - def __init__(self, topic: Optional[str] = None): + def __init__(self, topic="head_front_camera/rgb/image_raw"): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("img_msg") self.add_output_key("img_msg") - self.camera_qos = QoSProfile( + self.node = yasmin_ros.logger_node + + camera_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, ) - - yasmin_ros.logger_node.declare_parameter( - "image_topic", "/head_front_camera/rgb/image_raw" - ) - self.topic = ( - topic - if topic - else yasmin_ros.logger_node.get_parameter("image_topic") - .get_parameter_value() - .string_value + + self.image_sub = message_filters.Subscriber( + self.node, Image, "head_front_camera/rgb/image_raw", camera_qos ) - def execute(self, blackboard): - # if not rclpy.ok(): - # rclpy.init() + self.cache = message_filters.Cache(self.image_sub) + def execute(self, blackboard): try: - msg = wait_for_message(Image, yasmin_ros.logger_node, self.topic, qos_profile=self.camera_qos) - if msg is not None: - blackboard["img_msg"] = msg - else: - blackboard["img_msg"] = None - if blackboard["img_msg"] is None: - return "failed" - + blackboard["img_msg"] = self.cache.getLast() + return "succeeded" except Exception as e: yasmin.YASMIN_LOG_ERROR(str(e)) return "failed" - return "succeeded" + + +# UNUSED THROUGHOUT WHOLE REPO, MAYBE DELETE????? class GetPointCloud(State): @@ -68,7 +59,7 @@ def __init__(self, topic: Optional[str] = None): self.add_input_key("pcl_msg") self.add_output_key("pcl_msg") - + self.camera_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, @@ -92,7 +83,10 @@ def execute(self, blackboard): try: blackboard["pcl_msg"] = None blackboard["pcl_msg"] = wait_for_message( - PointCloud2, yasmin_ros.logger_node, self.topic, qos_profile=self.camera_qos + PointCloud2, + yasmin_ros.logger_node, + self.topic, + qos_profile=self.camera_qos, ) if blackboard["pcl_msg"] is None: return "failed" @@ -102,6 +96,7 @@ def execute(self, blackboard): return "succeeded" +# ALSO NEVER USED, MAYBE DELETE AS WELL???? class GetImageAndPointCloud(State): def __init__(self): super().__init__(outcomes=["succeeded", "failed"]) @@ -110,7 +105,7 @@ def __init__(self): self.add_output_key("pcl_msg") self.add_output_key("img_msg") - + self.camera_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, diff --git a/skills/src/lasr_skills/wait_for_person.py b/skills/src/lasr_skills/wait_for_person.py index f998bdd2d..1114c7c83 100755 --- a/skills/src/lasr_skills/wait_for_person.py +++ b/skills/src/lasr_skills/wait_for_person.py @@ -10,10 +10,7 @@ def __init__( self, image_topic: str = "/head_front_camera/rgb/image_raw", ): - super().__init__( - outcomes=["succeeded", "failed"], - handle_sigint=True, - ) + super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detections") self.add_state( diff --git a/skills/src/lasr_skills/wait_for_person_in_area.py b/skills/src/lasr_skills/wait_for_person_in_area.py index dfdd007a7..e02990f9e 100644 --- a/skills/src/lasr_skills/wait_for_person_in_area.py +++ b/skills/src/lasr_skills/wait_for_person_in_area.py @@ -1,57 +1,76 @@ import rclpy +from typing import Union +import yasmin import yasmin_ros from yasmin import State, StateMachine -from lasr_skills import Detect3DInArea +from lasr_skills import Detect3DInArea, Wait from shapely import Polygon as ShapelyPolygon - class CheckForPerson(State): def __init__(self): super().__init__(outcomes=["done", "not_done"]) self.add_input_key("detections_3d") def execute(self, blackboard): - if len(blackboard["detections_3d"]): + people = len(blackboard["detections_3d"]) + + if people: + yasmin.YASMIN_LOG_INFO(f"Found {people} people in wait area.") return "done" - else: - return "not_done" + + return "not_done" class WaitForPersonInArea(StateMachine): - def __init__(self): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + def __init__( + self, + polygon: Union[ShapelyPolygon, None] = None, + polygon_param: Union[str, None] = None, + ): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("polygon") self.add_output_key("detections_3d") - node = yasmin_ros.logger_node + self.detection_polygon = None - top_left = rclpy.parameter.parameter_value_to_python( - node.get_parameter("door_polygon.top_left").get_parameter_value() - ) - top_right = rclpy.parameter.parameter_value_to_python( - node.get_parameter("door_polygon.top_right").get_parameter_value() - ) - bottom_left = rclpy.parameter.parameter_value_to_python( - node.get_parameter("door_polygon.bottom_left").get_parameter_value() - ) - bottom_right = rclpy.parameter.parameter_value_to_python( - node.get_parameter("door_polygon.bottom_right").get_parameter_value() - ) + if polygon: + self.detection_polygon = polygon + elif polygon_param: + node = yasmin_ros.logger_node + + top_left = rclpy.parameter.parameter_value_to_python( + node.get_parameter(f"{polygon_param}.top_left").get_parameter_value() + ) + top_right = rclpy.parameter.parameter_value_to_python( + node.get_parameter(f"{polygon_param}.top_right").get_parameter_value() + ) + bottom_left = rclpy.parameter.parameter_value_to_python( + node.get_parameter(f"{polygon_param}.bottom_left").get_parameter_value() + ) + bottom_right = rclpy.parameter.parameter_value_to_python( + node.get_parameter( + f"{polygon_param}.bottom_right" + ).get_parameter_value() + ) - door_polygon = ShapelyPolygon([top_left, top_right, bottom_right, bottom_left]) + self.detection_polygon = ShapelyPolygon( + [top_left, top_right, bottom_right, bottom_left] + ) self.add_state( "DETECT_PEOPLE_3D", Detect3DInArea( - area_polygon=door_polygon, + area_polygon=self.detection_polygon, filter=["person"], z_min=-10, z_max=10.0, ), transitions={"succeeded": "CHECK_FOR_PERSON", "failed": "failed"}, + remappings={"detections_3d": "detections_3d"}, ) self.add_state( "CHECK_FOR_PERSON", diff --git a/tasks/GPSR/GPSR/agent.py b/tasks/GPSR/GPSR/agent.py index b8d0efcf9..a13290e08 100644 --- a/tasks/GPSR/GPSR/agent.py +++ b/tasks/GPSR/GPSR/agent.py @@ -1,45 +1,212 @@ -#!/usr/bin/env python3 -"""LLM agent using Ollama for inference.""" +"""Local LLM + cloud LLM dispatch.""" -from typing import Optional +import asyncio +import json +import time +from concurrent.futures import FIRST_COMPLETED, ThreadPoolExecutor, wait +from typing import Callable, Optional import ollama +from GPSR.planner import run_planner, run_announce + + +def _cloud_query( + prompt: str, + system_prompt: str, + host: str, + port: int, + timeout_sec: float = 10.0, +) -> str: + async def _go(): + reader, writer = await asyncio.wait_for( + asyncio.open_connection(host, port), + timeout=timeout_sec, + ) + try: + msg = {"type": "query", "prompt": prompt} + if system_prompt: + msg["system_prompt"] = system_prompt + writer.write((json.dumps(msg) + "\n").encode()) + await writer.drain() + while True: + line = await asyncio.wait_for(reader.readline(), timeout=timeout_sec) + if not line: + raise ConnectionError("server closed connection") + resp = json.loads(line.decode().strip()) + if resp.get("type") == "answer": + return resp["text"] + if resp.get("type") == "error": + raise RuntimeError(resp.get("text", "cloud error")) + finally: + writer.close() + await writer.wait_closed() + + return asyncio.run(_go()) + class Agent: def __init__( self, model: str = "llama3.2", - system_prompt: Optional[str] = None, host: str = "http://localhost:11434", + cloud_host: str = "", + cloud_port: int = 8765, + cloud_timeout_sec: float = 20.0, + system_prompt: Optional[str] = None, + use_cloud: bool = False, ): - self.model = model - self.system_prompt = system_prompt - self.client = ollama.Client(host=host) - - def query(self, prompt: str, system_prompt: Optional[str] = None) -> str: - messages = [] - sp = system_prompt or self.system_prompt - if sp: - messages.append({"role": "system", "content": sp}) - messages.append({"role": "user", "content": prompt}) - - response = self.client.chat(model=self.model, messages=messages) - return response["message"]["content"] + self._model = model + self._host = host + self._system_prompt = system_prompt + self._cloud_host = cloud_host.strip() + self._cloud_port = cloud_port + self._cloud_timeout = cloud_timeout_sec + self._use_cloud = use_cloud + self._client = None if use_cloud else ollama.Client(host=host, timeout=300.0) + + @property + def cloud_enabled(self) -> bool: + return bool(self._cloud_host) + + @classmethod + def from_node(cls, node, log: Optional[Callable[[str], None]] = None) -> "Agent": + agent = cls( + model=node.get_parameter("llm_model").value, + host=node.get_parameter("llm_host").value, + cloud_host=node.get_parameter("cloud_host").value, + cloud_port=node.get_parameter("cloud_port").value, + cloud_timeout_sec=node.get_parameter("cloud_timeout_sec").value, + ) + if log: + cloud = "off" if not agent.cloud_enabled else agent._cloud_host + log(f"Agent ready | local={agent._model} | cloud={cloud}") + return agent def query_json(self, prompt: str, system_prompt: Optional[str] = None) -> str: - messages = [] - sp = system_prompt or self.system_prompt - if sp: - messages.append({"role": "system", "content": sp}) - messages.append({"role": "user", "content": prompt}) - - response = self.client.chat( - model=self.model, - messages=messages, + sp = system_prompt or self._system_prompt or "" + if self._use_cloud: + return _cloud_query( + prompt, sp, self._cloud_host, self._cloud_port, self._cloud_timeout + ) + response = self._client.chat( + model=self._model, + messages=[{"role": "user", "content": prompt}], format="json", ) return response["message"]["content"] - def query_vision(self, prompt: str, image_path: str) -> str: - raise NotImplementedError("VLM support not yet implemented") + def _clone(self, use_cloud: bool) -> "Agent": + return Agent( + model=self._model, + host=self._host, + cloud_host=self._cloud_host, + cloud_port=self._cloud_port, + cloud_timeout_sec=self._cloud_timeout, + system_prompt=self._system_prompt, + use_cloud=use_cloud, + ) + + def plan( + self, + world: dict, + command: str, + log: Optional[Callable[[str], None]] = None, + ) -> dict: + if not self.cloud_enabled: + plan = run_planner(self._clone(False), world, command) + plan["source"] = "local" + return plan + + deadline = time.monotonic() + self._cloud_timeout + t0 = time.monotonic() + pool = ThreadPoolExecutor(max_workers=2) + local_f = pool.submit(run_planner, self._clone(False), world, command) + cloud_f = pool.submit(run_planner, self._clone(True), world, command) + local_plan = None + cloud_failed = False + + try: + pending = {local_f, cloud_f} + while pending: + remaining = deadline - time.monotonic() + if remaining <= 0: + break + done, pending = wait( + pending, timeout=remaining, return_when=FIRST_COMPLETED + ) + for future in done: + if future is cloud_f: + try: + plan = future.result() + except Exception as e: + cloud_failed = True + if log: + log(f"Cloud unavailable ({e}) — using local") + if local_plan is not None: + local_plan["source"] = "local" + return local_plan + break + plan["source"] = "cloud" + if log: + log( + f"Cloud planner finished in {time.monotonic() - t0:.1f}s" + ) + return plan + try: + local_plan = future.result() + if cloud_f.done(): + try: + plan = cloud_f.result() + plan["source"] = "cloud" + if log: + log( + f"Cloud planner finished in {time.monotonic() - t0:.1f}s" + ) + return plan + except Exception as e: + cloud_failed = True + if log: + log(f"Cloud unavailable ({e}) — using local") + local_plan["source"] = "local" + return local_plan + if cloud_failed: + if log: + log("Using local plan (cloud unavailable)") + local_plan["source"] = "local" + return local_plan + if log: + log( + "Local planner finished first — waiting for cloud until timeout" + ) + except Exception as e: + if log: + log(f"Local planner failed: {e}") + + if local_plan is not None: + local_plan["source"] = "local" + return local_plan + + plan = local_f.result() + plan["source"] = "local" + if log: + log("Using local plan") + return plan + finally: + pool.shutdown(wait=False, cancel_futures=False) + + def announce( + self, + command: str, + plan_description: str, + steps: list, + source: str = "local", + log: Optional[Callable[[str], None]] = None, + ) -> str: + backend = self._clone(use_cloud=(source == "cloud")) + try: + return run_announce(backend, command, plan_description, steps) + except Exception as e: + if log: + log(f"Announce failed ({e})") + return "" diff --git a/tasks/GPSR/GPSR/planner.py b/tasks/GPSR/GPSR/planner.py new file mode 100644 index 000000000..2a161bc0f --- /dev/null +++ b/tasks/GPSR/GPSR/planner.py @@ -0,0 +1,106 @@ +"""Skill selector → refiner → planner pipeline.""" + +import json +import time + +from GPSR.prompts import ( + ANNOUNCE_PLAN_PROMPT, + PLANNER_PROMPT, + SKILL_REFINER_PROMPT, + SKILL_SELECTOR_PROMPT, + parse_json as _parse_json, +) +from GPSR.world import format_objects, format_people, selected_skill_lines + + +class SkillSelectorError(Exception): + """Stage 1 failed — no plan available.""" + + +def _fail_safe(text: str = "I could not generate a plan for that command.") -> dict: + return { + "skill": "say", + "skill_args": {"text": text}, + "plan_description": text, + "steps": [{"skill": "say", "args": {"text": text}}], + } + + +def run_planner(backend, world: dict, command: str) -> dict: + """Run the full pipeline using a backend with query_json().""" + t0 = time.perf_counter() + command = command.strip() + # Retrive skill lines from world + skill_lines = world["skill_lines"] + try: + raw = backend.query_json( + SKILL_SELECTOR_PROMPT.format(skill_lines=skill_lines, command=command), + ) + selection = _parse_json(raw) + except Exception as e: + raise SkillSelectorError(str(e)) from e + + can_do = selection.get("can_do", True) + reason = selection.get("reason", "") + skills = selection.get("selected_skills", []) + + # If the command cannot be done, return a fail safe USE LLM TO DECIDE IF THE COMMAND CAN BE DONE + if not can_do: + result = _fail_safe(reason or "I'm sorry, I don't know how to do that.") + result["elapsed_sec"] = round(time.perf_counter() - t0, 2) + return result + # Correct the skills using the LLM making sure that the skills respect the rules and respect the style + try: + raw = backend.query_json( + SKILL_REFINER_PROMPT.format( + command=command, + selected_skills=json.dumps(skills), + ), + ) + refined = _parse_json(raw) + skills = refined.get("refined_skills", skills) + except Exception: + pass + + try: + raw = backend.query_json( + PLANNER_PROMPT.format( + general_knowledge=world["general_knowledge"], + selected_skill_lines=selected_skill_lines(skills, skill_lines), + selected_skill_names=", ".join(skills), + locations=", ".join(world["locations"].keys()) or "none", + objects=format_objects(world["objects"]), + people=format_people(world["people"]), + command=command, + ), + ) + parsed = _parse_json(raw) + steps = parsed.get("steps", []) + plan_description = parsed.get("plan_description", "") + if not steps: + raise ValueError("empty steps") + except Exception: + result = _fail_safe() + result["elapsed_sec"] = round(time.perf_counter() - t0, 2) + return result + + return { + "skill": steps[0]["skill"], + "skill_args": steps[0].get("args", {}), + "plan_description": plan_description, + "steps": steps, + "elapsed_sec": round(time.perf_counter() - t0, 2), + } + + +def run_announce(backend, command: str, plan_description: str, steps: list) -> str: + """One LLM call: turn a plan into a spoken announcement sentence.""" + raw = backend.query_json( + ANNOUNCE_PLAN_PROMPT.format( + command=command, + plan_description=plan_description, + steps_json=json.dumps(steps), + ), + ) + parsed = _parse_json(raw) + return (parsed.get("announcement") or "").strip() diff --git a/tasks/GPSR/GPSR/prompts.py b/tasks/GPSR/GPSR/prompts.py new file mode 100644 index 000000000..d5ee26dd5 --- /dev/null +++ b/tasks/GPSR/GPSR/prompts.py @@ -0,0 +1,241 @@ +import json + +# TODO: add memory to the examples +# TODO: Reduce the number of examples to the most important ones now with the local LLM is the good solution they are good when we provide a lot of examples but +# with memory and in case of CLOUD LLM we can reduce the number of examples. a test should be done to check the performance reducing the number of examples with gemma3. + +SKILL_SELECTOR_PROMPT = """You are the skill selector for a robot. +Given a command, pick which skills from the list below are needed to execute it. +Output ONLY JSON. No explanation. + +Available skills: +{skill_lines} + +Output schema: +{{ + "can_do": true, + "reason": "", + "selected_skills": ["skill_name", ...] +}} + +RULES: +- selected_skills contains ONLY the skill names (e.g. "find_object", "go_to_location"), never arguments or signatures (never "find_object(apple, kitchen)"). +- can_do=false ONLY when NO skill matches the kind of action (e.g. cooking, flying). +- Missing locations/objects/people are NOT a reason for can_do=false — the planner handles those and manages the different cases. +- Any question, greeting, or request for information: can_do=true, selected_skills=["say"]. +- If the command needs navigation AND another action, include "go_to_location" in selected_skills. + +EXAMPLES: +Command: find the apple in the kitchen +JSON: {{"can_do": true, "reason": "need to navigate and search", "selected_skills": ["go_to_location", "find_object"]}} + +Command: count the people in the living room +JSON: {{"can_do": true, "reason": "need to navigate and count", "selected_skills": ["go_to_location", "count_people"]}} + +Command: how many drinks are in the kitchen +JSON: {{"can_do": true, "reason": "need to navigate and count objects", "selected_skills": ["go_to_location", "count_objects"]}} + +Command: tell me the name of the person in the bedroom +JSON: {{"can_do": true, "reason": "need to navigate and get info", "selected_skills": ["go_to_location", "get_person_info"]}} + +Command: what is the biggest object on the table +JSON: {{"can_do": true, "reason": "need to navigate and find by property", "selected_skills": ["go_to_location", "find_object_by_property"]}} + +Command: follow the person until they stop +JSON: {{"can_do": true, "reason": "follow skill", "selected_skills": ["follow_person"]}} + +Command: follow morgan to the exit +JSON: {{"can_do": true, "reason": "follow skill", "selected_skills": ["follow_person"]}} + +Command: say hello +JSON: {{"can_do": true, "reason": "just speak", "selected_skills": ["say"]}} + +Command: what is your team affiliation +JSON: {{"can_do": true, "reason": "answer with known info", "selected_skills": ["say"]}} + +Command: what day is it today +JSON: {{"can_do": true, "reason": "answer with known info", "selected_skills": ["say"]}} + +Command: introduce yourself +JSON: {{"can_do": true, "reason": "just speak", "selected_skills": ["say"]}} + +Command: make me a sandwich +JSON: {{"can_do": false, "reason": "no skill for cooking on the available skills", "selected_skills": []}} + +Command: {command} +JSON: """ + +# To add some rules on the plan a pick up action should be followed by a go to location and a place object action. a give to person action should be followed by a go to location action. +SKILL_REFINER_PROMPT = """You are a robot skill checker. +You receive a list of skills chosen for a command and must fix missing dependencies. +Output ONLY JSON. No explanation. + +DEPENDENCY RULES (apply all): +1. pick_up requires find_object before it — if find_object is missing, add it. +2. place_object requires pick_up before it — if pick_up is missing, add it. +3. give_to_person requires pick_up before it — if pick_up is missing, add it. +4. find_object, find_person, count_objects, count_people, get_person_info, find_object_by_property require go_to_location before them — if go_to_location is missing, add it. +5. guide_person and follow_person do NOT require go_to_location — never add it for those alone. +6. Output skills in correct execution order. +7. Never remove a skill from the input list — only add missing dependencies. +8. Keep skill names exactly as given (bare names, no arguments). + +Output schema: +{{ + "refined_skills": ["skill_name", ...] +}} + +EXAMPLES: + +Command: pick up the apple +Input skills: ["pick_up"] +JSON: {{"refined_skills": ["go_to_location", "find_object", "pick_up"]}} + +Command: bring the cola from the kitchen to the bedroom +Input skills: ["go_to_location", "pick_up", "place_object"] +JSON: {{"refined_skills": ["go_to_location", "find_object", "pick_up", "go_to_location", "place_object"]}} + +Command: count the apples in the kitchen +Input skills: ["count_objects"] +JSON: {{"refined_skills": ["go_to_location", "count_objects"]}} + +Command: how many people are waving in the living room +Input skills: ["count_people"] +JSON: {{"refined_skills": ["go_to_location", "count_people"]}} + +Command: tell me the name of the person in the bedroom +Input skills: ["get_person_info"] +JSON: {{"refined_skills": ["go_to_location", "get_person_info"]}} + +Command: find the apple in the kitchen and bring it to charlie +Input skills: ["go_to_location", "find_object", "pick_up", "give_to_person"] +JSON: {{"refined_skills": ["go_to_location", "find_object", "pick_up", "give_to_person"]}} + +Command: give the cola to robin +Input skills: ["go_to_location", "find_person", "pick_up", "give_to_person"] +JSON: {{"refined_skills": ["go_to_location", "find_object", "pick_up", "find_person", "give_to_person"]}} + +Command: guide charlie from the kitchen to the living room +Input skills: ["guide_person"] +JSON: {{"refined_skills": ["guide_person"]}} + +Command: escort robin from the bedroom to the office +Input skills: ["guide_person"] +JSON: {{"refined_skills": ["guide_person"]}} + +Command: say hello +Input skills: ["say"] +JSON: {{"refined_skills": ["say"]}} + +Command: {command} +Input skills: {selected_skills} +JSON: """ + +# TODO: check the rules when we will have the final files +PLANNER_PROMPT = """You are a robot planner. Output ONE JSON plan using only the given skills. +General knowledge: {general_knowledge} +Known locations: {locations} +Known objects: {objects} +Known people: {people} + +RULES: +- Known locations are ONLY: {locations}. Furniture, appliances, and fixtures (sofa, bathroom, waste basket, refrigerator, coatrack, garage, sink, shelf, etc.) are NOT valid locations. +- FIRST check every room/place mentioned in the command. If ANY of them is NOT in known locations: output ONLY a single say step refusing. Do not plan any other steps. +- Only after confirming all locations are known: use ALL selected skills in the plan. +- find_object and find_person can search for ANY object/person, even if not in the known lists — do NOT refuse for unknown objects when find_object is selected. +- Fill args from the command and known world. +- say text contains only the spoken words. + +Skills available for this command: +{selected_skill_lines} + +EXAMPLES: +Command: go to the bathroom | Skills: go_to_location | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "bathroom unknown", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, the bathroom is not on my map."}}}}]}} + +Command: locate a snack in the bathroom | Skills: go_to_location, find_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "bathroom unknown", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, the bathroom is not on my map."}}}}]}} + +Command: put the pringles on the sofa | Skills: go_to_location, find_object, pick_up, place_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "sofa is not a known location", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, the sofa is not a place I can navigate to."}}}}]}} + +Command: fetch the apple and place it in the bathroom | Skills: go_to_location, find_object, pick_up, place_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "bathroom is not a known location", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, the bathroom is not on my map."}}}}]}} + +Command: fetch the pringles and put them on the sofa | Skills: go_to_location, find_object, pick_up, place_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "sofa is not a known location", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, the sofa is not a place I can navigate to."}}}}]}} + +Command: go to the waste basket then take the sponge and put it on the refrigerator | Skills: go_to_location, find_object, pick_up, place_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "waste basket and refrigerator are not known locations", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, waste basket and refrigerator are not on my map."}}}}]}} + +Command: lead Simone from the coatrack to the bathroom | Skills: guide_person | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "coatrack and bathroom are not known locations", "steps": [{{"skill": "say", "args": {{"text": "I'm sorry, the coatrack and bathroom are not on my map."}}}}]}} + +Command: find a pizza in the kitchen | Skills: go_to_location, find_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "go to kitchen and search for pizza", "steps": [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}, {{"skill": "find_object", "args": {{"object": "pizza", "location": "kitchen"}}}}]}} + +Command: find the cola in the kitchen | Skills: go_to_location, find_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "go to kitchen and find cola", "steps": [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}, {{"skill": "find_object", "args": {{"object": "cola", "location": "kitchen"}}}}]}} + +Command: count the apples in the office | Skills: go_to_location, count_objects | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "go to office and count apples", "steps": [{{"skill": "go_to_location", "args": {{"location": "office"}}}}, {{"skill": "count_objects", "args": {{"object": "apple", "location": "office"}}}}]}} + +Command: how many people waving in the living room | Skills: go_to_location, count_people | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "go to living room and count people", "steps": [{{"skill": "go_to_location", "args": {{"location": "living room"}}}}, {{"skill": "count_people", "args": {{"gesture": "waving", "location": "living room"}}}}]}} + +Command: bring the cola from the kitchen to the bedroom | Skills: go_to_location, find_object, pick_up, place_object | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "fetch cola and bring to bedroom", "steps": [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}, {{"skill": "find_object", "args": {{"object": "cola", "location": "kitchen"}}}}, {{"skill": "pick_up", "args": {{"object": "cola"}}}}, {{"skill": "go_to_location", "args": {{"location": "bedroom"}}}}, {{"skill": "place_object", "args": {{"location": "bedroom"}}}}]}} + +Command: meet Jane in the kitchen and escort her to the bedroom | Skills: go_to_location, find_person, guide_person | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "find Jane in kitchen then escort to bedroom", "steps": [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}, {{"skill": "find_person", "args": {{"name": "jane", "location": "kitchen"}}}}, {{"skill": "guide_person", "args": {{"name": "jane", "start": "kitchen", "end": "bedroom"}}}}]}} + +Command: find a person in the kitchen and say hi | Skills: go_to_location, find_person, say | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "go to kitchen find person and say hi", "steps": [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}, {{"skill": "find_person", "args": {{"location": "kitchen"}}}}, {{"skill": "say", "args": {{"text": "Hi!"}}}}]}} + +Command: guide charlie from kitchen to living room | Skills: guide_person | Known locations: bedroom, kitchen, living room, office +Plan: {{"plan_description": "guide charlie to living room", "steps": [{{"skill": "guide_person", "args": {{"name": "charlie", "start": "kitchen", "end": "living room"}}}}]}} + +Command: {command} | Skills: {selected_skill_names} | Known locations: {locations} +Plan: """ + +# JUST TO REPRAHSE THE PLAN IN A NATURAL WAY BEFORE EXECUTING IT +ANNOUNCE_PLAN_PROMPT = """You are a robot assistant. +Turn the plan below into ONE spoken announcement listing every step in order. +Start with "Here is my plan." then say Step 1, Step 2, ... Step N — one short phrase per step derived from the skill and args. +Use only words that will be spoken aloud. No bullet points or JSON in the announcement. + +User command: {command} +Plan summary: {plan_description} +Steps: {steps_json} + +Output ONLY JSON: +{{"announcement": ""}} + +EXAMPLES: +Command: bring the cola from the kitchen to the bedroom +Plan summary: fetch cola and bring to bedroom +Steps: [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}, {{"skill": "find_object", "args": {{"object": "cola", "location": "kitchen"}}}}, {{"skill": "pick_up", "args": {{"object": "cola"}}}}, {{"skill": "go_to_location", "args": {{"location": "bedroom"}}}}, {{"skill": "place_object", "args": {{"location": "bedroom"}}}}] +JSON: {{"announcement": "Here is my plan. Step 1: go to the kitchen. Step 2: find the cola. Step 3: pick up the cola. Step 4: go to the bedroom. Step 5: place the cola in the bedroom."}} + +Command: go to the kitchen +Plan summary: go to kitchen +Steps: [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}] +JSON: {{"announcement": "Here is my plan. Step 1: go to the kitchen."}} + +Command: locate the standing person in the office +Plan summary: go to office and find standing person +Steps: [{{"skill": "go_to_location", "args": {{"location": "office"}}}}, {{"skill": "find_person", "args": {{"pose": "standing", "location": "office"}}}}] +JSON: {{"announcement": "Here is my plan. Step 1: go to the office. Step 2: find the standing person."}} + +Command: {command} +Plan summary: {plan_description} +Steps: {steps_json} +JSON: """ + + +def parse_json(raw: str) -> dict: + start = raw.find("{") + end = raw.rfind("}") + 1 + if start == -1 or end == 0: + return {} + return json.loads(raw[start:end]) diff --git a/tasks/GPSR/GPSR/state_machine.py b/tasks/GPSR/GPSR/state_machine.py index 4ad1486fd..45c961352 100644 --- a/tasks/GPSR/GPSR/state_machine.py +++ b/tasks/GPSR/GPSR/state_machine.py @@ -1,22 +1,23 @@ #!/usr/bin/env python3 +import os +import sys + import rclpy import yasmin import yasmin_ros +from ament_index_python.packages import get_package_share_directory from rclpy.node import Node from threading import Thread - -try: - from rclpy.executors import EventsExecutor as Executor -except ImportError: - from rclpy.executors import MultiThreadedExecutor as Executor - -from GPSR.states import AnnouncePlan, DispatchSkill, QueryLLM, create_input_state +from rclpy.executors import MultiThreadedExecutor as Executor +from GPSR.states import DispatchSkill, KeyboardInputState, ListenState, QueryLLM -def _declare_param_if_needed(node, name, default): - if not node.has_parameter(name): - node.declare_parameter(name, default) +def _ensure_params_file() -> None: + if any(a == "--params-file" for a in sys.argv): + return + params = os.path.join(get_package_share_directory("GPSR"), "config", "params.yaml") + sys.argv.extend(["--ros-args", "--params-file", params]) class GPSRNode(Node): @@ -33,30 +34,28 @@ def __init__(self): def main(args=None): + _ensure_params_file() rclpy.init(args=args) node = GPSRNode() - _declare_param_if_needed(node, "llm_model", "llama3.2") - _declare_param_if_needed(node, "llm_host", "http://localhost:11434") - _declare_param_if_needed(node, "locations_package", "GPSR") - _declare_param_if_needed(node, "locations_file", "config/locations.yaml") - _declare_param_if_needed(node, "simulation", False) - _declare_param_if_needed(node, "input_mode", "keyboard") - _declare_param_if_needed(node, "input_prompt", "Enter command: ") - - input_mode = node.get_parameter("input_mode").value + input_mode = node.get_parameter("input_mode").value.strip().lower() simulation = node.get_parameter("simulation").value node.get_logger().info( f"Starting GPSR state machine (input_mode={input_mode}, simulation={simulation})..." ) + if input_mode == "keyboard": + wait_for_command = KeyboardInputState(node) + elif input_mode in ("mic", "microphone"): + wait_for_command = ListenState(node) + yasmin_ros.set_ros_loggers(node) sm = yasmin.StateMachine(outcomes=["succeeded", "failed"], handle_sigint=True) sm.add_state( "WAIT_FOR_COMMAND", - create_input_state(node), + wait_for_command, transitions={ "succeeded": "QUERY_LLM", "aborted": "WAIT_FOR_COMMAND", @@ -65,16 +64,9 @@ def main(args=None): sm.add_state( "QUERY_LLM", QueryLLM(node), - transitions={ - "succeeded": "ANNOUNCE_PLAN", - "failed": "WAIT_FOR_COMMAND", - }, - ) - sm.add_state( - "ANNOUNCE_PLAN", - AnnouncePlan(node), transitions={ "succeeded": "DISPATCH_SKILL", + "failed": "WAIT_FOR_COMMAND", }, ) sm.add_state( diff --git a/tasks/GPSR/GPSR/states/__init__.py b/tasks/GPSR/GPSR/states/__init__.py index 0ba4e0c88..7a2a7fea9 100644 --- a/tasks/GPSR/GPSR/states/__init__.py +++ b/tasks/GPSR/GPSR/states/__init__.py @@ -1,15 +1,11 @@ -from GPSR.states.input_state import create_input_state from GPSR.states.keyboard_input import KeyboardInputState from GPSR.states.listen import ListenState from GPSR.states.query_llm import QueryLLM from GPSR.states.dispatch_skill import DispatchSkill -from GPSR.states.announce_plan import AnnouncePlan __all__ = [ - "create_input_state", "KeyboardInputState", "ListenState", "QueryLLM", "DispatchSkill", - "AnnouncePlan", ] diff --git a/tasks/GPSR/GPSR/states/announce_plan.py b/tasks/GPSR/GPSR/states/announce_plan.py deleted file mode 100644 index ae190f2b2..000000000 --- a/tasks/GPSR/GPSR/states/announce_plan.py +++ /dev/null @@ -1,22 +0,0 @@ -import yasmin -from GPSR.tts import say - - -class AnnouncePlan(yasmin.State): - """Say the plan description before executing it.""" - - def __init__(self, node): - super().__init__(outcomes=["succeeded"]) - self.add_input_key("plan_description") - self.add_input_key("steps") - self.add_output_key("plan_description") - self.add_output_key("steps") - self.node = node - - def execute(self, blackboard): - desc = blackboard["plan_description"] - if not desc: - return "succeeded" - self.node.get_logger().info(f"Announcing plan: {desc}") - say(self.node, desc) - return "succeeded" diff --git a/tasks/GPSR/GPSR/states/dispatch_skill.py b/tasks/GPSR/GPSR/states/dispatch_skill.py index 00b7bbf7c..380d8b5ea 100644 --- a/tasks/GPSR/GPSR/states/dispatch_skill.py +++ b/tasks/GPSR/GPSR/states/dispatch_skill.py @@ -1,7 +1,7 @@ import yasmin from geometry_msgs.msg import Point, Pose, Quaternion -from GPSR.states.query_llm import load_locations +from GPSR.world import load_locations from GPSR.tts import say from lasr_skills import GoToLocation @@ -11,10 +11,7 @@ class DispatchSkill(yasmin.State): def __init__(self, node): super().__init__(outcomes=["succeeded", "failed"]) - self.add_input_key("skill") - self.add_input_key("skill_args") self.add_input_key("steps") - self.add_input_key("plan_description") self.node = node self.locations = load_locations(node) @@ -53,18 +50,12 @@ def _execute_step(self, skill, args): return self._say(args.get("text", "")) if skill == "go_to_location": return self._go_to_location(args.get("location", "")) - self.node.get_logger().warn(f"Unknown skill: {skill}") - self._say(f"I don't know how to {skill}") - return "failed" + self.node.get_logger().info(f"Skipping skill '{skill}' (not yet actuated)") + return "succeeded" def execute(self, blackboard): - steps = blackboard["steps"] if "steps" in blackboard else None - - if steps: - for step in steps: - outcome = self._execute_step(step["skill"], step.get("args", {})) - if outcome == "failed": - return "failed" - return "succeeded" - - return self._execute_step(blackboard["skill"], blackboard["skill_args"]) + for step in blackboard["steps"]: + outcome = self._execute_step(step["skill"], step.get("args", {})) + if outcome == "failed": + return "failed" + return "succeeded" diff --git a/tasks/GPSR/GPSR/states/input_state.py b/tasks/GPSR/GPSR/states/input_state.py deleted file mode 100644 index c6685e761..000000000 --- a/tasks/GPSR/GPSR/states/input_state.py +++ /dev/null @@ -1,15 +0,0 @@ -from GPSR.states.keyboard_input import KeyboardInputState -from GPSR.states.listen import ListenState - - -def create_input_state(node): - """Return the input state configured by the `input_mode` parameter.""" - mode = node.get_parameter("input_mode").value.strip().lower() - - if mode == "keyboard": - return KeyboardInputState(node) - if mode in ("mic", "microphone"): - return ListenState(node) - - node.get_logger().warn(f"Unknown input_mode '{mode}', falling back to microphone") - return ListenState(node) diff --git a/tasks/GPSR/GPSR/states/keyboard_input.py b/tasks/GPSR/GPSR/states/keyboard_input.py index 78cfc4bc0..b06046393 100644 --- a/tasks/GPSR/GPSR/states/keyboard_input.py +++ b/tasks/GPSR/GPSR/states/keyboard_input.py @@ -1,8 +1,10 @@ import yasmin +INPUT_PROMPT = "Enter command: " + class KeyboardInputState(yasmin.State): - """Read a voice command from stdin instead of the microphone action server.""" + """Read a command from keyboard instead of the microphone action server.""" def __init__(self, node): super().__init__(outcomes=["succeeded", "aborted"]) @@ -10,7 +12,7 @@ def __init__(self, node): self.node = node def execute(self, blackboard): - prompt = self.node.get_parameter("input_prompt").value + prompt = INPUT_PROMPT self.node.get_logger().info(f"Keyboard input mode — {prompt}") try: diff --git a/tasks/GPSR/GPSR/states/query_llm.py b/tasks/GPSR/GPSR/states/query_llm.py index 7e0c89f62..a3a7b75fd 100644 --- a/tasks/GPSR/GPSR/states/query_llm.py +++ b/tasks/GPSR/GPSR/states/query_llm.py @@ -1,110 +1,65 @@ -import json -import os +import time import yasmin -import yaml -from ament_index_python.packages import get_package_share_directory from GPSR.agent import Agent - -SYSTEM_PROMPT = """You are a robot assistant. Given a voice command, output ONLY a JSON object — no explanation, no extra text. - -Available skills: -- go_to_location: navigate to a named location. Args: {{"location": ""}} -- say: speak a sentence. Args: {{"text": ""}} -- find_object: find an object in a location. Args: {{"object": "", "location": ""}} -- pick_up: pick up an object. Args: {{"object": ""}} -- give_to_person: give an object to a person. Args: {{"object": ""}} - -Known locations: {locations} - -Output format: -{{ - "plan_description": "", - "steps": [ - {{"skill": "", "args": {{...}}}}, - ... - ] -}} - -Examples: - "go to the kitchen" -> {{"plan_description": "I will navigate to the kitchen.", "steps": [{{"skill": "go_to_location", "args": {{"location": "kitchen"}}}}]}} - "find a pear in the bathroom then fetch it and bring it to the person raising their right arm in the bedroom" -> {{"plan_description": "I will go to the bathroom to find the pear, pick it up, then go to the bedroom and give it to the person raising their right arm.", "steps": [{{"skill": "go_to_location", "args": {{"location": "bathroom"}}}}, {{"skill": "find_object", "args": {{"object": "pear", "location": "bathroom"}}}}, {{"skill": "pick_up", "args": {{"object": "pear"}}}}, {{"skill": "go_to_location", "args": {{"location": "bedroom"}}}}, {{"skill": "give_to_person", "args": {{"object": "pear"}}}}]}} - -Command: """ - - -def load_locations(node): - """Load named navigation goals from the configured locations file.""" - package = node.get_parameter("locations_package").value - locations_file = node.get_parameter("locations_file").value - locations_path = os.path.join(get_package_share_directory(package), locations_file) - - if not os.path.exists(locations_path): - node.get_logger().warn(f"Locations file not found: {locations_path}") - return {} - - with open(locations_path) as f: - data = yaml.safe_load(f) or {} - return data.get("locations", {}) +from GPSR.planner import SkillSelectorError +from GPSR.world import build_world class QueryLLM(yasmin.State): - """YASMIN state that turns a transcribed phrase into a structured skill call.""" + """Skill selector → refiner → planner (+ optional announce).""" def __init__(self, node): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("sequence") - self.add_output_key("skill") - self.add_output_key("skill_args") - self.add_output_key("plan_description") self.add_output_key("steps") self.node = node - locations = load_locations(node) - location_names = list(locations.keys()) if locations else [] - self.system_prompt = SYSTEM_PROMPT.format(locations=", ".join(location_names)) - - self.node.get_logger().info("Loading agent (Ollama)...") - self.agent = Agent( - model=node.get_parameter("llm_model").value, - system_prompt=self.system_prompt, - host=node.get_parameter("llm_host").value, + self.world = build_world(node) + self.agent = Agent.from_node(node, log=self.node.get_logger().info) + self.node.get_logger().info( + "QueryLLM ready (skill selector + refiner + planner)." ) - self.node.get_logger().info("Agent ready.") def execute(self, blackboard): + t0 = time.perf_counter() command = blackboard["sequence"].strip() - self.node.get_logger().info(f"LLM query: '{command}'") + self.node.get_logger().info(f"Query: '{command}'") + # Stage 1 — skill selector + # Stage 2 — skill refiner + # Stage 3 — planner + # (agent.plan runs all three; cloud + local in parallel) try: - raw = self.agent.query_json(command) - except Exception as e: - self.node.get_logger().error(f"Agent query failed: {e}") + plan = self.agent.plan( + self.world, + command, + log=lambda msg: self.node.get_logger().info(msg), + ) + except SkillSelectorError as e: + self.node.get_logger().error(f"Planner failed: {e}") return "failed" - self.node.get_logger().info(f"LLM raw output: '{raw}'") + source = plan.get("source", "local") + steps = plan["steps"] + + # Announce plan: LLM generates spoken summary as first say step + if not (len(steps) == 1 and steps[0].get("skill") == "say"): + announcement = self.agent.announce( + command, + plan["plan_description"], + steps, + source, + log=lambda msg: self.node.get_logger().info(msg), + ) + if announcement: + steps = [{"skill": "say", "args": {"text": announcement}}] + steps - try: - start = raw.find("{") - end = raw.rfind("}") + 1 - if start == -1 or end == 0: - raise ValueError("No JSON found in LLM output") - parsed = json.loads(raw[start:end]) - steps = parsed.get("steps", []) - plan_description = parsed.get("plan_description", "") - if not steps: - raise ValueError("No steps in plan") - except (ValueError, KeyError, json.JSONDecodeError) as e: - self.node.get_logger().warn(f"LLM parse error: {e}") - blackboard["skill"] = "say" - blackboard["skill_args"] = {"text": "I did not understand that command"} - blackboard["plan_description"] = "" - blackboard["steps"] = [] - return "succeeded" - - blackboard["plan_description"] = plan_description blackboard["steps"] = steps - blackboard["skill"] = steps[0]["skill"] - blackboard["skill_args"] = steps[0].get("args", {}) - self.node.get_logger().info(f"Plan: {plan_description}, Steps: {steps}") + + label = "CLOUD" if source == "cloud" else "LOCAL" + self.node.get_logger().info( + f"=== PLAN SOURCE: {label} === | {plan['plan_description']} | " + f"{len(steps)} steps | {time.perf_counter() - t0:.1f}s" + ) return "succeeded" diff --git a/tasks/GPSR/GPSR/states/speech_recovery.py b/tasks/GPSR/GPSR/states/speech_recovery.py deleted file mode 100644 index ae46cfdd3..000000000 --- a/tasks/GPSR/GPSR/states/speech_recovery.py +++ /dev/null @@ -1,207 +0,0 @@ -""" -State for recovering the speech transcribed via whisper (name and drink) by using -the spelling and pronounciation of a word. -""" - -import smach -import string -import jellyfish as jf -from smach import UserData -from typing import List - -# TODO test this state - - -class SpeechRecovery(smach.State): - def __init__(self, guest_id: int, last_resort: bool, input_type: str = ""): - smach.State.__init__( - self, - outcomes=["succeeded", "failed"], - input_keys=["guest_transcription", "guest_data"], - output_keys=["guest_data", "guest_transcription"], - ) - - self._guest_id = guest_id - self._last_resort = last_resort - self._input_type = input_type - self._available_names = [ - "sophie", - "julia", - "emma", - "sara", - "laura", - "hayley", - "susan", - "fleur", - "gabrielle", - "robin", - "john", - "liam", - "lucas", - "william", - "kevin", - "jesse", - "noah", - "harrie", - "peter", - ] - self._available_single_drinks = ["cola", "water", "milk", "fanta", "dubbelfris"] - self._available_double_drinks = ["ice", "tea", "big", "coke"] - self._double_drinks_dict = { - "ice": "ice tea", - "tea": "ice tea", - "big": "big coke", - "coke": "big coke", - } - self._available_drinks = list( - set(self._available_single_drinks).union(set(self._available_double_drinks)) - ) - self._excluded_words = [ - "my", - "name", - "is", - "and", - "favourite", - "drink", - "you", - "can", - "call", - "me", - ] - - def execute(self, userdata: UserData) -> str: - filtered_sentence = userdata.guest_transcription.lower().translate( - str.maketrans("", "", string.punctuation) - ) - sentence_split = filtered_sentence.split() - sentence_list = list(set(sentence_split) - set(self._excluded_words)) - if not sentence_list: - return "failed" - - if self._input_type == "name": - final_name = self._handle_name(sentence_list, self._last_resort) - if final_name != "unknown": - userdata.guest_data[self._guest_id]["name"] = final_name - return "succeeded" - return "failed" - - if self._input_type == "drink": - final_drink = self._handle_drink(sentence_list, self._last_resort) - if final_drink != "unknown": - userdata.guest_data[self._guest_id]["drink"] = final_drink - return "succeeded" - return "failed" - - if userdata.guest_data[self._guest_id]["name"] == "unknown": - userdata.guest_data[self._guest_id]["name"] = self._handle_name( - sentence_list, self._last_resort - ) - if userdata.guest_data[self._guest_id]["drink"] == "unknown": - userdata.guest_data[self._guest_id]["drink"] = self._handle_drink( - sentence_list, self._last_resort - ) - if ( - userdata.guest_data[self._guest_id]["name"] == "unknown" - or userdata.guest_data[self._guest_id]["drink"] == "unknown" - ): - return "failed" - return "succeeded" - - def _handle_name(self, sentence_list: List[str], last_resort: bool) -> str: - result = self._handle_similar_spelt(sentence_list, self._available_names, 1) - if result != "unknown": - return result - result = self._handle_similar_sound(sentence_list, self._available_names, 0) - if not last_resort or result != "unknown": - return result - return self._handle_closest_spelt(sentence_list, self._available_names) - - def _handle_drink(self, sentence_list: List[str], last_resort: bool) -> str: - result = self._infer_second_drink(sentence_list) - if result != "unknown": - return result - result = self._handle_similar_spelt(sentence_list, self._available_drinks, 1) - if result == "unknown": - result = self._handle_similar_sound( - sentence_list, self._available_drinks, 0 - ) - if result != "unknown": - if result in self._available_single_drinks: - return result - sentence_list.append(result) - return self._infer_second_drink(sentence_list) - if not last_resort: - return "unknown" - if self._recover_dubbelfris(sentence_list): - return "dubbelfris" - closest_spelt = self._handle_closest_spelt( - sentence_list, self._available_drinks - ) - if closest_spelt in self._available_single_drinks: - return closest_spelt - sentence_list.append(closest_spelt) - return self._infer_second_drink(sentence_list) - - def _handle_similar_spelt( - self, - sentence_list: List[str], - available_words: List[str], - distance_threshold: int, - ) -> str: - for input_word in sentence_list: - for available_word in available_words: - if ( - self._get_damerau_levenshtein_distance(input_word, available_word) - <= distance_threshold - ): - return available_word - return "unknown" - - def _handle_similar_sound( - self, - sentence_list: List[str], - available_words: List[str], - distance_threshold: int, - ) -> str: - for input_word in sentence_list: - for available_word in available_words: - if ( - self._get_levenshtein_soundex_distance(input_word, available_word) - <= distance_threshold - ): - return available_word - return "unknown" - - def _infer_second_drink(self, sentence_list: List[str]) -> str: - for input_word in sentence_list: - for available_word in self._available_double_drinks: - if input_word == available_word: - return self._double_drinks_dict[input_word] - return "unknown" - - def _handle_closest_spelt( - self, sentence_list: List[str], choices: List[str] - ) -> str: - closest_distance = float("inf") - closest_word = None - for input_word in sentence_list: - for available_word in choices: - distance = self._get_damerau_levenshtein_distance( - input_word, available_word - ) - if distance < closest_distance: - closest_distance = distance - closest_word = available_word - return closest_word - - def _recover_dubbelfris(self, sentence_list: List[str]) -> bool: - for word in sentence_list: - if self._get_levenshtein_soundex_distance("dubbelfris", word) < 3: - return True - return False - - def _get_damerau_levenshtein_distance(self, word_1: str, word_2: str) -> int: - return jf.damerau_levenshtein_distance(word_1, word_2) - - def _get_levenshtein_soundex_distance(self, word_1: str, word_2: str) -> int: - return jf.levenshtein_distance(jf.soundex(word_1), jf.soundex(word_2)) diff --git a/tasks/GPSR/GPSR/tts.py b/tasks/GPSR/GPSR/tts.py index 876de12b9..947763e74 100644 --- a/tasks/GPSR/GPSR/tts.py +++ b/tasks/GPSR/GPSR/tts.py @@ -8,10 +8,7 @@ def say(node: rclpy.node.Node, text: str): if not text: return node.get_logger().info(text) - try: - simulation = node.get_parameter("simulation").value - except Exception: - simulation = False + simulation = node.get_parameter("simulation").get_parameter_value().bool_value if simulation: _say_gtts(text) diff --git a/tasks/GPSR/GPSR/world.py b/tasks/GPSR/GPSR/world.py new file mode 100644 index 000000000..ef7d23a16 --- /dev/null +++ b/tasks/GPSR/GPSR/world.py @@ -0,0 +1,139 @@ +"""Load robot world config from yaml and format it for the LLM planner.""" + +import os + +import yaml +from ament_index_python.packages import get_package_share_directory + +__all__ = [ + "build_world", + "compact_skill_lines", + "format_objects", + "format_people", + "load_locations", + "selected_skill_lines", +] + + +def compact_skill_lines(skills_text: str) -> str: + """Turn raw skills.yaml text into compact lines for LLM prompts (name, args, purpose).""" + lines = [] + for raw in skills_text.splitlines(): + if ":" not in raw: + continue + name, desc = raw.split(":", 1) + name = name.strip().lstrip("- ") + desc = desc.strip() + args = "" + if "Args:" in desc: + purpose, _, arg_tail = desc.partition("Args:") + arg_names = [] + for part in arg_tail.split(","): + token = part.strip().split(" ")[0].strip(" .()") + if token and token.lower() != "none": + arg_names.append(token) + args = ", ".join(arg_names) + desc = purpose.strip() + purpose = desc.split(".")[0].strip() + lines.append(f"{name}({args}) — {purpose}") + return "\n".join(lines) + + +def format_objects(objects: dict) -> str: + """Format the objects dict as a single line for the planner prompt.""" + return ( + ", ".join( + f"{name} ({obj.get('category', '?')} in {obj.get('location', '?')})" + for name, obj in objects.items() + ) + or "none" + ) + + +def format_people(people: dict) -> str: + """Format the people dict as a single line for the planner prompt.""" + return ( + ", ".join( + f"{name} ({info.get('gender', '?')})" for name, info in people.items() + ) + or "none" + ) + + +def selected_skill_lines(selected_skills: list, all_skill_lines: str) -> str: + """Return only the skill lines chosen by the skill selector.""" + if not selected_skills: + return "say(text) — speak aloud" + result = [] + for line in all_skill_lines.splitlines(): + skill_name = line.split("(")[0].strip() + if skill_name in selected_skills: + result.append(line) + return "\n".join(result) if result else all_skill_lines + + +def _pkg_config(node, filename): + """Absolute path to a file under share/GPSR/config/.""" + return os.path.join(get_package_share_directory("GPSR"), "config", filename) + + +def load_skills_text(node): + """Load skills.yaml as plain text (comments stripped).""" + path = _pkg_config(node, "skills.yaml") + if not os.path.exists(path): + return "" + with open(path) as f: + lines = [l.rstrip() for l in f if l.strip() and not l.startswith("#")] + return "\n".join(lines) + + +def load_locations(node): + """Load locations.yaml → {name: {position, orientation}}.""" + path = _pkg_config(node, "locations.yaml") + if not os.path.exists(path): + return {} + with open(path) as f: + data = yaml.safe_load(f) or {} + return data.get("locations", {}) + + +def load_objects(node): + """Load objects.yaml → {name: {category, location, ...}}.""" + path = _pkg_config(node, "objects.yaml") + if not os.path.exists(path): + return {} + with open(path) as f: + data = yaml.safe_load(f) or {} + return data.get("objects", {}) + + +def load_people(node): + """Load people.yaml → {name: {gender, ...}}.""" + path = _pkg_config(node, "people.yaml") + if not os.path.exists(path): + return {} + with open(path) as f: + data = yaml.safe_load(f) or {} + return data.get("people", {}) + + +def load_general_knowledge(node): + """Load general_knowledge.yaml → free-text string for the planner.""" + path = _pkg_config(node, "general_knowledge.yaml") + if not os.path.exists(path): + return "" + with open(path) as f: + data = yaml.safe_load(f) or {} + return data.get("info", "") + + +def build_world(node) -> dict: + """Load all config yaml and return the world dict passed to the planner.""" + skills_text = load_skills_text(node) + return { + "locations": load_locations(node), + "objects": load_objects(node), + "people": load_people(node), + "general_knowledge": load_general_knowledge(node), + "skill_lines": compact_skill_lines(skills_text), + } diff --git a/tasks/GPSR/README.md b/tasks/GPSR/README.md new file mode 100644 index 000000000..37f347f69 --- /dev/null +++ b/tasks/GPSR/README.md @@ -0,0 +1,73 @@ +# GPSR Task + +General Purpose Service Robot task for RoboCup@Home. The robot listens to a natural language command, plans a sequence of skills via LLM, and executes them. + +## Quick start — planner test (simulation, keyboard input) + +### 1. Set params + +Make sure [config/params.yaml](config/params.yaml) has: + +```yaml +gpsr: + ros__parameters: + simulation: true + input_mode: "keyboard" +``` + +### 2. Start Ollama (first run, if you've already done you don't need to rerun these commands) + +```bash +ollama serve +ollama pull gemma3 # only first time +``` + +### 3. Build and run + +```bash +# from workspace root (Base/) +colcon build --packages-select GPSR +source install/setup.bash + +ros2 run GPSR sm --ros-args --params-file tasks/GPSR/config/params.yaml +``` + +Type your command when prompted, e.g.: + +``` +> Go to the kitchen and bring me a bottle of water +``` + +The robot will print and speak the planned steps. + +--- + +## Full simulation (with Nav2) + +Launch Nav2 and the GPSR node in two separate terminals. + +**Terminal 1 — Nav2:** +```bash +ros2 launch simulation nav.launch.py +``` + +**Terminal 2 — GPSR:** +```bash +ros2 run GPSR sm --ros-args --params-file tasks/GPSR/config/params.yaml +``` + +Set `simulation: false` in `params.yaml` to use the robot TTS instead of gtts. (only for real robot) + +--- + +## Config files + +| File | Purpose | +|------|---------| +| `config/params.yaml` | Runtime parameters (LLM host, input mode, simulation flag) | +| `config/skills.yaml` | Available skill signatures shown to the LLM | +| `config/locations.yaml` | Named 3D poses for navigation | +| `config/objects.yaml` | Object inventory (category, location, size) | +| `config/people.yaml` | Known people identities | +| `config/general_knowledge.yaml` | Arena context for the LLM | + diff --git a/tasks/GPSR/config/general_knowledge.yaml b/tasks/GPSR/config/general_knowledge.yaml new file mode 100644 index 000000000..91a6156b6 --- /dev/null +++ b/tasks/GPSR/config/general_knowledge.yaml @@ -0,0 +1,2 @@ +info: + "Your team is LASR from united kingdom at King's College London. Today is 11 June 2024, the day of the week is Thursday." \ No newline at end of file diff --git a/tasks/GPSR/config/locations.yaml b/tasks/GPSR/config/locations.yaml index 8c63d38d8..e2df635a8 100644 --- a/tasks/GPSR/config/locations.yaml +++ b/tasks/GPSR/config/locations.yaml @@ -1,11 +1,41 @@ locations: + bedroom: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: -0.765 + y: 2.524 + z: 0.0 kitchen: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 position: - x: 9.151 - y: -6.340 + x: 1.717 + y: -1.3 z: 0.0 + living room: orientation: + w: 1.0 x: 0.0 y: 0.0 - z: 0.891 - w: 0.454 + z: 0.0 + position: + x: 7.546 + y: 0.722 + z: 0.0 + office: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 3.2 + y: 4.1 + z: 0.0 diff --git a/tasks/GPSR/config/objects.yaml b/tasks/GPSR/config/objects.yaml new file mode 100644 index 000000000..99f261fc1 --- /dev/null +++ b/tasks/GPSR/config/objects.yaml @@ -0,0 +1,152 @@ +objects: + # --- drinks (cabinet in kitchen) --- + cola: + category: drink + location: kitchen + grabbable: true + size: small + aliases: [coke, soda] + milk: + category: drink + location: kitchen + grabbable: true + size: small + aliases: [] + orange juice: + category: drink + location: kitchen + grabbable: true + size: small + aliases: [oj] + juice pack: + category: drink + location: kitchen + grabbable: true + size: small + aliases: [] + + # --- fruits (desk in office) --- + apple: + category: fruit + location: office + grabbable: true + size: small + aliases: [] + banana: + category: fruit + location: office + grabbable: true + size: small + aliases: [] + orange: + category: fruit + location: office + grabbable: true + size: small + aliases: [] + pear: + category: fruit + location: office + grabbable: true + size: small + aliases: [] + + # --- food (pantry in kitchen) --- + mustard: + category: food + location: kitchen + grabbable: true + size: small + aliases: [] + tuna: + category: food + location: kitchen + grabbable: true + size: small + aliases: [] + sugar: + category: food + location: kitchen + grabbable: true + size: small + aliases: [] + spam: + category: food + location: kitchen + grabbable: true + size: small + aliases: [] + + # --- snacks (side tables in living room) --- + pringles: + category: snack + location: living room + grabbable: true + size: small + aliases: [] + cornflakes: + category: snack + location: living room + grabbable: true + size: small + aliases: [] + cheezit: + category: snack + location: living room + grabbable: true + size: small + aliases: [cheez-it] + + # --- dishes (kitchen table) --- + cup: + category: dish + location: kitchen + grabbable: true + size: small + aliases: [] + bowl: + category: dish + location: kitchen + grabbable: true + size: small + aliases: [] + plate: + category: dish + location: kitchen + grabbable: true + size: small + aliases: [] + + # --- toys (bookshelf in living room) --- + tennis ball: + category: toy + location: living room + grabbable: true + size: small + aliases: [] + rubiks cube: + category: toy + location: living room + grabbable: true + size: small + aliases: [rubik cube] + dice: + category: toy + location: living room + grabbable: true + size: small + aliases: [] + + # --- cleaning supplies (shelf in bedroom) --- + cleanser: + category: cleaning supply + location: bedroom + grabbable: true + size: small + aliases: [] + sponge: + category: cleaning supply + location: bedroom + grabbable: true + size: small + aliases: [] diff --git a/tasks/GPSR/config/params.yaml b/tasks/GPSR/config/params.yaml index 46a10254d..78d6a1b34 100644 --- a/tasks/GPSR/config/params.yaml +++ b/tasks/GPSR/config/params.yaml @@ -1,17 +1,13 @@ gpsr: ros__parameters: - llm_model: "llama3.2" + llm_model: "gemma3" llm_host: "http://localhost:11434" - locations_package: "GPSR" - locations_file: "config/locations.yaml" - simulation: false # true = use gtts on PC, false = use robot TTS - input_mode: "mic" # "keyboard" or "mic" - input_prompt: "Enter command: " - initial_pose: - x: 9.151 - y: -6.340 - yaw: 2.204 + cloud_host: "127.0.0.1" # server.py; + cloud_port: 8765 + cloud_timeout_sec: 10.0 + simulation: true # true = use gtts on PC, false = use robot TTS + input_mode: "keyboard" # "keyboard" or "mic" whisper: - energy_threshold: 6500 - device: "cpu" - state_machine_delay_sec: 30.0 + energy_threshold: 400 + phrase_duration: 10.0 + device: "cuda" diff --git a/tasks/GPSR/config/people.yaml b/tasks/GPSR/config/people.yaml new file mode 100644 index 000000000..097c6283f --- /dev/null +++ b/tasks/GPSR/config/people.yaml @@ -0,0 +1,10 @@ +people: + adel: + gender: neutral + description: "A person named Adel." + axel: + gender: male + description: "A man named Axel." + charlie: + gender: neutral + description: "A person named Charlie." \ No newline at end of file diff --git a/tasks/GPSR/config/skills.yaml b/tasks/GPSR/config/skills.yaml new file mode 100644 index 000000000..219fa0ba9 --- /dev/null +++ b/tasks/GPSR/config/skills.yaml @@ -0,0 +1,14 @@ +go_to_location: Navigate the robot to a named location in the map. Args: location (string). +say: Make the robot speak a sentence aloud. Use this for greetings, answers, announcements, and any informational reply. Args: text (string). +find_object: Search for an object by name or category in a room using the robot camera. Args: object (string), location (string). +pick_up: Grasp and lift the target object with the robot arm. The object must have been found first. Args: object (string). +place_object: Place the currently held object on a named surface. Args: location (string). +give_to_person: Hand the currently held object to a nearby person. Args: object (string). +find_person: Locate a person identified by name, pose (standing/sitting/lying), gesture (waving/raising arm/pointing), or clothing color. Args: name (optional), pose (optional), gesture (optional), clothes (optional), location (optional). +guide_person: Escort a person from a start location to a destination. Args: name (optional), start (string), end (string). +follow_person: Follow a person as they move until told to stop. Args: destination (optional). +count_objects: Count objects of a given name or category at a placement location. Args: object (string), location (string). +count_people: Count people with a given pose, gesture, or clothing in a room. Args: pose (optional), gesture (optional), clothes (optional), location (string). +get_person_info: Detect and report information about a person (name, pose, or gesture). Args: info (name|pose|gesture), location (string). +find_object_by_property: Find the object on a surface matching a comparative property (biggest/smallest/heaviest/lightest/thinnest). Args: property (string), object (optional), location (string). +answer_question: Listen to and answer a general knowledge question asked by a person. Use say to speak the answer. Args: none. diff --git a/tasks/GPSR/external/requirements.txt b/tasks/GPSR/external/requirements.txt new file mode 100644 index 000000000..aa2b70446 --- /dev/null +++ b/tasks/GPSR/external/requirements.txt @@ -0,0 +1 @@ +openai>=1.0.0 diff --git a/tasks/GPSR/external/server.py b/tasks/GPSR/external/server.py new file mode 100755 index 000000000..8be4fae21 --- /dev/null +++ b/tasks/GPSR/external/server.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +"""LLM proxy — receives prompt + optional system_prompt, returns text.""" + +import json +import os +import socketserver +import sys + +from openai import OpenAI + +HOST = os.environ.get("LLM_SERVER_HOST", "0.0.0.0") +PORT = int(os.environ.get("LLM_SERVER_PORT", "8765")) +OPENAI_URL = os.environ.get("ENDPOINT_URL") or os.environ.get("OPENAI_URL", "") +API_KEY = os.environ.get("API_KEY") or os.environ.get("OPENAI_API_KEY", "") +MODEL = os.environ.get("LLM_MODEL") or os.environ.get("OPENAI_MODEL", "") + +_client: OpenAI | None = None + + +def _send(wfile, msg: dict) -> None: + wfile.write((json.dumps(msg) + "\n").encode()) + wfile.flush() + + +def _query(prompt: str, system_prompt: str = "") -> str: + messages = [] + if system_prompt: + messages.append({"role": "system", "content": system_prompt}) + messages.append({"role": "user", "content": prompt}) + response = _client.chat.completions.create(model=MODEL, messages=messages) + return response.choices[0].message.content or "" + + +class Handler(socketserver.StreamRequestHandler): + def handle(self) -> None: + print( + f"Client connected: {self.client_address[0]}:{self.client_address[1]}", + flush=True, + ) + while True: + line = self.rfile.readline() + if not line: + break + try: + msg = json.loads(line.decode().strip()) + except json.JSONDecodeError: + _send(self.wfile, {"type": "error", "text": "invalid json"}) + continue + if msg.get("type") != "query": + _send(self.wfile, {"type": "error", "text": "expected type=query"}) + continue + prompt = (msg.get("prompt") or "").strip() + if not prompt: + _send(self.wfile, {"type": "error", "text": "empty prompt"}) + continue + system_prompt = (msg.get("system_prompt") or "").strip() + print(f"Query ({len(prompt)} chars)", flush=True) + try: + text = _query(prompt, system_prompt) + except Exception as e: + print(f"LLM error: {e}", flush=True) + _send(self.wfile, {"type": "error", "text": str(e)}) + continue + print(f"Answer ({len(text)} chars)", flush=True) + _send(self.wfile, {"type": "answer", "text": text}) + + +class Server(socketserver.ThreadingMixIn, socketserver.TCPServer): + allow_reuse_address = True + daemon_threads = True + + +def main() -> None: + global _client + if not API_KEY or not OPENAI_URL or not MODEL: + print("Set ENDPOINT_URL, API_KEY, LLM_MODEL", file=sys.stderr) + sys.exit(1) + _client = OpenAI(base_url=OPENAI_URL, api_key=API_KEY, timeout=300.0) + with Server((HOST, PORT), Handler) as server: + print(f"LLM proxy on {HOST}:{PORT} | model={MODEL}") + try: + server.serve_forever() + except KeyboardInterrupt: + print("\nStopped.") + + +if __name__ == "__main__": + main() diff --git a/tasks/GPSR/requirements.txt b/tasks/GPSR/requirements.txt index 5cd8fc31e..a2aac6ca7 100644 --- a/tasks/GPSR/requirements.txt +++ b/tasks/GPSR/requirements.txt @@ -1,3 +1,5 @@ ollama jellyfish PyYAML +gtts +pydub diff --git a/tasks/HRI/HRI/state_machine.py b/tasks/HRI/HRI/state_machine.py index 6891b2c1a..095cadf18 100644 --- a/tasks/HRI/HRI/state_machine.py +++ b/tasks/HRI/HRI/state_machine.py @@ -1,5 +1,3 @@ -from typing import List, Tuple, Dict - from threading import Thread import rclpy @@ -8,9 +6,9 @@ import yasmin import yasmin_ros -from geometry_msgs.msg import Point, PointStamped, Pose +from geometry_msgs.msg import PointStamped -from lasr_skills import Say, GoToLocation +from lasr_skills import Say, SafeGoToLocation, StartDoorSM, Rotate, FollowPerson from HRI.states import * @@ -27,6 +25,8 @@ class HRI(yasmin.StateMachine): def __init__(self): super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + self.guest_id = 1 + def wait_cb(blackboard, msg): yasmin.YASMIN_LOG_INFO("RECEIVED START SIGNAL") return "succeeded" @@ -34,7 +34,7 @@ def wait_cb(blackboard, msg): self.add_state( "WAIT_START", # Awaits start Signal for the task yasmin_ros.MonitorState( - topic_name="/receptionist/start", + topic_name="/hri/start", outcomes=["succeeded", "failed"], monitor_handler=wait_cb, msg_type=Empty, @@ -49,37 +49,145 @@ def wait_cb(blackboard, msg): self.add_state( "START_TIMER", StartTimer(), - transitions={"succeeded": "GREET", "failed": "START_TIMER"}, + transitions={"succeeded": "START_CON", "failed": "START_TIMER"}, ) - # self.add_state( - # "START_CON", # SM1: Waits for Door to open, then goes to start - # self.setup(), - # transitions={"succeeded": "GREET", "failed": "START_CON"}, - # ) + self.add_state( + "START_CON", # SM1: Waits for Door to open, then goes to start + self.setup(), + transitions={"succeeded": "GO_TO_DOOR", "failed": "START_CON"}, + ) + + self.add_state( + "GO_TO_DOOR", + SafeGoToLocation(location_param="door_pose"), + transitions={"succeeded": "GREET", "failed": "failed"}, + ) self.add_state( "GREET", # SM2: Greets guest - LookAndGreetGuest(last_resort=False, guest_id="guest1"), - transitions={"succeeded": "SEAT_GUEST", "failed": "failed"}, + LookAndGreetGuest(guest_id="guest1"), + transitions={"succeeded": "GUIDE_TO_SEAT", "failed": "failed"}, ) - # self.add_state( - # "GUIDE_TO_SEAT", # GUIDES GUEST TO SEATING AREA - # GoToLocation(location_param="seat_pose"), - # transitions={"succeeded": "SEAT_GUEST", "failed": "failed"}, - # ) + self.add_state( + "GUIDE_TO_SEAT", # GUIDES GUEST TO SEATING AREA + SafeGoToLocation(location_param="seat_pose"), + transitions={"succeeded": "SEAT_GUEST", "failed": "failed"}, + ) self.add_state( "SEAT_GUEST", # SM3: Locates and seats guest in free seat - SeatGuest(learn_host=False), + SeatGuest(guest_id="guest1"), + transitions={"succeeded": "CHECK", "failed": "failed"}, + ) + + self.add_state( + "CHECK", + yasmin.CbState(outcomes=["succeeded", "continue"], callback=self.check), + transitions={"succeeded": "INTRODUCE", "continue": "GO_TO_DOOR_2"}, + ) + + self.add_state( + "GO_TO_DOOR_2", + SafeGoToLocation(location_param="door_pose"), + transitions={"succeeded": "GREET_2", "failed": "failed"}, + ) + + self.add_state( + "GREET_2", # SM2: Greets guest + LookAndGreetGuest(guest_id="guest2"), + transitions={"succeeded": "GUIDE_TO_SEAT_2", "failed": "failed"}, + ) + + self.add_state( + "GUIDE_TO_SEAT_2", # GUIDES GUEST TO SEATING AREA + SafeGoToLocation(location_param="seat_pose"), + transitions={"succeeded": "SEAT_GUEST_2", "failed": "failed"}, + ) + + self.add_state( + "SEAT_GUEST_2", # SM3: Locates and seats guest in free seat + SeatGuest(guest_id="guest2"), + transitions={"succeeded": "CHECK", "failed": "failed"}, + ) + + self.add_state( + "INTRODUCE", + Introduce(), + transitions={"succeeded": "ROTATE", "failed": "ROTATE"}, + ) + + self.add_state( + "ROTATE", + Rotate(angle=180), + transitions={"succeeded": "FOLLOW_HOST", "failed": "failed"}, + ) + + self.add_state( + "FOLLOW_HOST", + FollowPerson(), + transitions={ + "succeeded": "PLACE_BAG", + "failed": "failed", + }, + ) + + self.add_state( + "PLACE_BAG", + PlaceBag(), + transitions={ + "succeeded": "succeeded", + "failed": "failed", + }, + ) + + self.add_state("STOP_TIMER", StopTimer(), transitions={"succeeded": "SAY_STOP"}) + + self.add_state( + "SAY_STOP", + Say(), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "canceled": "failed", + }, + remappings={"text": "time_text"}, + ) + + self.add_state( + "INTRODUCE", + Introduce(), transitions={"succeeded": "succeeded", "failed": "failed"}, ) + def check(self, blackboard): + guest1 = blackboard["guest_data"]["guest1"] + yasmin.YASMIN_LOG_INFO(f"{self.guest_id}") + + if self.guest_id == 2: + guest2 = blackboard["guest_data"]["guest2"] + yasmin.YASMIN_LOG_INFO("Guest1: ") + for key in guest1.keys(): + value = guest1[key] + yasmin.YASMIN_LOG_INFO(f"{key}: {value}") + yasmin.YASMIN_LOG_INFO("Guest2: ") + for key in guest2.keys(): + value = guest2[key] + yasmin.YASMIN_LOG_INFO(f"{key}: {value}") + else: + yasmin.YASMIN_LOG_INFO("Guest1: ") + for key in guest1.keys(): + value = guest1[key] + yasmin.YASMIN_LOG_INFO(f"{key}: {value}") + + self.guest_id += 1 + return "continue" if self.guest_id == 2 else "succeeded" + def setup(self): start_con_sm = yasmin.Concurrence( states={ - "SAY_START": Say(text="Start of HRI task."), + "SAY_START": Say(text="Start of H R I task."), "DOOR_START": StartDoorSM(), }, default_outcome="failed", @@ -125,17 +233,22 @@ def main(): face_detection_confidence = 0.2 bb["guest_data"] = { + "host": {"seated_point": None, "seating_detection": False}, "guest1": { "name": "", "drink": "", "detection": False, "seating_detection": False, + "attributes": {}, + "seated_point": None, }, "guest2": { "name": "", "drink": "", "detection": False, "seating_detection": False, + "attributes": {}, + "seated_point": None, }, } @@ -145,6 +258,7 @@ def main(): bb["confidence"] = face_detection_confidence bb["dataset"] = "hri" bb["drink_position"] = PointStamped() + bb["person_index"] = 0 outcome = sm(bb) diff --git a/tasks/HRI/HRI/states/__init__.py b/tasks/HRI/HRI/states/__init__.py index f7508d9ae..3cdc29b8b 100644 --- a/tasks/HRI/HRI/states/__init__.py +++ b/tasks/HRI/HRI/states/__init__.py @@ -1,10 +1,18 @@ from .speech_recovery import SpeechRecovery -from .learn_host_face import LearnHostFace + from .hri_learn_faces import HRILearnFaces from .seat_guest import SeatGuest from .get_name_and_drink import GetNameAndDrink from .timer_states import StartTimer, StopTimer -from .start_door_sm import StartDoorSM from .get_attributes import GetGuestAttributes from .get_person_point import GetPersonPoint from .greet import LookAndGreetGuest + +from .clearSeatingDetections import ClearSeatingDetections +from .getGuestData import GetGuestData +from .getIntroductionStr import GetIntroductionStr +from .recognise import Recognise + +from .introduce import Introduce + +from .place_bag import PlaceBag diff --git a/tasks/HRI/HRI/states/clearSeatingDetections.py b/tasks/HRI/HRI/states/clearSeatingDetections.py new file mode 100644 index 000000000..eb466fa9c --- /dev/null +++ b/tasks/HRI/HRI/states/clearSeatingDetections.py @@ -0,0 +1,20 @@ +import yasmin +from yasmin import Blackboard + + +class ClearSeatingDetections(yasmin.State): + """ + Clears the seating detection for all guests in the guest data. + This is to ensure that we can re-detect guests when they are seated. + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("guest_data") + self.add_output_key("guest_data") + + def execute(self, blackboard: Blackboard) -> str: + blackboard["seat_indexes"] = {"guest1": None, "guest2": None, "host": None} + for guest_id in blackboard["guest_data"]: + blackboard["guest_data"][guest_id]["seating_detection"] = False + return "succeeded" diff --git a/tasks/HRI/HRI/states/getGuestData.py b/tasks/HRI/HRI/states/getGuestData.py new file mode 100644 index 000000000..c88ae7a98 --- /dev/null +++ b/tasks/HRI/HRI/states/getGuestData.py @@ -0,0 +1,64 @@ +from typing import Optional +import yasmin +from yasmin import Blackboard + + +class GetGuestData(yasmin.State): + + _guest_to_introduce: Optional[str] + _guest_to_introduce_to: Optional[str] + + def __init__( + self, + guest_to_introduce: Optional[str] = None, + guest_to_introduce_to: Optional[str] = None, + ): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("guest_data") + self.add_input_key("named_guest_detection") + self.add_output_key("relevant_guest_data") + self.add_output_key("introduce_to") + + """ + Blackboard keys: + - guest_data: Dictionary of all guests keyed by id (host, guest1, guest2) + each containing name, drink, and interest. + - named_guest_detection: Detection result containing the recognised guest id. + + Output keys: + - relevant_guest_data: Data dictionary for the guest being introduced. + - introduce_to: Display name (str) of the guest being introduced to. + """ + + self._guest_to_introduce = guest_to_introduce + self._guest_to_introduce_to = guest_to_introduce_to + + def execute(self, blackboard: Blackboard) -> str: + guest_data = blackboard["guest_data"] + + # Who we are speaking about + if self._guest_to_introduce is not None: + blackboard["relevant_guest_data"] = guest_data[self._guest_to_introduce] + else: + reid = blackboard["named_guest_detection"].name + blackboard["relevant_guest_data"] = guest_data.get(reid, guest_data["host"]) + + # Who we are speaking to + if self._guest_to_introduce_to is not None: + blackboard["introduce_to"] = guest_data[self._guest_to_introduce_to]["name"] + else: + reid = blackboard["named_guest_detection"].name + if reid not in guest_data: + blackboard["introduce_to"] = guest_data["host"]["name"] + else: + blackboard["introduce_to"] = guest_data[reid]["name"] + + yasmin.YASMIN_LOG_INFO( + f"Introducing: {self._guest_to_introduce}, to {self._guest_to_introduce_to}" + ) + string = blackboard["introduce_to"] + test = blackboard["relevant_guest_data"] + yasmin.YASMIN_LOG_INFO("We 'introduce_to': \n" + str(string)) + yasmin.YASMIN_LOG_INFO("The relevant guest data: \n" + str(test)) + + return "succeeded" diff --git a/tasks/HRI/HRI/states/getIntroductionStr.py b/tasks/HRI/HRI/states/getIntroductionStr.py new file mode 100644 index 000000000..6d8653413 --- /dev/null +++ b/tasks/HRI/HRI/states/getIntroductionStr.py @@ -0,0 +1,22 @@ +import yasmin +from yasmin import Blackboard + + +class GetIntroductionStr(yasmin.State): + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("relevant_guest_data") + self.add_input_key("introduce_to") + self.add_output_key("text") + + def execute(self, blackboard: Blackboard) -> str: + guest_to_introduce_data = blackboard["relevant_guest_data"] + guest_to_introduce_to = blackboard["introduce_to"] + + blackboard["text"] = ( + f"Hello {guest_to_introduce_to}, " + f"this is {guest_to_introduce_data['name']}. " + f"Their favourite drink is {guest_to_introduce_data['drink']} " + ) + return "succeeded" diff --git a/tasks/HRI/HRI/states/get_attributes.py b/tasks/HRI/HRI/states/get_attributes.py index 839cb8d1f..923fcc50e 100644 --- a/tasks/HRI/HRI/states/get_attributes.py +++ b/tasks/HRI/HRI/states/get_attributes.py @@ -30,7 +30,7 @@ def __init__(self, guest_id: str): ) self.add_input_key("guest_data") - self.add_input_key("clip_detection_dict") + self.add_input_key("attributes") self.add_output_key("guest_data") self._guest_id: str = guest_id @@ -38,7 +38,7 @@ def __init__(self, guest_id: str): def execute(self, blackboard) -> str: try: blackboard["guest_data"][self._guest_id]["attributes"] = blackboard[ - "clip_detection_dict" + "attributes" ] blackboard["guest_data"][self._guest_id]["detection"] = True return "succeeded" @@ -61,7 +61,7 @@ def __init__(self, guest_id: str): self.InitialiseDetectionFlag(guest_id=self._guest_id), transitions={ "succeeded": "GET_GUEST_ATTRIBUTES", - "failed": "GET_GUEST_ATTRIBUTES", + "failed": "failed", }, ) self.add_state( diff --git a/tasks/HRI/HRI/states/get_name_and_drink.py b/tasks/HRI/HRI/states/get_name_and_drink.py index a9f3e8352..407f08949 100755 --- a/tasks/HRI/HRI/states/get_name_and_drink.py +++ b/tasks/HRI/HRI/states/get_name_and_drink.py @@ -26,21 +26,26 @@ def __init__(self, task, guest_id): self.add_input_key("guest_transcription") self.add_input_key("guest_data") self.add_output_key("guest_data") + self.add_output_key("placeholders") self.task = task self.guest_id = guest_id def _create_req(self, blackboard): request = HRITaskQueryLlm.Request( - llm_input=blackboard['guest_transcription'], task=self.task + llm_input=blackboard["guest_transcription"], task=self.task ) return request def _handle_resp(self, blackboard, result): result = result.response - blackboard["guest_data"][self.guest_id][self.task] = result.name if self.task == "name" else result.favourite_drink - + blackboard["guest_data"][self.guest_id][self.task] = ( + result.name if self.task == "name" else result.favourite_drink + ) + + if self.task == "name": + blackboard["placeholders"] = result.name return "succeeded" @@ -60,7 +65,7 @@ def __init__(self, guest_id: str): def execute(self, blackboard) -> str: if not self._recovery_name_and_drink_required(blackboard): - if blackboard["guest_data"][self._guest_id]["name"] == "unknown": + if blackboard["guest_data"][self._guest_id]["name"] == "": outcome = "failed_name" else: outcome = "failed_drink" @@ -76,8 +81,8 @@ def _recovery_name_and_drink_required(self, blackboard) -> bool: """ return ( - blackboard["guest_data"][self._guest_id]["name"] == "unknown" - and blackboard["guest_data"][self._guest_id]["drink"] == "unknown" + blackboard["guest_data"][self._guest_id]["name"] == "" + and blackboard["guest_data"][self._guest_id]["drink"] == "" ) def __init__( @@ -86,8 +91,7 @@ def __init__( last_resort: bool, ): super().__init__( - outcomes=["succeeded", "failed", "failed_name", "failed_drink"], - handle_sigint=True, + outcomes=["succeeded", "failed", "failed_name", "failed_drink"] ) self.add_input_key("guest_transcription") diff --git a/tasks/HRI/HRI/states/greet.py b/tasks/HRI/HRI/states/greet.py index b8fc8e46b..eddbb082a 100644 --- a/tasks/HRI/HRI/states/greet.py +++ b/tasks/HRI/HRI/states/greet.py @@ -1,6 +1,14 @@ import yasmin -from lasr_skills import Say, StartEyeTracker, WaitForPersonInArea, AskAndListen +from lasr_skills import ( + Say, + StartEyeTracker, + WaitForPersonInArea, + AskAndListen, + ReceiveObject, + StopEyeTracker, + Wait, +) from HRI.states import ( GetNameAndDrink, GetGuestAttributes, @@ -8,26 +16,80 @@ GetPersonPoint, ) -""" -Robot is already at door, since: - SM1 detects door ---(door opening)---> robot goes through door to waiting area ---(waits a few seconds)---> robot goes back to door area - -So Robot is at door and it: - Faces person -> Start Eye tracker -> Greet guest -> Listen -> recognise name and drink -> listen loop -""" - class LookAndGreetGuest(yasmin.StateMachine): + def __init__(self, guest_id): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("guest_data") + self.add_output_key("guest_data") + self.add_output_key("person_detections") + + look_and_greet = yasmin.Concurrence( + states={ + "GREET_ONLY": GreetGuest(last_resort=False, guest_id=guest_id), + "EYE_TRACKER": StartEyeTracker(), + }, + default_outcome="failed", + outcome_map={ + "succeeded": { + "GREET_ONLY": "succeeded", + "EYE_TRACKER": "canceled", + } + }, + ) + + self.add_state( + "SAY_WAITING_FOR_GUEST", + Say(text="I am waiting for a guest."), + transitions={ + "succeeded": "WAIT_FOR_GUEST", + "aborted": "WAIT_FOR_GUEST", + "canceled": "WAIT_FOR_GUEST", + }, + ) + self.add_state( + "WAIT_FOR_GUEST", + WaitForPersonInArea(polygon_param="door_polygon"), + transitions={ + "succeeded": "GET_PERSON_POINT", + "failed": "SAY_WAITING_FOR_GUEST", + }, + remappings={"detections_3d": "person_detections"}, + ) + self.add_state( + "GET_PERSON_POINT", + GetPersonPoint(), + transitions={ + "succeeded": "LOOK_AND_GREET", + "failed": "SAY_WAITING_FOR_GUEST", + }, + ) + + self.add_state( + "LOOK_AND_GREET", + look_and_greet, + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + + +class GreetGuest(yasmin.StateMachine): def __init__(self, last_resort, guest_id): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("guest_data") self.add_output_key("guest_data") self.add_output_key("person_detections") + attribute = yasmin.CbState( + outcomes=["succeeded", "failed"], callback=self.get_guest1_attributes + ) + + attribute.add_input_key("guest_data") + attribute.add_output_key("text") + conc_face_attribute = yasmin.Concurrence( states={ "GET_ATTRIBUTES": GetGuestAttributes(guest_id=guest_id), - "LEARN_FACE": HRILearnFaces(guest_id=guest_id), + "LEARN_FACE": HRILearnFaces(guest_id=guest_id, dataset_size=10), }, default_outcome="failed", outcome_map={ @@ -87,60 +149,126 @@ def __init__(self, last_resort, guest_id): conc_name_drink_face.add_output_key("guest_data") self.add_state( - "SAY_WAITING_FOR_GUEST", - Say(text="I am waiting for a guest."), + "GREET_AND_ASK_GUEST", + AskAndListen( + tts_phrase="Please say 'Hi Tiago' for me to begin listening. What is your name and drink?", + ), transitions={ - "succeeded": "WAIT_FOR_GUEST", - "aborted": "WAIT_FOR_GUEST", - "canceled": "WAIT_FOR_GUEST", + "succeeded": "GET_NAME_DRINK_FACE", + "failed": "failed", }, + remappings={"transcribed_speech": "guest_transcription"}, ) + + transition = "GET_ATTRIBUTE_STR" if guest_id == "guest2" else "SAY_WELCOME" + self.add_state( - "WAIT_FOR_GUEST", - WaitForPersonInArea(), + "GET_NAME_DRINK_FACE", + conc_name_drink_face, transitions={ - "succeeded": "GET_PERSON_POINT", - "failed": "SAY_WAITING_FOR_GUEST", + "succeeded": transition, + "failed": "failed", + "failed_vision": "failed", + "failed_face": "failed", + "failed_attributes": "failed", }, - remappings={"detections_3d": "person_detections"}, ) + self.add_state( - "GET_PERSON_POINT", - GetPersonPoint(), + "SAY_WELCOME", + Say(format_str="Welcome to the party {}. Please follow me to be seated."), transitions={ - "succeeded": "START_EYE_TRACKER", - "failed": "SAY_WAITING_FOR_GUEST", + "succeeded": "STOP_EYE_TRACKING_1", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "STOP_EYE_TRACKING_1", + StopEyeTracker(), + transitions={ + "succeeded": "succeeded", + "failed": "failed", }, ) + + self.add_state( + "GET_ATTRIBUTE_STR", + attribute, + transitions={"succeeded": "SAY_ATTRIBUTE", "failed": "failed"}, + ) + self.add_state( - "START_EYE_TRACKER", - StartEyeTracker(), + "SAY_ATTRIBUTE", + Say(), transitions={ - "succeeded": "GREET_AND_ASK_GUEST", - "aborted": "SAY_WAITING_FOR_GUEST", + "succeeded": "STOP_EYE_TRACKING_2", + "aborted": "failed", "canceled": "failed", - 'timeout': 'GREET_AND_ASK_GUEST', }, ) + self.add_state( - "GREET_AND_ASK_GUEST", - AskAndListen( - tts_phrase="Please say 'Hi Tiago' for me to begin listening. What is your name and drink?", - ), + "STOP_EYE_TRACKING_2", + StopEyeTracker(), transitions={ - "succeeded": "GET_NAME_DRINK_FACE", - "failed": "GREET_AND_ASK_GUEST", + "succeeded": "WAIT", + "failed": "failed", }, - remappings={"transcribed_speech": "guest_transcription"}, ) + self.add_state( - "GET_NAME_DRINK_FACE", - conc_name_drink_face, + "WAIT", Wait(2), transitions={"succeeded": "GRAB_BAG", "failed": "failed"} + ) + + self.add_state( + "GRAB_BAG", + ReceiveObject(object_name="bag"), + transitions={"succeeded": "SAY_WELCOME_2", "failed": "failed"}, + ) + + self.add_state( + "SAY_WELCOME_2", + Say(text="Please follow me to be seated."), transitions={ "succeeded": "succeeded", - "failed": "failed", - "failed_vision": "failed", - "failed_face": "failed", - "failed_attributes": "failed", + "aborted": "failed", + "canceled": "failed", }, ) + + def get_guest1_attributes(self, blackboard): + attribute_str = "" + attributes = blackboard["guest_data"]["guest1"]["attributes"] + guest2_name = blackboard["guest_data"]["guest2"]["name"] + guest1_name = blackboard["guest_data"]["guest1"]["name"] + + for attribute in attributes.keys(): + value = attributes[attribute] + if attribute == "hair_color": + attribute_str += f" have {value} coloured hair." + elif attribute == "hair_length": + attribute_str += f" have {value} hair." + elif attribute == "glasses": + attribute_str += ( + " are wearing glasses." if value else " are not wearing glasses." + ) + elif attribute == "hat": + attribute_str += ( + " are wearing a hat." if value else " are not wearing a hat." + ) + elif attribute == "shirt_color": + attribute_str += f" are wearing a {value} coloured shirt." + else: + yasmin.YASMIN_LOG_ERROR( + f"The attribute {attribute} is not handled currently." + ) + + text = ( + f"Hello {guest2_name}, welcome to the party! {guest1_name} has already arrived and is sitting down. They " + + attribute_str + ) + yasmin.YASMIN_LOG_INFO(f"Attribute string: {text}") + blackboard["text"] = text + return "succeeded" diff --git a/tasks/HRI/HRI/states/hri_learn_faces.py b/tasks/HRI/HRI/states/hri_learn_faces.py index c70d7b539..87d70db01 100644 --- a/tasks/HRI/HRI/states/hri_learn_faces.py +++ b/tasks/HRI/HRI/states/hri_learn_faces.py @@ -104,22 +104,29 @@ class CheckDoneState(State): def __init__(self, dataset_size: int): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("num_images") + self.add_output_key("num_images") self._dataset_size = dataset_size def execute(self, blackboard): - if blackboard['num_images'] >= self._dataset_size: - yasmin.YASMIN_LOG_INFO("Collected enough images for the guest.") - return "succeeded" - else: - num_images = blackboard["num_images"] - yasmin.YASMIN_LOG_WARN( - f"Not enough images collected for the guest: {num_images}/{self._dataset_size}." - ) - return "failed" + if blackboard["num_images"] > self._dataset_size: + blackboard["num_images"] = 0 + try: + if blackboard["num_images"] == self._dataset_size: + yasmin.YASMIN_LOG_INFO("Collected enough images for the guest.") + return "succeeded" + else: + num_images = blackboard["num_images"] + yasmin.YASMIN_LOG_WARN( + f"Not enough images collected for the guest: {num_images}/{self._dataset_size}." + ) + return "failed" + except Exception as e: + blackboard["num_images"] = 0 + yasmin.YASMIN_LOG_WARN(f"An error was raised: {e}") def __init__(self, guest_id: str, dataset_size: int = 3): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("guest_data") diff --git a/tasks/HRI/HRI/states/introduce.py b/tasks/HRI/HRI/states/introduce.py new file mode 100644 index 000000000..b0bf52663 --- /dev/null +++ b/tasks/HRI/HRI/states/introduce.py @@ -0,0 +1,319 @@ +""" +State machine that introduces the greeted guest to all other guests/host present in the +seating area. + +Ported from SMACH to YASMIN. +""" + +import yasmin +import yasmin_ros +from shapely.geometry import Polygon as ShapelyPolygon + +from lasr_skills import ( + Say, + DetectAllInPolygon, + StartEyeTracker, + StopEyeTracker, + PlayMotion, + Wait, + LookToPoint, +) + +from geometry_msgs.msg import Point, PointStamped, Pose +from std_msgs.msg import Header + +from HRI.states import ( + ClearSeatingDetections, + GetGuestData, + GetIntroductionStr, + Recognise, +) + + +class Introduce(yasmin.StateMachine): + """ + State machine that introduces a guest to all other guests/host present in + the seating area. + + Replaces smach.Iterator with a CheckDone loop pattern. + + Blackboard keys required before calling sm(): + - guest_data: Dict of all guests keyed by id + - guest_seat_point: PointStamped of the incoming guest's seat + - seated_guest_locs: List of Point locations of all seated guests + - person_index: Set to 0 before calling sm() + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("guest_data") + self.add_input_key("guest_seat_point") + self.add_input_key("seated_guest_locs") + + self._node = yasmin_ros.logger_node + + self.seating_area = ShapelyPolygon( + [ + self._node.get_parameter("seat_area.top_left").value, + self._node.get_parameter("seat_area.top_right").value, + self._node.get_parameter("seat_area.bottom_right").value, + self._node.get_parameter("seat_area.bottom_left").value, + ] + ) + + loop_state = yasmin.CbState( + outcomes=["succeeded", "continue", "failed"], + callback=self._loop_person_index, + ) + loop_state.add_input_key("person_index") + loop_state.add_input_key("people_det") + loop_state.add_input_key("guest_data") + loop_state.add_output_key("person_index") + loop_state.add_output_key("person_point") + + guest_loop = yasmin.CbState( + outcomes=["succeeded", "continue"], callback=self._loop_guest + ) + guest_loop.add_input_key("guest_data") + guest_loop.add_output_key("guest_data") + + host_point = yasmin.CbState( + outcomes=["succeeded", "failed"], + callback=self._get_host, + ) + + host_point.add_input_key("guest_data") + host_point.add_output_key("host_point") + + self.add_state( + "RESET_SEATING_DETECTIONS", + ClearSeatingDetections(), + transitions={"succeeded": "LOOP_PERSON_STATE", "failed": "failed"}, + ) + + # self.add_state( + # "FIND_PEOPLE", + # DetectAllInPolygon( + # polygon=self.seating_area, + # object_filter=["person"], + # min_coverage=0.7, + # min_new_object_dist=0.50, + # min_confidence=0.5, + # ), + # transitions={"succeeded": "LOOP_PERSON_STATE", "failed": "failed"}, + # remappings={"detected_objects": "people_detected"}, + # ) + + self.add_state( + "LOOP_PERSON_STATE", + loop_state, + transitions={ + "succeeded": "GRAB_GUEST_POINT", + "continue": "LOOK_AT_PERSON", + "failed": "failed", + }, + ) + + self.add_state( + "LOOK_AT_PERSON", + LookToPoint(), + transitions={ + "succeeded": "WAIT", + "aborted": "WAIT", + "canceled": "failed", + "timeout": "WAIT", + }, + remappings={"pointstamped": "person_point_stamped"}, + ) + + self.add_state( + "WAIT", Wait(2), transitions={"succeeded": "RECOGNISE", "failed": "failed"} + ) + + self.add_state( + "RECOGNISE", + Recognise(), + transitions={ + "succeeded": "RESET_HEAD_1", + "aborted": "failed", + "no_detections": "RESET_HEAD_1", + }, + ) + + self.add_state( + "RESET_HEAD_1", + PlayMotion("look_centre"), + transitions={ + "succeeded": "LOOP_PERSON_STATE", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "GRAB_GUEST_POINT", + guest_loop, + transitions={"succeeded": "GET_HOST", "continue": "GET_INTRODUCTION_STR"}, + ) + + self.add_state( + "GET_INTRODUCTION_STR", + GetIntroductionStr(), + transitions={"succeeded": "LOOK_AT_GUEST", "failed": "failed"}, + ) + + self.add_state( + "LOOK_AT_GUEST", + LookToPoint(), + transitions={ + "succeeded": "SAY_INTRODUCTION", + "aborted": "SAY_INTRODUCTION", + "canceled": "failed", + "timeout": "SAY_INTRODUCTION", + }, + remappings={"pointstamped": "guest_point_stamped"}, + ) + + self.add_state( + "SAY_INTRODUCTION", + Say(), + transitions={ + "succeeded": "RESET_HEAD_2", + "aborted": "RESET_HEAD_2", + "canceled": "RESET_HEAD_2", + }, + ) + + self.add_state( + "RESET_HEAD_2", + PlayMotion("look_centre"), + transitions={ + "succeeded": "GRAB_GUEST_POINT", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "GET_HOST", + host_point, + transitions={ + "succeeded": "LOOK_AT_HOST", + "failed": "failed", + }, + ) + + self.add_state( + "LOOK_AT_HOST", + LookToPoint(), + transitions={ + "succeeded": "SAY_INTRODUCTION", + "aborted": "SAY_INTRODUCTION", + "canceled": "failed", + "timeout": "SAY_INTRODUCTION", + }, + remappings={"pointstamped": "host_pointstamped"}, + ) + + self.add_state( + "SAY_HOST", + Say( + text="Hello host! I have a bag for you. Can you stand in front of me to lead the way." + ), + transitions={ + "succeeded": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", + }, + ) + + def _loop_person_index(self, blackboard): + guest1point = blackboard["guest_data"]["guest1"]["seated_point"] + guest2point = blackboard["guest_data"]["guest2"]["seated_point"] + host = blackboard["guest_data"]["host"]["seated_point"] + people_det = len(blackboard["people_det"]) + index = blackboard["person_index"] + + indexes = [i for i in range(people_det)] + + yasmin.YASMIN_LOG_INFO(str(index)) + yasmin.YASMIN_LOG_INFO("Guest1 point: " + str(guest1point)) + yasmin.YASMIN_LOG_INFO("Guest2 point: " + str(guest2point)) + yasmin.YASMIN_LOG_INFO("Host point: " + str(host)) + yasmin.YASMIN_LOG_INFO("Total detections (seats + people): " + str(people_det)) + + if guest1point is not None and guest2point is not None and host is not None: + return "succeeded" + elif index < people_det: + point = blackboard["people_det"][index].point + point_stamped = PointStamped(header=Header(frame_id="map"), point=point) + blackboard["person_point_stamped"] = point_stamped + index += 1 + blackboard["person_index"] = index + return "continue" + elif guest2point is not None and host is not None: + index2 = blackboard["seat_indexes"]["guest2"] + indexh = blackboard["seat_indexes"]["host"] + for i in indexes: + if i != index2 and i != indexh: + index = i + blackboard["guest_data"]["guest1"]["seated_point"] = blackboard[ + "people_det" + ][index].point + guest2point = blackboard["guest_data"]["guest1"]["seated_point"] + yasmin.YASMIN_LOG_INFO("Fallback Guest1 point: " + str(guest2point)) + return "succeeded" + elif guest1point is not None and host is not None: + index2 = blackboard["seat_indexes"]["guest1"] + indexh = blackboard["seat_indexes"]["host"] + for i in indexes: + if i != index2 and i != indexh: + index = i + blackboard["guest_data"]["guest2"]["seated_point"] = blackboard[ + "people_det" + ][index].point + guest2point = blackboard["guest_data"]["guest2"]["seated_point"] + yasmin.YASMIN_LOG_INFO("Fallback Guest2 point: " + str(guest2point)) + return "succeeded" + + return "failed" + + def _get_host(self, blackboard): + if blackboard["guest_data"]["host"]["seated_point"]: + poinstamped = PointStamped( + header=Header(frame_id="map"), + point=blackboard["guest_data"]["host"]["seated_point"], + ) + blackboard["host_pointstamped"] = poinstamped + return "succeeded" + else: + yasmin.YASMIN_LOG_INFO(f"No host") + return "failed" + + def _loop_guest(self, blackboard): + if ( + blackboard["guest_data"]["guest1"]["seating_detection"] + and blackboard["guest_data"]["guest2"]["seating_detection"] + ): + return "succeeded" + + id = ( + "guest1" + if not blackboard["guest_data"]["guest1"]["seating_detection"] + else "guest2" + ) + + point = blackboard["guest_data"][id]["seated_point"] + blackboard["guest_point_stamped"] = PointStamped( + header=Header(frame_id="map"), point=point + ) + yasmin.YASMIN_LOG_INFO(id) + yasmin.YASMIN_LOG_INFO(str(point)) + blackboard["guest_data"][id]["seating_detection"] = True + blackboard["introduce_to"] = blackboard["guest_data"][id]["name"] + blackboard["relevant_guest_data"] = ( + blackboard["guest_data"]["guest2"] + if id == "guest1" + else blackboard["guest_data"]["guest1"] + ) + return "continue" diff --git a/tasks/HRI/HRI/states/learn_host_face.py b/tasks/HRI/HRI/states/learn_host_face.py deleted file mode 100644 index ddb40f786..000000000 --- a/tasks/HRI/HRI/states/learn_host_face.py +++ /dev/null @@ -1,68 +0,0 @@ -import yasmin -from yasmin import State, StateMachine - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import Header -from geometry_msgs.msg import PointStamped - -from .hri_learn_faces import HRILearnFaces -from lasr_skills import LookToPoint - -""" - This is used by seat_guest.py -""" - - -class GetLookPoint(State): - """State to get the look point for the guest to be seated.""" - - def __init__(self): - super().__init__(outcomes=["succeeded", "failed"]) - self.add_input_key("seated_guest_locs") - - self.add_output_key("pointstamped") - - def execute(self, blackboard): - """Set the pointstamped to the guest seat point.""" - if not blackboard["seated_guest_locs"]: - yasmin.YASMIN_LOG_WARN("No seated guest locations provided.") - return "failed" - point = blackboard["seated_guest_locs"][0] - blackboard["pointstamped"] = PointStamped( - header=Header(frame_id="map"), point=point - ) # TODO: Change to 'map' when 2dnav is fixed - return "succeeded" - - -class LearnHostFace(StateMachine): - """State machine to learn the host's face. Assumes seated guest is the host""" - - def __init__(self): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) - - self.add_input_key("guest_data") - self.add_input_key("seated_guest_locs") - - self.add_output_key("guest_data") - - self.add_state( - "GET_HOST_LOOK_POINT", - GetLookPoint(), - transitions={"succeeded": "LOOK_TO_HOST", "failed": "failed"}, - ) - self.add_state( - "LOOK_TO_HOST", - LookToPoint(), - transitions={ - "succeeded": "LEARN_HOST_FACE", - "aborted": "failed", - "canceled": "failed", - }, - ) - self.add_state( - "LEARN_HOST_FACE", - HRILearnFaces(guest_id="host", dataset_size=5), - transitions={"succeeded": "succeeded", "failed": "failed"}, - ) diff --git a/tasks/HRI/HRI/states/locate_and_follow_host.py b/tasks/HRI/HRI/states/locate_and_follow_host.py new file mode 100644 index 000000000..ca22555ab --- /dev/null +++ b/tasks/HRI/HRI/states/locate_and_follow_host.py @@ -0,0 +1,65 @@ +import yasmin +from yasmin import StateMachine, State, Concurrence, Blackboard +import yasmin_ros +from yasmin_viewer import YasminViewerPub + +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.executors import MultiThreadedExecutor + +import numpy as np +import tf2_ros as tf +from typing import Optional + +from shapely.geometry import Polygon as ShapelyPolygon +from shapely.geometry import Point as ShapelyPoint + +from std_msgs.msg import Header +from geometry_msgs.msg import Point, PointStamped +from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point + +from .learn_host_face import LearnHostFace +from lasr_vision_interfaces.msg import Detection3D +from lasr_skills import ( + FollowPerson, + Detect3DInArea, + AskAndListen, + Say, + Wait, + WaitForPersonInArea, +) + +from HRI.states import ( + GetPersonPoint, +) + +from yasmin_viewer import YasminViewerPub + + +class RequestHostForGuiding(StateMachine): + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + + self._node = yasmin_ros.logger_node + + self.add_state( + "ACKNOWLEDGE_BAG", + Say( + text="I have a bag for the host. Can the host stand in front of me? I will wait for you." + ), + transitions={ + "succeeded": "WAIT_FOR_HOST", + "aborted": "WAIT_FOR_HOST", + "canceled": "WAIT_FOR_HOST", + }, + ) + + self.add_state( + "FOLLLOW_HOST", + FollowPerson(), + transitions={ + "succeeded": "succeeded", + "failed": "failed", # If failed we should try to drop bag anyway? or have person raise hand as secondary recovery + }, + ) diff --git a/tasks/HRI/HRI/states/place_bag.py b/tasks/HRI/HRI/states/place_bag.py new file mode 100644 index 000000000..1e06ed8b0 --- /dev/null +++ b/tasks/HRI/HRI/states/place_bag.py @@ -0,0 +1,418 @@ +import math +import rclpy +from rclpy.time import Time +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy + + +import time +import traceback + +import yasmin +from yasmin import State, Blackboard, StateMachine, Concurrence +import yasmin_ros +from yasmin_ros import ServiceState +from yasmin_viewer import YasminViewerPub + +import tf2_ros + +from std_msgs.msg import Header +from geometry_msgs.msg import ( + PointStamped, + PoseWithCovarianceStamped, + Pose, + PolygonStamped, + Point, +) +from visualization_msgs.msg import Marker +from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point +from shapely.geometry import Polygon as ShapelyPolygon + +from lasr_skills import ( + DetectKeypoints3D, + Wait, + PlayMotion, + Say, + Rotate, + FollowPerson, + LookToPoint, + AskAndListen, +) +import random + + +class CalculateDropPoint(State): + def __init__(self): + super().__init__(outcomes=["valid_point", "invalid_point", "failed"]) + self.add_input_key("keypoint_detections_3d") + self.add_output_key("drop_point") + + self.node = yasmin_ros.logger_node + self.debug_pub = self.node.create_publisher( + Marker, + "/place_bag/debug/drop_point", + 10, + ) + + def calcuate_point(self, elbow_point, wrist_point, angle_max=70.0): + """Returns (valid point, Point)""" + v_x = wrist_point.x - elbow_point.x + v_y = wrist_point.y - elbow_point.y + v_z = wrist_point.z - elbow_point.z + + t = -elbow_point.z / v_z + + drop_point = Point() + drop_point.x = elbow_point.x + (t * v_x) + drop_point.y = elbow_point.y + (t * v_y) + drop_point.z = 0.0 + + # Angle between z and pointing + angle = math.degrees(math.asin(abs(v_z) / math.sqrt(v_x**2 + v_y**2 + v_z**2))) + yasmin.YASMIN_LOG_INFO(f"Pointing at {drop_point} at angle{angle} from z.") + + if v_z >= 0 or angle > angle_max: + yasmin.YASMIN_LOG_INFO("Not Pointing at floor") + return False, drop_point + + # DEBUG + marker = Marker() + marker.header.frame_id = "map" + marker.header.stamp = self.node.get_clock().now().to_msg() + marker.id = 1 + marker.type = Marker.SPHERE + marker.action = Marker.ADD + + marker.pose.position = drop_point + + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + + self.debug_pub.publish(marker) + + return True, drop_point + + def execute(self, blackboard): + try: + # Use detect_keypoints to get points of right elbow and wrist. + # calcuate vector and follow vector untill it hits z=0 (floor) and retrive drop point + + # ----- Get elbow and wrist points + elbow_point = None + wrist_point = None + for detection in blackboard[ + "keypoint_detections_3d" + ].detections: # Assumed closest person is + kp = {k.keypoint_name: k.point for k in detection.keypoints} + + if "right_elbow" in kp and "right_wrist" in kp: + elbow_point = kp["right_elbow"] + wrist_point = kp["right_wrist"] + break + + if None in (elbow_point, wrist_point): + return "invalid_point" + + # ----- Calculate drop point + valid, drop_point = self.calcuate_point(elbow_point, wrist_point) + + if not valid: + return "invalid_point" + + point_stamped = PointStamped() + point_stamped.header.frame_id = "map" + point_stamped.header.stamp = Time().to_msg() + point_stamped.point = drop_point + blackboard["drop_point"] = point_stamped + + return "valid_point" + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"The following error occured: {e}") + return "failed" + + +class PlacingMotion(StateMachine): + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("drop_point") + + self.add_state( + "PRE_NAV", + PlayMotion("pre_navigation"), + transitions={ + "succeeded": "FACE_DROP_POINT", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "FACE_DROP_POINT", + Rotate(mode="point"), + transitions={"succeeded": "LOOK_AT_DROP_POINT", "failed": "failed"}, + remappings={"target_point": "drop_point"}, + ) + + self.add_state( + "LOOK_AT_DROP_POINT", + LookToPoint(), + transitions={ + "succeeded": "PLACE_MOTION", + "aborted": "failed", + "canceled": "failed", + "timeout": "failed", + }, + remappings={"pointstamped": "drop_point"}, + ) + + self.add_state( + "PLACE_MOTION", + PlayMotion("reach_arm_vertical_gripper"), # Motion goes heres + transitions={ + "succeeded": "PLAYMOTION_BREAK_1", + "aborted": "failed", + "canceled": "failed", + }, + ) + self.add_state( + "PLAYMOTION_BREAK_1", + Wait(1), + transitions={"succeeded": "RELEASE_BAG", "failed": "failed"}, + ) + + self.add_state( + "RELEASE_BAG", + PlayMotion("open"), # Motion goes heres + transitions={ + "succeeded": "PLAYMOTION_BREAK_2", + "aborted": "failed", + "canceled": "failed", + }, + ) + self.add_state( + "PLAYMOTION_BREAK_2", + Wait(1), + transitions={"succeeded": "RESET", "failed": "failed"}, + ) + self.add_state( + "RESET", + PlayMotion("home"), # Motion goes heres + transitions={ + "succeeded": "CLOSE_GRIPPER", + "aborted": "failed", + "canceled": "failed", + }, + ) + self.add_state( + "CLOSE_GRIPPER", + PlayMotion("close"), # Motion goes heres + transitions={ + "succeeded": "LOOK_CENTRE", + "aborted": "failed", + "canceled": "failed", + }, + ) + self.add_state( + "LOOK_CENTRE", + PlayMotion("look_centre"), # Motion goes heres + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "canceled": "failed", + }, + ) + + +class PlaceBag(StateMachine): + def __init__(self): + # Outcomes align perfectly with your main locate_and_follow_host.py plan + super().__init__(outcomes=["succeeded", "failed"]) + + self.add_state( + "POST_NAV", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "REQUEST_DROP_POINT", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "REQUEST_DROP_POINT", + Say( + text="With your right hand, please point where on the floor I should place the bag. " + ), + transitions={ + "succeeded": "WAIT_FOR_POINT", + "aborted": "WAIT_FOR_POINT", + "canceled": "WAIT_FOR_POINT", + }, + ) + self.add_state( + "WAIT_FOR_POINT", + Wait(3), + transitions={"succeeded": "DETECT3D_POSE", "failed": "failed"}, + ) + self.add_state( + "DETECT3D_POSE", + DetectKeypoints3D(), + transitions={ + "succeeded": "FIND_DROP_POINT", + "failed": "NO_POSE_FOUND", # Some recovery then return back + }, + ) + self.add_state( + "NO_POSE_FOUND", + Say(text="I can't see where you are pointing properly. I will try again. "), + transitions={ + "succeeded": "WAIT_FOR_POINT", + "aborted": "WAIT_FOR_POINT", + "canceled": "WAIT_FOR_POINT", + }, + ) + + self.add_state( + "FIND_DROP_POINT", + CalculateDropPoint(), + transitions={ + "valid_point": "LOOK_AT_DROP_POINT_1", + "invalid_point": "REQUEST_NEW_POINT", + "failed": "failed", + }, + ) + + self.add_state( + "LOOK_AT_DROP_POINT_1", + LookToPoint(), + transitions={ + "succeeded": "ASK_TO_STEP_AWAY", + "aborted": "failed", + "canceled": "failed", + "timeout": "failed", + }, + remappings={"pointstamped": "drop_point"}, + ) + + # TODO: Add a 3d detect in area check to ensure area is empty + + self.add_state( + "REQUEST_NEW_POINT", + Say( + text="I cannot place the bag there. Please point somewhere on the floor. " + ), + transitions={ + "succeeded": "DETECT3D_POSE", + "aborted": "DETECT3D_POSE", + "canceled": "DETECT3D_POSE", + }, + ) + + self.add_state( + "ASK_TO_STEP_AWAY", + Say( + text="Please step away. I will wait a few seconds, then place the bag." + ), + transitions={ + "succeeded": "WAIT", + "aborted": "WAIT", + "canceled": "WAIT", + }, + ) + + self.add_state( + "WAIT", + Wait(3), + transitions={"succeeded": "PLACE_BAG_MOTION", "failed": "failed"}, + ) + + # Add a sweep here and make a seperate SM for this + + # TODO: Navigate close to the point + + self.add_state( + "PLACE_BAG_MOTION", + PlacingMotion(), + transitions={"succeeded": "FINISH", "failed": "failed"}, + ) + + self.add_state( + "FINISH", + Say(text="I have finished the task. "), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "canceled": "failed", + }, + ) + + +""" + 1. Ask person to point where on the floor to place the bag with right hand. + 2. take keypoints of (elbow and wrist) pose and create vector to find point where to place + 3. Ask to step away + 4. Do playmotion of 'Drop Item' + 5. drive close enough to the point + 6. Open Gripper + 7. Home + 8. Go to end/ finish task. + +""" +## Later can adapt to check if they want to drop on table/ chair and if it is low enough ok if not request floor. + + +def main(): + rclpy.init() + + yasmin_ros.set_ros_loggers() + + sm = StateMachine(outcomes=["succeeded", "failed"]) + sm.add_state( + "CALL_HOST", + Say(text="I have a bag. Can the host stand infront of me to lead the way."), + transitions={ + "succeeded": "FOLLOW_HOST", + "aborted": "failed", + "canceled": "failed", + }, + ) + + sm.add_state( + "FOLLOW_HOST", + FollowPerson(), + transitions={ + "succeeded": "PLACE_BAG", + "failed": "failed", + }, + ) + + sm.add_state( + "PLACE_BAG", + PlaceBag(), + transitions={ + "succeeded": "succeeded", + "failed": "failed", + }, + ) + # sm = PlaceBag() + sm.set_sigint_handler(True) + bb = Blackboard() + bb["z_sweep_min"] = -10 + bb["z_sweep_max"] = 50 + + YasminViewerPub(sm, "Follow_Person") + + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(outcome) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/tasks/HRI/HRI/states/recognise.py b/tasks/HRI/HRI/states/recognise.py new file mode 100644 index 000000000..994f43f69 --- /dev/null +++ b/tasks/HRI/HRI/states/recognise.py @@ -0,0 +1,163 @@ +from typing import List, Dict, Optional + + +import rclpy +import yasmin +import yasmin_ros +import numpy as np +import cv2 +import time +from yasmin import Blackboard +from cv_bridge import CvBridge +from sensor_msgs.msg import Image, CameraInfo + +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +import message_filters + +from lasr_vision_interfaces.msg import Detection3D +from lasr_vision_interfaces.srv import Recognise3D, YoloDetection3D +from . import HRILearnFaces + + +class Recognise(yasmin_ros.ServiceState): + def __init__(self): + super().__init__( + srv_type=Recognise3D, + srv_name="/lasr_vision_reid/recognise", + create_request_handler=self._create_request, + response_handler=self._handle_resp, + outcomes=["no_detections"], + ) + + self.add_output_key("guest_data") + + self.image_pub = self._node.create_publisher(Image, "recognise/image", 10) + + camera_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + ) + + self.data = None + + depth_info = message_filters.Subscriber( + self._node, + CameraInfo, + "head_front_camera/depth/camera_info", + qos_profile=camera_qos, + ) + + self.cache = message_filters.Cache(depth_info) + + image_sub = message_filters.Subscriber( + self._node, Image, "head_front_camera/rgb/image_raw", qos_profile=camera_qos + ) + + depth_sub = message_filters.Subscriber( + self._node, + Image, + "head_front_camera/depth/image_raw", + qos_profile=camera_qos, + ) + + self.ts = message_filters.ApproximateTimeSynchronizer( + [image_sub, depth_sub], queue_size=10, slop=0.1 + ) + + self.ts.registerCallback(self.callback) + + def callback(self, image_msg, depth_msg): + if self.data is None: + self.data = (image_msg, depth_msg) + + def _create_request(self, blackboard): + self.data = None + + request = Recognise3D.Request() + + while self.data is None: + yasmin.YASMIN_LOG_INFO("Waiting for synced rgb and depth frames") + time.sleep(1) + + image, depth = self.data + + self.image_pub.publish(image) + + request.image_raw = image + request.depth_image = depth + request.depth_camera_info = self.cache.getLast() + request.threshold = 0.2 + request.target_frame = "map" + + return request + + def _handle_resp(self, blackboard, response): + detected = False + if len(response.detections) == 0: + return "no_detections" + else: + for detection in response.detections: + if detection.name == "unknown": + continue + yasmin.YASMIN_LOG_INFO(detection.name) + yasmin.YASMIN_LOG_INFO(str(detection.point)) + blackboard["guest_data"][detection.name][ + "seated_point" + ] = detection.point + blackboard["seat_indexes"][detection.name] = blackboard["person_index"] + detected = True + + return "aborted" if not detected else "succeeded" + + +def check(blackboard): + dict = blackboard["guest_data"] + yasmin.YASMIN_LOG_INFO(str(dict)) + return "succeeded" + + +def main(): + global check + rclpy.init() + + yasmin_ros.set_ros_loggers() + + sm = yasmin.StateMachine(outcomes=["succeeded", "failed"], handle_sigint=True) + + check = yasmin.CbState(outcomes=["succeeded"], callback=check) + + sm.add_state( + "ADD_FACE", + HRILearnFaces(guest_id="guest1", dataset_size=10), + transitions={"succeeded": "RECOGNISE", "failed": "failed"}, + ) + + sm.add_state( + "RECOGNISE", + Recognise(), + transitions={ + "succeeded": "CHECK", + "aborted": "failed", + "no_detections": "failed", + }, + ) + + sm.add_state("CHECK", check, transitions={"succeeded": "succeeded"}) + + bb = Blackboard() + bb["guest_data"] = { + "guest1": { + "name": "", + "drink": "", + "detection": False, + "seating_detection": False, + "attributes": {}, + "seated_point": None, + } + } + + outcome = sm(bb) + + rclpy.shutdown() diff --git a/tasks/HRI/HRI/states/seat_guest.py b/tasks/HRI/HRI/states/seat_guest.py index 08c628b24..293bef5e0 100644 --- a/tasks/HRI/HRI/states/seat_guest.py +++ b/tasks/HRI/HRI/states/seat_guest.py @@ -19,7 +19,6 @@ from geometry_msgs.msg import Point, PointStamped from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point -from .learn_host_face import LearnHostFace from lasr_vision_interfaces.msg import Detection3D from lasr_skills import ( PlayMotion, @@ -28,8 +27,11 @@ Say, Wait, DetectAllInPolygon, + StopEyeTracker, ) +from HRI.states import HRILearnFaces + from yasmin_viewer import YasminViewerPub @@ -65,36 +67,6 @@ def __init__( self._tf_buffer = tf.Buffer(cache_time=Duration(seconds=10.0)) self._tf_listener = tf.TransformListener(self._tf_buffer, self._node) - def _determine_side_of_sofa(self, sofa_detection: Detection3D) -> str: - """Determines which side of the sofa is empty, in order to seat - the guest there. - - Args: - sofa_detection (Detection3D): Detection of the other - guest who is already sat on the sofa. - - Returns: - str: "left" or "right" - which side of the sofa is empty. - """ - sofa_guest_point = sofa_detection.point - - if self._left_sofa_area.contains( - ShapelyPoint(sofa_guest_point.x, sofa_guest_point.y) - ): - result = "right" - elif self._right_sofa_area.contains( - ShapelyPoint(sofa_guest_point.x, sofa_guest_point.y) - ): - result = "left" - else: - yasmin.YASMIN_LOG_WARN( - "Sofa guest point is not within the left or right sofa area. " - "Defaulting to 'right'." - ) - result = "right" - - return result - def execute(self, blackboard): """ Input: @@ -103,129 +75,79 @@ def execute(self, blackboard): """ yasmin.YASMIN_LOG_WARN("Finding seat in seat guest") - seat_sofa = True - seated_guests_loc = [ - detection.point - for detection in blackboard["non_sofa_detections"] - if detection.name == "person" - ] - seated_guests_sofa_loc = [ - detection.point - for detection in blackboard["sofa_detections"] - if detection.name == "person" - ] - seated_guest_locs = seated_guests_loc + seated_guests_sofa_loc + left_sofa_occupied = False + right_sofa_occupied = False + unseated_sofa_persons = [] + non_sofa_chairs = {} + people = [] + + for detection in blackboard["seat_detections"]: + detection_point = ShapelyPoint( + detection.point.x, detection.point.y, detection.point.z + ) + if detection.name == "person": + people.append(detection) + if self._left_sofa_area.contains(detection_point): + left_sofa_occupied = True + elif self._right_sofa_area.contains(detection_point): + right_sofa_occupied = True + else: + unseated_sofa_persons.append(detection_point) + elif ( + detection.name == "chair" + and not self._right_sofa_area.contains(detection_point) + and not self._left_sofa_area.contains(detection_point) + ): + non_sofa_chairs.update({detection_point: False}) + yasmin.YASMIN_LOG_INFO( - f"Detected {len(seated_guest_locs)} seated guests in the seating area." + "Detected this many people in sweep: " + str(len(people)) ) - yasmin.YASMIN_LOG_INFO(f"Detections are: {seated_guest_locs}") - if len(seated_guest_locs) > 2: - yasmin.YASMIN_LOG_WARN( - f"Too many people detected: {len(seated_guest_locs)} detected, max allowed is 2." + + if len(people) == 1: + blackboard["pointstamped"] = PointStamped( + header=Header(frame_id="map"), point=people[0].point ) - blackboard["seated_guest_locs"] = seated_guest_locs[:2] else: - blackboard["seated_guest_locs"] = seated_guest_locs - sofa_detections = blackboard["sofa_detections"] - yasmin.YASMIN_LOG_WARN( - f"people on the sofa: {len(sofa_detections)} detected, max allowed is {self._max_people_on_sofa}." - ) - if len(blackboard["sofa_detections"]) > self._max_people_on_sofa: - yasmin.YASMIN_LOG_WARN( - f"Too many people on the sofa: {len(sofa_detections)} detected, max allowed is {self._max_people_on_sofa}." - ) - seat_sofa = False - elif len(blackboard["sofa_detections"]) == self._max_people_on_sofa: - yasmin.YASMIN_LOG_WARN(f"Sofa max capacity has been reached.") - seat_sofa = False + blackboard["people_det"] = people + + for chair_detection in non_sofa_chairs.keys(): + for person_detection in unseated_sofa_persons: + if chair_detection.distance(person_detection) < 0.2: + non_sofa_chairs[chair_detection] = True # Chair is occupied + break - if seat_sofa: + if left_sofa_occupied != right_sofa_occupied: + seating_side = "left" if right_sofa_occupied else "right" + blackboard["seating_string"] = ( + "The sofa that I'm looking at is occupied by one person. " + f"Please take a seat next to them on the {seating_side} side of the sofa." + ) blackboard["guest_seat_point"] = PointStamped( header=Header(frame_id="map"), point=self._sofa_point ) - if len(blackboard["sofa_detections"]) == 0: - blackboard["seating_string"] = ( - "The sofa that I'm looking at is empty. Please take a seat anywhere on the sofa." - ) - elif len(blackboard["sofa_detections"]) == 1: - seating_side = self._determine_side_of_sofa( - blackboard["sofa_detections"][0] - ) - blackboard["seating_string"] = ( - "The sofa that I'm looking at is occupied by one person. " - f"Please take a seat next to them on the {seating_side} side of the sofa." - ) - else: - seated_guests_xywh = [ - detection.xywh - for detection in blackboard["non_sofa_detections"] - if detection.name == "person" - ] - done = False - for detection in blackboard["non_sofa_detections"]: - if done: + elif left_sofa_occupied and right_sofa_occupied: + for chair in non_sofa_chairs.keys(): + if not non_sofa_chairs[chair]: + blackboard["seating_string"] = ( + "The sofa that I'm looking at is at full capacity. I have found an extra seat for you. Please sit down in the seat I am looking at." + ) + blackboard["guest_seat_point"] = PointStamped( + header=Header(frame_id="map"), + point=Point(x=chair.x, y=chair.y, z=chair.z), + ) break - if detection.name == "chair": - # Check if a person is sitting on the chair - chair_bbox = detection.xywh - overlap_pct = 0.0 - for guest_xywh in seated_guests_xywh: - overlap_pct_current = ( - np.maximum( - 0, - np.minimum( - chair_bbox[0] + chair_bbox[2], - guest_xywh[0] + guest_xywh[2], - ) - - np.maximum(chair_bbox[0], guest_xywh[0]), - ) - * np.maximum( - 0, - np.minimum( - chair_bbox[1] + chair_bbox[3], - guest_xywh[1] + guest_xywh[3], - ) - - np.maximum(chair_bbox[1], guest_xywh[1]), - ) - ) / (chair_bbox[2] * chair_bbox[3]) - overlap_pct = max(overlap_pct, overlap_pct_current) - if overlap_pct > 0.5: - yasmin.YASMIN_LOG_INFO( - f"Detected a person sitting on a chair with bbox {chair_bbox}, with overlap percentage {overlap_pct:.2f}." - ) - continue - else: - yasmin.YASMIN_LOG_INFO( - f"No person detected sitting on chair with bbox {chair_bbox}." - ) - blackboard["guest_seat_point"] = PointStamped( - header=Header(frame_id="map"), - point=Point( - x=detection.point.x, - y=detection.point.y, - z=detection.point.z, - ), - ) - blackboard["seating_string"] = ( - "The sofa is full, but I have found a chair for you. Please take a seat on the chair that I'm looking at." - ) - done = True - - if not done: - blackboard["seating_string"] = ( - "Uh oh, I couldn't find a seat for you. Please take a seat anywhere in the seating area." - ) - blackboard["guest_seat_point"] = PointStamped( - header=Header(frame_id="map"), - point=Point( - x=self._sofa_point.x, y=self._sofa_point.y, z=self._sofa_point.z - ), - ) + else: + blackboard["seating_string"] = ( + "The sofa that I'm looking at is empty. Please take a seat anywhere on the sofa." + ) + blackboard["guest_seat_point"] = PointStamped( + header=Header(frame_id="map"), point=self._sofa_point + ) return "succeeded" -# TODO: update so that it the params are optional and directly loaded from the params (overriden by param if provided) class SeatGuest(StateMachine): """ args: @@ -241,116 +163,50 @@ class SeatGuest(StateMachine): def __init__( self, - seating_area: Optional[ShapelyPolygon] = None, - sofa_area: Optional[ShapelyPolygon] = None, - sofa_point: Optional[Point] = None, - left_sofa_area: Optional[ShapelyPolygon] = None, - right_sofa_area: Optional[ShapelyPolygon] = None, - max_people_on_sofa: Optional[int] = None, - learn_host: bool = False, + guest_id: str, ): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("guest_data") self.add_output_key("guest_seat_point") - self.add_output_key("seated_guest_locs") self._node = yasmin_ros.logger_node self.__load_ros_parameters() - # TODO: Update to allow local paramters overriding ros param - - seating_area_minus_sofa = self.seating_area.difference(self.sofa_area) - - def check(blackboard): - detections = blackboard['guest_data'] - yasmin.YASMIN_LOG_INFO(str(detections)) - - return 'succeeded' - - # self.userdata.z_sweep_min = ( - # -0.5 - # ) # TODO: Remove when testing on robot move as paramter to detect3d... - # self.userdata.z_sweep_max = 100 # TODO: Remove when testing on robot - # self.blackboard["seated_guest_locs"] = [] - - self.add_state('CHECK', yasmin.CbState(outcomes=['succeeded'], callback=check), transitions={'succeeded': 'SAY_FINDING_SEAT'}) - - ### ADD IN STOP EYE TRACKER self.add_state( "SAY_FINDING_SEAT", Say(text="I will now find a seat for you."), transitions={ - "succeeded": "LOOK_TO_SOFA", - "aborted": "LOOK_TO_SOFA", - "canceled": "LOOK_TO_SOFA", - }, - ) - self.add_state( - "LOOK_TO_SOFA", - LookToPoint( - pointstamped=PointStamped( - header=Header(frame_id="map"), - point=self.sofa_point, # TODO: Change to 'map' when 2dnav is fixed - ) - ), - transitions={ - "succeeded": "DETECT_SOFA", + "succeeded": "RESET_HEAD_1", "aborted": "failed", "canceled": "failed", - "timeout": "DETECT_SOFA", # Sometimes completes action but still timesouts? }, ) - self.add_state( - "DETECT_SOFA", - Detect3DInArea( - area_polygon=self.sofa_area, - filter=["person"], - z_min=-10, - z_max=50.0, - confidence=0.7, - ), - transitions={"succeeded": "RESET_HEAD_1", "failed": "failed"}, - remappings={"detections_3d": "sofa_detections"}, - ) + self.add_state( "RESET_HEAD_1", PlayMotion(motion_name="look_centre"), transitions={ - "succeeded": "DETECT_NON_SOFA", + "succeeded": "DETECT_ALL_PEOPLE_SEATS", "aborted": "failed", "canceled": "failed", }, ) - # self.add_state( - # "DETECT_NON_SOFA", - # Detect3DInArea( - # area_polygon=seating_area_minus_sofa, - # filter=["person", "chair"], - # z_min=-10, - # z_max=50.0, - # confidence=0.5, - # ), - # transitions={"succeeded": "PROCESS_DETECTIONS", "failed": "failed"}, - # remappings={"detections_3d": "non_sofa_detections"}, - # ) self.add_state( - "DETECT_NON_SOFA", + "DETECT_ALL_PEOPLE_SEATS", DetectAllInPolygon( - polygon=seating_area_minus_sofa, # TODO: Verify Potential type mismatch (BaseGeometry vs accepted ShapelyPolygon) + polygon=self.seating_area, object_filter=["person", "chair"], - min_coverage=1.0, + min_coverage=0.7, min_new_object_dist=0.50, min_confidence=0.5, ), transitions={"succeeded": "PROCESS_DETECTIONS", "failed": "failed"}, - remappings={"detected_objects": "non_sofa_detections"}, + remappings={"detected_objects": "seat_detections"}, ) - # Process detections - if learn_host: - detection_transition = "SAY_AND_LEARN_HOST_FACE" - else: - detection_transition = "LOOK_TO_SEAT" + + transition = "LOOK_HOST" if guest_id == "guest1" else "LOOK_TO_SEAT" + self.add_state( "PROCESS_DETECTIONS", ProcessDetections( @@ -359,40 +215,35 @@ def check(blackboard): left_sofa_area=self.left_sofa_area, right_sofa_area=self.right_sofa_area, ), - transitions={"succeeded": detection_transition, "failed": "failed"}, + transitions={"succeeded": transition, "failed": "failed"}, ) - if learn_host: - # Look to the only person detection and learn the host's face. - sm_con = Concurrence( - states={ - "SAY_LEARN_HOST_FACE": Say( - text="I'm quickly remembering the host's face." - ), - "LEARN_HOST_FACE": LearnHostFace(), - }, - default_outcome="failed", - outcome_map={ - "succeeded": { - "SAY_LEARN_HOST_FACE": "succeeded", - "LEARN_HOST_FACE": "succeeded", - }, - "failed": { - "SAY_LEARN_HOST_FACE": "aborted", - "LEARN_HOST_FACE": "failed", - }, - }, - ) - sm_con.add_input_key("guest_data") - sm_con.add_input_key("seated_guest_locs") - sm_con.add_output_key("guest_data") - sm_con.add_output_key("seated_guest_locs") + self.add_state( + "LOOK_HOST", + LookToPoint(), + transitions={ + "succeeded": "SAY_HOST", + "aborted": "SAY_HOST", + "canceled": "SAY_HOST", + "timeout": "SAY_HOST", + }, + ) - self.add_state( - "SAY_AND_LEARN_HOST_FACE", - sm_con, - transitions={"succeeded": "LOOK_TO_SEAT", "failed": "LOOK_TO_SEAT"}, - ) + self.add_state( + "SAY_HOST", + Say(text="I am going to quickly learn the host's face."), + transitions={ + "succeeded": "LEARN_HOST", + "aborted": "LEARN_HOST", + "canceled": "LEARN_HOST", + }, + ) + + self.add_state( + "LEARN_HOST", + HRILearnFaces(guest_id="host", dataset_size=10), + transitions={"succeeded": "LOOK_TO_SEAT", "failed": "failed"}, + ) self.add_state( "LOOK_TO_SEAT", @@ -401,7 +252,7 @@ def check(blackboard): "succeeded": "SAY_SEAT_GUEST", "aborted": "SAY_SEAT_GUEST", "canceled": "SAY_SEAT_GUEST", - "timeout": "SAY_SEAT_GUEST" + "timeout": "SAY_SEAT_GUEST", }, remappings={"pointstamped": "guest_seat_point"}, ) @@ -432,22 +283,6 @@ def check(blackboard): ) def __load_ros_parameters(self): - # Declare parameters - # self._node.declare_parameter("sofa_point.x", 0.0) - # self._node.declare_parameter("sofa_point.y", 0.0) - # self._node.declare_parameter("sofa_point.z", 0.0) - - # self._node.declare_parameter("seat_area.top_left", [0.0, 0.0]) - # self._node.declare_parameter("seat_area.top_right", [0.0, 0.0]) - # self._node.declare_parameter("seat_area.bottom_right", [0.0, 0.0]) - # self._node.declare_parameter("seat_area.bottom_left", [0.0, 0.0]) - - # self._node.declare_parameter("sofa_area.top_left", [0.0, 0.0]) - # self._node.declare_parameter("sofa_area.top_right", [0.0, 0.0]) - # self._node.declare_parameter("sofa_area.bottom_right", [0.0, 0.0]) - # self._node.declare_parameter("sofa_area.bottom_left", [0.0, 0.0]) - - # self._node.declare_parameter("max_people_on_sofa", 2) # Load parameters from file self.seating_area = ShapelyPolygon( @@ -477,8 +312,6 @@ def __load_ros_parameters(self): self._node.get_parameter("sofa_area.bottom_left").value ), } - - # TODO: Check if number of section on sofa depends on number of sofa_middle_top = (sofa_area["top_right"] + sofa_area["top_left"]) / 2 sofa_middle_bottom = (sofa_area["bottom_left"] + sofa_area["bottom_right"]) / 2 @@ -513,12 +346,14 @@ def __load_ros_parameters(self): self._node.get_parameter("max_people_on_sofa").value ) + try: from rclpy.executors import EventsExecutor as Executor except ImportError: from rclpy.executors import MultiThreadedExecutor as Executor from threading import Thread + class HRI_node(Node): def __init__(self): super().__init__( @@ -532,15 +367,14 @@ def __init__(self): self._spin_thread = Thread(target=self._executor.spin) self._spin_thread.start() -def main(): +def main(): rclpy.init() node = HRI_node() yasmin_ros.set_ros_loggers(node) try: - #TODO: Try with learn_host=True sm = SeatGuest(learn_host=False) bb = Blackboard() @@ -565,7 +399,6 @@ def main(): }, } - YasminViewerPub(sm, "HRI_SM3") outcome = sm(bb) diff --git a/tasks/HRI/config/debug.rviz b/tasks/HRI/config/debug.rviz index e517c2c1e..909ebb40d 100644 --- a/tasks/HRI/config/debug.rviz +++ b/tasks/HRI/config/debug.rviz @@ -4,18 +4,17 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - /TF1/Frames1 - /TF1/Tree1 - - /Image1 - /Image1/Topic1 - /PointCloud21/Topic1 - - /Polygon1 - /Polygon2 - /PointStamped1 - /Marker1 + - /MarkerArray1 + - /MarkerArray1/Topic1 Splitter Ratio: 0.5833333134651184 - Tree Height: 645 + Tree Height: 372 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -648,7 +647,7 @@ Visualization Manager: Enabled: true Name: Controller - Class: rviz_default_plugins/Image - Enabled: false + Enabled: true Max Value: 1 Median window: 5 Min Value: 0 @@ -660,7 +659,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Best Effort Value: /head_front_camera/rgb/image_raw - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -698,7 +697,7 @@ Visualization Manager: - Alpha: 1 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 - Enabled: true + Enabled: false Name: Polygon Topic: Depth: 5 @@ -707,7 +706,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /projected_fov_polygon - Value: true + Value: false - Alpha: 1 Class: rviz_default_plugins/Polygon Color: 255; 0; 0 @@ -724,7 +723,7 @@ Visualization Manager: - Alpha: 1 Class: rviz_default_plugins/PointStamped Color: 204; 41; 204 - Enabled: true + Enabled: false History Length: 10 Name: PointStamped Radius: 0.10000000149011612 @@ -735,7 +734,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /sweep_points - Value: true + Value: false - Class: rviz_default_plugins/Marker Enabled: true Name: Marker @@ -747,7 +746,19 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /yolo/detect3d/yolo11n_seg_pt + Value: /lasr_vision_reid/recognise/points + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 1 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /yolo/pose3d/yolo11n_pose_pt Value: true Enabled: true Global Options: @@ -787,43 +798,43 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10.055266380310059 + Distance: 0.18920794129371643 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 1.1280258893966675 + Y: 0.01059875637292862 + Z: 1.9105149507522583 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.919796884059906 + Pitch: 0.16979792714118958 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 0.9223852157592773 + Yaw: 1.4442015886306763 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 1131 - Hide Left Dock: true + collapsed: false + Height: 1163 + Hide Left Dock: false Hide Right Dock: true Image: - collapsed: true + collapsed: false Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000415fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000002c0000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000001b1000001230000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000fb0000000a0049006d00610067006500000003010000014f0000002800ffffff000000010000010f00000415fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000415000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003c00000041500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000435fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001af000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000001b1000001230000011200fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000fb0000000a0049006d00610067006501000001f0000002800000002800ffffff000000010000010f00000415fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000415000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000006100000043500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 960 - X: 960 - Y: 32 + Width: 1920 + X: 0 + Y: 240 diff --git a/tasks/HRI/config/lab.yaml b/tasks/HRI/config/lab.yaml index a9a1aa1b5..5236db1b1 100644 --- a/tasks/HRI/config/lab.yaml +++ b/tasks/HRI/config/lab.yaml @@ -1,65 +1,69 @@ hri: # the `hri` Node's Parameters ros__parameters: + # Start location after door start_pose: position: - x: 6.945671558380127 - y: 2.1550674438476562 + x: 2.620011965794007 + y: 0.4284228083916832 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.8876724994180473 - w: 0.46047533460210094 + z: -0.9676446468413096 + w: 0.25231693847095826 + # Where to wait for guests door_pose: position: - x: 7.2768144607543945 - y: -2.1811742782592773 + x: 0.9737232865224763 + y: 0.6644210227864706 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.5462976677684211 - w: 0.8375911044123998 + z: 0.9883189417995438 + w: 0.15239970236266812 door_polygon: - top_left: [2.565998077392578, -0.5245494842529297] - top_right: [2.1353719234466553, -1.3906874656677246] - bottom_right: [0.9952375888824463, -0.7686729431152344] - bottom_left: [1.384078025817871, 0.08977162837982178] + top_left: [-0.2931315004825592, 0.8144665956497192] + top_right: [-0.08571681380271912, 1.1960442066192627] + bottom_right: [0.38185712695121765, 1.0947651863098145] + bottom_left: [0.185118168592453, 0.6390931606292725] # Where to position self for seating guests seat_pose: position: - x: 0.7253542411729228 - y: -0.41696111136241965 + x: 0.9241805428867255 + y: -0.37066992922907555 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.7903108029987692 - w: 0.6127061568675809 + z: -0.798049181701085 + w: 0.6025923195546294 + + # Where the robot looks at the general sofa sofa_point: - x: 2.175074577331543 - y: -0.014040738344192505 + x: 0.31079837679862976 + y: -2.7174582481384277 z: 0.5 # From robot POV: [top left, top right,bottom right, bottom left ] # General area to perform detections in seat_area: - top_left: [1.8483556509017944, 0.7650331854820251] - top_right: [2.1753768920898438, 0.05751645565032959] - bottom_right: [2.8799538612365723, 0.3364943861961365] - bottom_left: [2.374444007873535, 1.0547375679016113] + top_left: [0.7725991010665894, -3.1269145011901855] + top_right: [-0.8415073156356812, -2.6533150672912598] + bottom_right: [-0.5009860992431641, -1.2560838460922241] + bottom_left: [1.1633232831954956, -1.7793333530426025] # Max number of seats max_people_on_sofa: 2 # Seatable area sofa_area: - top_left: [2.63527774810791, 0.5103752613067627] - top_right: [2.6295571327209473, -0.6804611682891846] - bottom_right: [1.0060914039611816, -0.661674976348877] - bottom_left: [1.0884835243225098,0.6545710563659668] + top_left: [0.8283745646476746, -3.395517587661743] + top_right: [-0.34002554416656494, -3.0816760063171387] + bottom_right: [-0.20183980464935303, -2.4824330806732178] + bottom_left: [1.0337331295013428, -2.806881904602051] diff --git a/tasks/HRI/config/place_bag_debug.rviz b/tasks/HRI/config/place_bag_debug.rviz new file mode 100644 index 000000000..59b3ddc1a --- /dev/null +++ b/tasks/HRI/config/place_bag_debug.rviz @@ -0,0 +1,799 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Image1/Topic1 + - /Marker1 + - /MarkerArray1 + - /PointCloud21 + - /PointCloud21/Topic1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 466 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_7_link: + Alpha: 1 + Show Axes: false + Show Trail: false + arm_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_antenna_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_antenna_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_cover_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_dock_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_mic_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mic_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mic_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_mic_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_sonar_01_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_sonar_02_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_sonar_03_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_left_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_left_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_right_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_right_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_left_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_left_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_right_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_right_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + gripper_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_front_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_front_camera_orbbec_aux_joint_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + suspension_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + suspension_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_fixed_column_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_fixed_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_ft_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_ft_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: scan_raw + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /head_front_camera/rgb/image_raw + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /place_bag/debug/drop_point + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + person_0: true + person_0_lines: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /yolo/pose3d/yolo11n_pose_pt + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /head_front_camera/depth/rgb/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10.375131607055664 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.004258632659912109 + Y: -1.1227388381958008 + Z: 0.003994941711425781 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.553482174873352 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 1.0610255002975464 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1131 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000415fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000020d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001230000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000fb0000000a0049006d0061006700650100000377000000d90000002800ffffff000000010000010f00000415fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000415000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002500000041500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 960 + X: 960 + Y: 32 diff --git a/tasks/HRI/launch/HRI.launch.py b/tasks/HRI/launch/HRI.launch.py index add28aea0..55640f27d 100644 --- a/tasks/HRI/launch/HRI.launch.py +++ b/tasks/HRI/launch/HRI.launch.py @@ -27,10 +27,10 @@ def generate_launch_description(): ) ) - vision_clip = Node( - package="lasr_vision_clip", - executable="vqa", - name="lasr_vision_clip_service", + vlm = Node( + package="lasr_vlm", + executable="vlm_service", + name="lasr_vlm_service", output="screen", ) @@ -40,12 +40,9 @@ def generate_launch_description(): name="lasr_vision_reid", output="screen", ) - + llm_service = Node( - package='lasr_llm', - executable='hri_task_service', - name='llm', - output='screen' + package="lasr_llm", executable="hri_task_service", name="llm", output="screen" ) eye_tracker = Node( @@ -56,11 +53,10 @@ def generate_launch_description(): ) transcribe_speech = Node( - package='lasr_speech_recognition_whisper', - executable='transcribe_microphone_server', - name='whisper_mic_server', - output='screen', - + package="lasr_speech_recognition_whisper", + executable="transcribe_microphone_server", + name="whisper_mic_server", + output="screen", ) state_machine = Node( @@ -78,10 +74,10 @@ def generate_launch_description(): load_motions, yolo_service, reid_service, - vision_clip, + vlm, eye_tracker, - state_machine, transcribe_speech, llm_service, + state_machine, ] ) diff --git a/tasks/HRI/setup.py b/tasks/HRI/setup.py index cc12e953e..74b688b48 100644 --- a/tasks/HRI/setup.py +++ b/tasks/HRI/setup.py @@ -51,6 +51,8 @@ def run(self): "seat_guest = HRI.states.seat_guest:main", "sm = HRI.state_machine:main", "start_sm = HRI.states.start_door_sm:main", + "recognise = HRI.states.recognise:main", + "place_bag = HRI.states.place_bag:main", ], }, ) diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index c7acf2288..f0e2c75f7 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -1,17 +1,186 @@ -/**: +pick_and_place: ros__parameters: pick_and_place: + + # Category the referee designates as trash. + trash_category: "fruit" + + # DINING TABLE table: + # Where the robot stands to pick objects from the table yes pose: - position: {x: 4.65, y: 2.5, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.2919} - look_point: [5.25, 2.27, 0.75] - polygon: [3.5, 1.0, 5.5, 1.0, 5.5, 3.5, 3.5, 3.5] - search_polygon: [5.3, 3.8, 7.8, 3.8, 7.8, 5.9, 5.3, 5.9] + position: {x: -8.146629333496094, y: 21.995290756225586, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.598939, w: 0.800795} + + # Detection polygon — rectangle around the table top in map frame yes + polygon: + top_left: [-8.301767349243164, 23.88897705078125] + top_right: [-7.104084014892578, 23.345989227294922] + bottom_right: [-7.559137344360352, 22.08161163330078] + bottom_left: [-8.659490585327148, 22.590110778808594] + + # Z height range for detection filtering z_min: 0.7 z_max: 1.5 + + # MoveIt collision box for the table + collision: + detect: true + frame_id: map + size: [1.02, 1.2, 0.9] + position: [1.2, -3.9, 0.37] + + # DESTINATION 1: dishwasher yes + dishwasher: + pose: + position: {x: -5.7190327644348145, y: 23.41750144958496, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.611978, w: 0.790875} + + # DESTINATION 2: trash bin yes + trash_bin: + pose: + position: {x: -6.85477, y: 22.4255, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.599564, w: 0.800327} + + # DESTINATION 3: cabinet yes cabinet: pose: - position: {x: 0.0, y: 0.0, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} - objects: ["cup", "can", "bottle", "bowl", "box"] \ No newline at end of file + position: {x: -9.75769329071045, y: 22.429922103881836, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.792694, w: 0.60962} + + # Detection polygon for cereal/milk detection inside cabinet (from real) + polygon: + top_left: [-9.97590446472168, 21.317359924316406] + top_right: [-10.988214492797852, 21.317359924316406] + bottom_right: [-10.830141067504883, 21.885536193847656] + bottom_left: [-9.953857421875, 21.592342376708984] + + # Shelves — used by ScanShelves also from real + shelf_order: ["top", "middle", "bottom", "extra_bottom"] + shelves: + top: + torso_lift_joint: 0.30 + look_point: [-10.44567584991455, 21.501087188720703, 1.1333880424499519] + polygon: + top_left: [-9.97590446472168, 21.317359924316406] + top_right: [-10.988214492797852, 21.317359924316406] # fix this + bottom_right: [-10.830141067504883, 21.885536193847656] + bottom_left: [-9.953857421875, 21.592342376708984] + z_min: 1.05 + z_max: 1.40 + + middle: + torso_lift_joint: 0.15 + look_point: [-10.44567584991455, 21.501087188720703, 0.72] + polygon: + top_left: [-9.97590446472168, 21.317359924316406] + top_right: [-10.988214492797852, 21.317359924316406] # fix this + bottom_right: [-10.830141067504883, 21.885536193847656] + bottom_left: [-9.953857421875, 21.592342376708984] + z_min: 0.60 + z_max: 0.95 + + bottom: + torso_lift_joint: 0.0 + look_point: [-10.44567584991455, 21.501087188720703, 0.38] + polygon: + top_left: [-9.97590446472168, 21.317359924316406] + top_right: [-10.988214492797852, 21.317359924316406] # fix this + bottom_right: [-10.830141067504883, 21.885536193847656] + bottom_left: [-9.953857421875, 21.592342376708984] + z_min: 0.25 + z_max: 0.60 + + extra_bottom: + torso_lift_joint: 0.0 + look_point: [-10.44567584991455, 21.501087188720703, 0.12] + polygon: + top_left: [-9.97590446472168, 21.317359924316406] + top_right: [-10.988214492797852, 21.317359924316406] # fix this + bottom_right: [-10.830141067504883, 21.885536193847656] + bottom_left: [-9.953857421875, 21.592342376708984] + z_min: 0.0 + z_max: 0.25 + + # BREAKFAST SURFACE (bowl and spoon pickup location) + breakfast_surface: + pose: + position: {x: -6.90986967086792, y: 21.280567169189453, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.125551, w: 0.992087} + # update with real coordinates + polygon: + top_left: [-5.61154317855835, 21.451658248901367] + top_right: [-5.85907506942749, 20.46249008178711] + bottom_right: [-6.411557197570801, 20.629091262817383] + bottom_left: [-6.269176006317139, 21.64197540283203] + + # EXTRA SURFACE (two common objects for cleanup) + extra_surface: + pose: + position: {x: -6.90986967086792, y: 21.280567169189453, z: 0.0} # update with real pose + orientation: {x: 0.0, y: 0.0, z: -0.125551, w: 0.992087} + # update with real coordinates + polygon: + top_left: [-5.61154317855835, 21.451658248901367] + top_right: [-5.85907506942749, 20.46249008178711] + bottom_right: [-6.411557197570801, 20.629091262817383] + bottom_left: [-6.269176006317139, 21.64197540283203] + + # GRASP (not used without manipulation) + grasp: + enable: false + reach: 0.80 + use_moveit: false + publish_box: false + + objects: + cup: + category: "dish" + coke: + category: "drink" + bowl: + category: "dish" + apple: + category: "fruit" + fork: + category: "dish" + knife: + category: "dish" + spoon: + category: "dish" + plate: + category: "dish" + cornflakes: + category: "food" + instant_noodles: + category: "food" + milk: + category: "drink" + pepsi: + category: "drink" + rubiks_cube: + category: "toys" + dishwasher_tab: + category: "cleaning_supplies" + toothpaste: + category: "cleaning_supplies" + sponge: + category: "sponge" + red_bull: + category: "drink" + soju: + category: "drink" + peach: + category: "fruit" + red_bellpepper: + category: "fruit" + yellow_bellpepper: + category: "fruit" + lemon: + category: "fruit" + mangostane: + category: "fruit" + seaweed: + category: "snack" + pringles: + category: "snack" \ No newline at end of file diff --git a/tasks/pick_and_place/config/sim.yaml b/tasks/pick_and_place/config/sim.yaml new file mode 100644 index 000000000..1583bd217 --- /dev/null +++ b/tasks/pick_and_place/config/sim.yaml @@ -0,0 +1,185 @@ +pick_and_place: + ros__parameters: + pick_and_place: + + # Category the referee designates as trash. + trash_category: "fruit" + + # DINING TABLE + table: + # Where the robot stands to pick objects from the table yes + pose: + position: {x: 2.0604363441467285, y: -3.2225301551818848, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.951549, w: 0.307496} + + # Detection polygon — rectangle around the table top in map frame yes + polygon: + top_left: [1.6, -2.8] + top_right: [2.5, -2.8] + bottom_right: [2.5, -3.7] + bottom_left: [1.6, -3.7] + # Z height range for detection filtering + z_min: 0.7 + z_max: 1.5 + + # MoveIt collision box for the table + collision: + detect: true + frame_id: map + size: [1.02, 1.2, 0.9] + position: [1.2, -3.9, 0.37] + + # DESTINATION 1: dishwasher yes + dishwasher: + pose: + position: {x: 5.870972633361816, y: -3.6086063385009766, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.46432, w: 0.885668} + + # DESTINATION 2: trash bin yes + trash_bin: + pose: + position: {x: 1.288726806640625, y: 1.04266834259033203, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.46432, w: 0.885668} + + # DESTINATION 3: cabinet yes + cabinet: + pose: + position: {x: -4.844698905944824, y: 1.6842019081115723, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.951549, w: 0.307496} + + # Detection polygon for cereal/milk detection inside cabinet (from real) + polygon: + top_left: [-6.120231628417969, 1.5627683401107788] + top_right: [-6.419377326965332, 2.451091766357422] + bottom_right: [-6.020194053649902, 2.8703858852386475] + bottom_left: [-5.440605163574219, 1.8351893424987793] + + # Shelves — used by ScanShelves also from real + shelf_order: ["top", "middle", "bottom", "extra_bottom"] + shelves: + top: + torso_lift_joint: 0.30 + look_point: [-10.44567584991455, 21.501087188720703, 1.1333880424499519] + polygon: + top_left: [-6.120231628417969, 1.5627683401107788] + top_right: [-6.419377326965332, 2.451091766357422] + bottom_right: [-6.020194053649902, 2.8703858852386475] + bottom_left: [-5.440605163574219, 1.8351893424987793] + z_min: 1.05 + z_max: 1.40 + + middle: + torso_lift_joint: 0.15 + look_point: [-10.44567584991455, 21.501087188720703, 0.72] + polygon: + top_left: [-6.120231628417969, 1.5627683401107788] + top_right: [-6.419377326965332, 2.451091766357422] + bottom_right: [-6.020194053649902, 2.8703858852386475] + bottom_left: [-5.440605163574219, 1.8351893424987793] + z_min: 0.60 + z_max: 0.95 + + bottom: + torso_lift_joint: 0.0 + look_point: [-10.44567584991455, 21.501087188720703, 0.38] + polygon: + top_left: [-6.120231628417969, 1.5627683401107788] + top_right: [-6.419377326965332, 2.451091766357422] + bottom_right: [-6.020194053649902, 2.8703858852386475] + bottom_left: [-5.440605163574219, 1.8351893424987793] + z_min: 0.25 + z_max: 0.60 + + extra_bottom: + torso_lift_joint: 0.0 + look_point: [-10.44567584991455, 21.501087188720703, 0.12] + polygon: + top_left: [-6.120231628417969, 1.5627683401107788] + top_right: [-6.419377326965332, 2.451091766357422] + bottom_right: [-6.020194053649902, 2.8703858852386475] + bottom_left: [-5.440605163574219, 1.8351893424987793] + z_min: 0.0 + z_max: 0.25 + + # BREAKFAST SURFACE (bowl and spoon pickup location) + breakfast_surface: + pose: + position: {x: 3.5734567642211914, y: 0.6986422538757324, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.840512, w: 0.541793} + # update with real coordinates + polygon: + top_left: [1.103219985961914, 2.7031068801879883] + top_right: [2.8933119773864746, 3.8241472244262695] + bottom_right: [4.074957847595215, 1.7860803604125977] + bottom_left: [2.7978620529174805, 0.6587891578674316] + + # EXTRA SURFACE (two common objects for cleanup) + extra_surface: + pose: + position: {x: 3.5734567642211914, y: 0.6986422538757324, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.840512, w: 0.541793} + # update with real coordinates + polygon: + top_left: [4.103219985961914, 1.7031068801879883] + top_right: [5.8933119773864746, 0.8241472244262695] + bottom_right: [4.074957847595215, 1.7860803604125977] + bottom_left: [3.7978620529174805, 0.6587891578674316] + + # GRASP (not used without manipulation) + grasp: + enable: false + reach: 0.80 + use_moveit: false + publish_box: false + + objects: + cup: + category: "dish" + coke: + category: "drink" + bowl: + category: "dish" + apple: + category: "fruit" + fork: + category: "dish" + knife: + category: "dish" + spoon: + category: "dish" + plate: + category: "dish" + cornflakes: + category: "food" + instant_noodles: + category: "food" + milk: + category: "drink" + pepsi: + category: "drink" + rubiks_cube: + category: "toys" + dishwasher_tab: + category: "cleaning_supplies" + toothpaste: + category: "cleaning_supplies" + sponge: + category: "sponge" + red_bull: + category: "drink" + soju: + category: "drink" + peach: + category: "fruit" + red_bellpepper: + category: "fruit" + yellow_bellpepper: + category: "fruit" + lemon: + category: "fruit" + mangostane: + category: "fruit" + seaweed: + category: "snack" + pringles: + category: "snack" \ No newline at end of file diff --git a/tasks/pick_and_place/launch/pick_and_place.launch.py b/tasks/pick_and_place/launch/pick_and_place.launch.py index aa2a6ecd3..b135d7333 100644 --- a/tasks/pick_and_place/launch/pick_and_place.launch.py +++ b/tasks/pick_and_place/launch/pick_and_place.launch.py @@ -1,14 +1,47 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - config = os.path.join( - get_package_share_directory("pick_and_place"), "config", "config.yaml" - ) + """ + Launch file for the Pick and Place task using YOLO detection. + + Launch SEPARATELY before running this: + - Simulator / robot bringup + - Nav2 + localisation + + Start the task after everything is ready: + ros2 topic pub --once /pick_and_place/start std_msgs/msg/Empty {} + """ + pkg_pp = get_package_share_directory("pick_and_place") + config = os.path.join(pkg_pp, "config", "config.yaml") + + use_sim = LaunchConfiguration("use_sim") + return LaunchDescription([ + DeclareLaunchArgument( + "use_sim", + default_value="true", + description="Set to false when running on the real robot " + "to disable the point_head_stub.", + ), + + # ── Perception: YOLO detection ──────────────────────────────────────── + Node( + package="lasr_vision_yolo", + executable="yolo_service_node", + name="lasr_vision_yolo", + output="screen", + parameters=[{ + "preload": ["/path/to/lasr_vision_yolo/models/best.pt"]}], + ), + + # ── Task: state machine ─────────────────────────────────────────────── Node( package="pick_and_place", executable="state_machine", @@ -16,7 +49,11 @@ def generate_launch_description(): output="screen", parameters=[config], ), + + # ── Head stub (simulation only) ─────────────────────────────────────── + # Remove this when testing on the real robot by passing use_sim:=false Node( + condition=IfCondition(False), package="pick_and_place", executable="point_head_stub", name="point_head_stub", diff --git a/tasks/pick_and_place/launch/serve_breakfast.launch.py b/tasks/pick_and_place/launch/serve_breakfast.launch.py new file mode 100644 index 000000000..cf9085e4c --- /dev/null +++ b/tasks/pick_and_place/launch/serve_breakfast.launch.py @@ -0,0 +1,27 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory("pick_and_place"), "config", "config.yaml" + ) + return LaunchDescription( + [ + Node( + package="pick_and_place", + executable="test_serve_breakfast", + name="pick_and_place", + output="screen", + parameters=[config], + ), + Node( + package="pick_and_place", + executable="point_head_stub", + name="point_head_stub", + output="screen", + ), + ] + ) diff --git a/tasks/pick_and_place/pick_and_place/detect_tuner.py b/tasks/pick_and_place/pick_and_place/detect_tuner.py new file mode 100644 index 000000000..47c2af876 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/detect_tuner.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python3 +"""Interactive open-vocab DETECTION TUNER. Park in front of the table, run, watch +RViz Image /detect_tuner/image + the per-detection table. Tune live with ros2 param set.""" +import math +import time + +import numpy as np +import cv2 + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from rclpy.duration import Duration as RDuration +from rclpy.time import Time as RTime + +from sensor_msgs.msg import Image, CameraInfo +from geometry_msgs.msg import PointStamped +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration + +import tf2_ros +from tf2_geometry_msgs import do_transform_point +from cv_bridge import CvBridge + +from lasr_vision_interfaces.srv import OpenVocabDetect + + +class DetectTuner(Node): + def __init__(self): + super().__init__("detect_tuner") + self.declare_parameter("rgb_topic", "/head_front_camera/rgb/image_raw") + self.declare_parameter("depth_topic", "/head_front_camera/depth/image_raw") + self.declare_parameter("info_topic", "/head_front_camera/rgb/camera_info") + self.declare_parameter("queries", ["cup", "can", "bottle", "box", "apple"]) + self.declare_parameter("box_threshold", 0.25) + self.declare_parameter("text_threshold", 0.20) + self.declare_parameter("period", 3.0) + self.declare_parameter("map_frame", "map") + self.declare_parameter("head_tilt", -0.6) # rad; 99.0 to skip + self.declare_parameter("tilt_once", True) + + self.bridge = CvBridge() + self._rgb = None + self._depth = None + self._info = None + + qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST) + rgb_t = self.get_parameter("rgb_topic").value + depth_t = self.get_parameter("depth_topic").value + info_t = self.get_parameter("info_topic").value + self.create_subscription(Image, rgb_t, self._rgb_cb, qos) + self.create_subscription(Image, depth_t, self._depth_cb, qos) + self.create_subscription(CameraInfo, info_t, self._info_cb, qos) + + self._pub = self.create_publisher(Image, "/detect_tuner/image", 10) + self._ovd = self.create_client(OpenVocabDetect, "open_vocab/detect") + self._tf = tf2_ros.Buffer() + self._tfl = tf2_ros.TransformListener(self._tf, self) + self._head = ActionClient( + self, FollowJointTrajectory, "/head_controller/follow_joint_trajectory") + self.get_logger().info( + f"detect_tuner up. RGB={rgb_t}\n" + f" -> RViz: add Image display on /detect_tuner/image\n" + f" -> tune live: ros2 param set /detect_tuner box_threshold 0.35") + + def _rgb_cb(self, m): + self._rgb = m + + def _depth_cb(self, m): + self._depth = m + + def _info_cb(self, m): + self._info = m + + def _spin(self, secs): + end = time.time() + secs + while time.time() < end and rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.05) + + def _wait_future(self, fut, secs): + end = time.time() + secs + while not fut.done() and time.time() < end and rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.05) + return fut.result() if fut.done() else None + + def tilt_head(self, tilt): + if not self._head.wait_for_server(timeout_sec=5.0): + self.get_logger().warn("head controller not available — skipping tilt") + return + g = FollowJointTrajectory.Goal() + t = JointTrajectory() + t.joint_names = ["head_1_joint", "head_2_joint"] + pt = JointTrajectoryPoint() + pt.positions = [0.0, float(tilt)] + pt.time_from_start = Duration(sec=2) + t.points = [pt] + g.trajectory = t + self._head.send_goal_async(g) + self.get_logger().info(f"tilting head to {tilt:.2f} rad") + self._spin(3.0) + + def _project(self, cx, cy): + if self._depth is None or self._info is None or self._rgb is None: + return None, None + enc = self._depth.encoding + depth = self.bridge.imgmsg_to_cv2(self._depth, enc) + scale = 0.001 if enc in ("16UC1", "mono16") else 1.0 + h, w = depth.shape[:2] + px = int(np.clip(cx, 0, w - 1)) + py = int(np.clip(cy, 0, h - 1)) + d = float(depth[py, px]) * scale + if d <= 0.0 or math.isnan(d) or d > 8.0: + return d, None + K = self._info.k + fx, fy, cxp, cyp = K[0], K[4], K[2], K[5] + cam = self._rgb.header.frame_id + ps = PointStamped() + ps.header.frame_id = cam + ps.header.stamp = self._rgb.header.stamp + ps.point.x = (px - cxp) * d / fx + ps.point.y = (py - cyp) * d / fy + ps.point.z = d + map_frame = self.get_parameter("map_frame").value + try: + tr = self._tf.lookup_transform(map_frame, cam, RTime(), + timeout=RDuration(seconds=0.5)) + mp = do_transform_point(ps, tr).point + return d, (mp.x, mp.y, mp.z) + except Exception: + return d, None + + @staticmethod + def _color(conf): + if conf >= 0.45: + return (0, 255, 0) + if conf >= 0.30: + return (0, 255, 255) + return (0, 0, 255) + + def detect_once(self): + if self._rgb is None or self._info is None: + self.get_logger().warn("waiting for camera image/info…") + return + if not self._ovd.wait_for_service(timeout_sec=2.0): + self.get_logger().error("open_vocab/detect not available — is the node up?") + return + queries = list(self.get_parameter("queries").value) + box_thr = float(self.get_parameter("box_threshold").value) + text_thr = float(self.get_parameter("text_threshold").value) + req = OpenVocabDetect.Request() + req.image = self._rgb + req.queries = queries + req.box_threshold = box_thr + req.text_threshold = text_thr + resp = self._wait_future(self._ovd.call_async(req), 120.0) + if resp is None: + self.get_logger().error("detect timed out (CPU slow? disable clip_rerank)") + return + dets = [] + for d in resp.detections: + if len(d.xywh) < 4: + continue + cx, cy, w, h = (float(d.xywh[0]), float(d.xywh[1]), + float(d.xywh[2]), float(d.xywh[3])) + depth, mapxyz = self._project(cx, cy) + dets.append((d.name, float(d.confidence), (cx, cy, w, h), depth, mapxyz)) + dets.sort(key=lambda x: x[1], reverse=True) + self.get_logger().info( + f"\n=== {len(dets)} detection(s) queries={queries} " + f"box_thr={box_thr} text_thr={text_thr} ===") + for i, (name, conf, (cx, cy, w, h), depth, mapxyz) in enumerate(dets): + dstr = f"{depth:.2f}m" if depth is not None else "n/a" + mstr = (f"map=({mapxyz[0]:.2f},{mapxyz[1]:.2f},{mapxyz[2]:.2f})" + if mapxyz is not None else "map=n/a") + self.get_logger().info( + f" [{i}] {name:<14} conf={conf:.2f} " + f"cxywh=({cx:.0f},{cy:.0f},{w:.0f},{h:.0f}) depth={dstr} {mstr}") + try: + img = self.bridge.imgmsg_to_cv2(self._rgb, "bgr8") + for name, conf, (cx, cy, w, h), depth, _ in dets: + x1, y1 = int(cx - w / 2), int(cy - h / 2) + x2, y2 = int(cx + w / 2), int(cy + h / 2) + col = self._color(conf) + cv2.rectangle(img, (x1, y1), (x2, y2), col, 2) + dstr = f" {depth:.2f}m" if depth is not None else "" + cv2.putText(img, f"{name} {conf:.2f}{dstr}", (x1, max(0, y1 - 6)), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, col, 2) + out = self.bridge.cv2_to_imgmsg(img, "bgr8") + out.header = self._rgb.header + self._pub.publish(out) + except Exception as e: + self.get_logger().warn(f"annotate/publish failed: {e}") + + def run(self): + if (self.get_parameter("tilt_once").value + and abs(float(self.get_parameter("head_tilt").value)) < 1.6): + self._spin(1.0) + self.tilt_head(self.get_parameter("head_tilt").value) + while rclpy.ok(): + self.detect_once() + self._spin(max(0.5, float(self.get_parameter("period").value))) + + +def main(): + rclpy.init() + node = DetectTuner() + try: + node.run() + except KeyboardInterrupt: + pass + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/shelf.py b/tasks/pick_and_place/pick_and_place/shelf.py new file mode 100644 index 000000000..99552661e --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/shelf.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +import rclpy +import yasmin +import yasmin_ros +from pick_and_place.states.scan_shelves import ScanShelves +from pick_and_place.states.classify_category import ClassifyCategory +from pick_and_place.states.choose_shelf import ChooseShelf +from pick_and_place.states.instruct_place import InstructPlace + +def main(): + rclpy.init() + node = rclpy.create_node( + "pick_and_place", # ← match the config node name + allow_undeclared_parameters=True, + automatically_declare_parameters_from_overrides=True, + ) + yasmin_ros.set_ros_loggers(node) + + bb = yasmin.Blackboard() + + + + # ── Object to test placing ──────────────────────────────────────── + bb["object_name"] = "coke" # change to test different items + bb["selected_object_name"] = "coke" + bb["object_category"] = "" + bb["chosen_shelf"] = "" + bb["chosen_shelf_str"] = "" + bb["destination"] = "cabinet" + bb["destination_str"] = "the cabinet" + bb["shelf_data"] = {} + bb["shelf_category"] = "" + bb["object_names"] = [] + bb["detected_objects"] = [] + bb["debug_images"] = [] + # ───────────────────────────────────────────────────────────────── + + # Step 1 — Scan shelves (robot must be in front of cabinet) + print("Scanning shelves...") + scanner = ScanShelves() + outcome = scanner.execute(bb) + print(f"ScanShelves outcome: {outcome}") + print(f"shelf_data: {bb['shelf_data']}") + + if outcome == "failed": + print("Scan failed — check YOLO service and robot position.") + rclpy.shutdown() + return + + # Step 2 — Classify + classifier = ClassifyCategory(task="object") + outcome = classifier.execute(bb) + print(f"ClassifyCategory outcome: {outcome}") + print(f"object_category: {bb['object_category']}") + + # Step 3 — Choose shelf + chooser = ChooseShelf() + outcome = chooser.execute(bb) + print(f"ChooseShelf outcome: {outcome}") + print(f"chosen_shelf: {bb['chosen_shelf']}") + print(f"chosen_shelf_str: {bb['chosen_shelf_str']}") + + # Step 4 — Instruct place + placer = InstructPlace() + outcome = placer.execute(bb) + print(f"InstructPlace outcome: {outcome}") + + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/state_machine.py b/tasks/pick_and_place/pick_and_place/state_machine.py index edff0a654..788216017 100644 --- a/tasks/pick_and_place/pick_and_place/state_machine.py +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -2,175 +2,140 @@ import rclpy from rclpy.node import Node - +from std_msgs.msg import Empty +import signal import yasmin import yasmin_ros from yasmin_viewer import YasminViewerPub -from lasr_skills import Say, GoToLocation +from lasr_skills import Say from pick_and_place.states import ( Start, - ScanShelves, - FindAndGoToTable, - DetectObjects, - SelectAndVisualiseObject, - ClassifyCategory, - ChooseShelf, - InstructPick, - InstructPlace, + TableCleanup, + ServeBreakfast, + ExtraSurfaceCleanup, ) from rclpy.executors import MultiThreadedExecutor as Executor + class PickAndPlace(yasmin.StateMachine): """ - Main state machine for the Pick and Place task. + Top-level state machine for the Pick and Place task. - Physical manipulation is delegated to a human operator - via verbal instructions — the robot perceives, reasons, and speaks. + Orchestrates three independent sub-machines in the rulebook's + suggested order: clean the dining table, serve breakfast, then + clean the extra surface. Each sub-machine is self-contained and + can be tested in isolation. Flow: - START - → SCAN_SHELVES (build shelf category map) - → FIND_AND_GO_TO_TABLE (locate and navigate to table) - → DETECT_OBJECTS (detect all objects on table) - → SELECT_OBJECT (pick first object, visualise for referee) - → CLASSIFY_CATEGORY (determine object category) - → CHOOSE_SHELF (match object to correct shelf) - → INSTRUCT_PICK (tell operator to pick up object) - → GO_TO_CABINET (navigate to cabinet) - → INSTRUCT_PLACE (tell operator which shelf to place on) - → GO_TO_TABLE (navigate back to table) - → DETECT_OBJECTS (re-scan, loop until table empty) - → succeeded + WAIT_START -> SAY_START -> START (door, drive to table) + -> SAY_STARTING_CLEANUP -> TABLE_CLEANUP (TableCleanup) + -> SAY_STARTING_BREAKFAST -> SERVE_BREAKFAST (ServeBreakfast) + -> SAY_STARTING_EXTRA_SURFACE -> EXTRA_SURFACE_CLEANUP + -> SAY_TASK_COMPLETE + -> succeeded """ def __init__(self): super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) - # ── Entry ───────────────────────────────────────────────────────────── + # ── Entry: door wait, drive to table ────────────────────────────────────── self.add_state( "START", Start(), transitions={ - "succeeded": "SCAN_SHELVES", - "failed": "failed", + "succeeded": "SAY_STARTING_CLEANUP", + "failed": "failed", }, ) - # ── Scan cabinet shelves (done once) ────────────────────────────────── + # Phase 1: table cleanup self.add_state( - "SCAN_SHELVES", - ScanShelves(), + "SAY_STARTING_CLEANUP", + Say(text="I will now clean up the dining table."), transitions={ - "succeeded": "DETECT_OBJECTS", - "failed": "failed", + "succeeded": "TABLE_CLEANUP", + "aborted": "TABLE_CLEANUP", + "canceled": "TABLE_CLEANUP", }, ) - # ── Find and navigate to table ──────────────────────────────────────── self.add_state( - "FIND_AND_GO_TO_TABLE", - FindAndGoToTable(), + "TABLE_CLEANUP", + TableCleanup(), transitions={ - "succeeded": "DETECT_OBJECTS", - "failed": "DETECT_OBJECTS", # proceed even if table not found + "succeeded": "SAY_STARTING_BREAKFAST", + "failed": "SAY_STARTING_BREAKFAST", # continue regardless }, ) - # ── Detect all objects on table ─────────────────────────────────────── - # Re-entered at the top of every loop iteration + # Phase 2: serve breakfast self.add_state( - "DETECT_OBJECTS", - DetectObjects(), + "SAY_STARTING_BREAKFAST", + Say(text="I will now set up breakfast."), transitions={ - "succeeded": "SELECT_OBJECT", - "failed": "DETECT_OBJECTS", # retry until objects found + "succeeded": "SERVE_BREAKFAST", + "aborted": "SERVE_BREAKFAST", + "canceled": "SERVE_BREAKFAST", }, ) - # ── Select object and visualise for referee ─────────────────────────── self.add_state( - "SELECT_OBJECT", - SelectAndVisualiseObject(), + "SERVE_BREAKFAST", + ServeBreakfast(), transitions={ - "succeeded": "CLASSIFY_CATEGORY", - "failed": "DETECT_OBJECTS", # re-scan if nothing to select + "succeeded": "SAY_STARTING_EXTRA_SURFACE", + "failed": "SAY_STARTING_EXTRA_SURFACE", }, ) - # ── Classify selected object into a category ────────────────────────── + # Phase 3: extra surface cleanup self.add_state( - "CLASSIFY_CATEGORY", - ClassifyCategory(task="object"), + "SAY_STARTING_EXTRA_SURFACE", + Say(text="I will now check the extra surface."), transitions={ - "succeeded": "CHOOSE_SHELF", - "failed": "CHOOSE_SHELF", # proceed with unknown category - "empty": "DETECT_OBJECTS", # nothing to classify, re-scan + "succeeded": "EXTRA_SURFACE_CLEANUP", + "aborted": "EXTRA_SURFACE_CLEANUP", + "canceled": "EXTRA_SURFACE_CLEANUP", }, ) - # ── Choose which shelf to place object on ───────────────────────────── self.add_state( - "CHOOSE_SHELF", - ChooseShelf(), + "EXTRA_SURFACE_CLEANUP", + ExtraSurfaceCleanup(), transitions={ - "succeeded": "INSTRUCT_PICK", - "failed": "DETECT_OBJECTS", + "succeeded": "SAY_TASK_COMPLETE", + "failed": "SAY_TASK_COMPLETE", }, ) - # ── Instruct operator to pick up object ─────────────────────────────── + # Done self.add_state( - "INSTRUCT_PICK", - InstructPick(), + "SAY_TASK_COMPLETE", + Say( + text="I have completed the pick and place task. " + "The table is clean and breakfast is ready." + ), transitions={ - "succeeded": "GO_TO_CABINET", - "failed": "INSTRUCT_PICK", # retry instruction + "succeeded": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", }, ) - # ── Navigate to cabinet ─────────────────────────────────────────────── - self.add_state( - "GO_TO_CABINET", - GoToLocation(location_param="pick_and_place.cabinet.pose"), - transitions={ - "succeeded": "INSTRUCT_PLACE", - "failed": "GO_TO_CABINET", # retry navigation - }, - ) - - # ── Instruct operator where to place object ─────────────────────────── - self.add_state( - "INSTRUCT_PLACE", - InstructPlace(), - transitions={ - "succeeded": "GO_TO_TABLE", - "failed": "INSTRUCT_PLACE", # retry instruction - }, - ) - - # ── Navigate back to table for next object ──────────────────────────── - self.add_state( - "GO_TO_TABLE", - GoToLocation(location_param="pick_and_place.table.pose"), - transitions={ - "succeeded": "DETECT_OBJECTS", # loop back for next object - "failed": "GO_TO_TABLE", # retry navigation - }, - ) class PickAndPlaceNode(Node): def __init__(self): super().__init__( node_name="pick_and_place", allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=True, # ← ПОВЕРНУТИ + automatically_declare_parameters_from_overrides=True, ) self._executor = Executor() self._executor.add_node(self) - self._spin_thread = Thread(target=self._executor.spin) + self._spin_thread = Thread(target=self._executor.spin, daemon=True) self._spin_thread.start() @@ -188,27 +153,45 @@ def main(): bb = yasmin.Blackboard() # Initialise all blackboard keys used across the machine - bb["detected_objects"] = [] - bb["selected_object"] = None + bb["detected_objects"] = [] + bb["selected_object"] = None bb["selected_object_name"] = "" - bb["object_name"] = "" - bb["object_category"] = "" - bb["shelf_data"] = {} - bb["chosen_shelf"] = "" - bb["chosen_shelf_str"] = "" - bb["table_pose"] = None - bb["debug_images"] = [] + bb["object_name"] = "" + bb["object_category"] = "" + bb["shelf_data"] = {} + bb["chosen_shelf"] = "" + bb["chosen_shelf_str"] = "" + bb["destination"] = "" + bb["destination_str"] = "" + bb["location"] = None + bb["table_pose"] = None + bb["debug_images"] = [] + bb["last_rgb_image"] = None + bb["dishwasher_opened"] = False + + def shutdown(sig=None, frame=None): + yasmin.YASMIN_LOG_INFO("Shutting down Pick and Place...") + node._executor.shutdown(wait=False) + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() + + signal.signal(signal.SIGINT, shutdown) + signal.signal(signal.SIGTERM, shutdown) try: outcome = sm(bb) yasmin.YASMIN_LOG_INFO(f"Pick and Place finished with outcome: {outcome}") except Exception as e: yasmin.YASMIN_LOG_WARN(str(e)) - - if rclpy.ok(): - node.destroy_node() - rclpy.shutdown() + finally: + # Stop the executor spin thread cleanly + node._executor.shutdown() + node._spin_thread.join() + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tasks/pick_and_place/pick_and_place/states/__init__.py b/tasks/pick_and_place/pick_and_place/states/__init__.py index 004fc900b..0ac1d1ab0 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -4,6 +4,17 @@ from .detect_objects import DetectObjects from .select_and_visualize_object import SelectAndVisualiseObject from .classify_category import ClassifyCategory +from .decide_destination import DecideDestination from .choose_shelf import ChooseShelf from .instruct_pick import InstructPick -from .instruct_place import InstructPlace \ No newline at end of file +from .instruct_place import InstructPlace +from .serve_breakfast import ServeBreakfast +from .add_table_collision import AddTableCollision + +# from .grasp_object import GraspObject +from .approach_table import ApproachTable +from .serve_breakfast import ServeBreakfast +from .table_cleanup import TableCleanup +from .extra_surface_cleanup import ExtraSurfaceCleanup +from .detect_trash_floor import DetectFloorTrash +from .scan_shelves_if_needed import ScanShelvesIfNeeded diff --git a/tasks/pick_and_place/pick_and_place/states/add_table_collision.py b/tasks/pick_and_place/pick_and_place/states/add_table_collision.py new file mode 100644 index 000000000..817714864 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/add_table_collision.py @@ -0,0 +1,418 @@ +import time +import numpy as np + +import yasmin +import yasmin_ros + +import rclpy +import rclpy.duration +from rclpy.time import Time as ROS2Time +from rclpy.duration import Duration as ROS2Duration +from rclpy.action import ActionClient +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import Pose, PointStamped, Point +from sensor_msgs.msg import Image, CameraInfo + +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration as DurationMsg + +import tf2_ros +from tf2_geometry_msgs import do_transform_point +from cv_bridge import CvBridge + +from lasr_vision_interfaces.srv import OpenVocabDetect +from geometry_msgs.msg import Pose, PointStamped, Point, Quaternion + + +class AddTableCollision(yasmin.State): + """ + Publishes the table as a box collision object to /collision_object so MoveIt + plans the arm AROUND it (otherwise it plans through the table and the sim + physics explodes). + + PRIMARY PATH (detect=True): look down, run open-vocab detection for "table", + take the largest returned box, grid-sample the depth image inside it, project + those pixels into MAP, and build the box from the REAL table surface + (centre x/y, top height z, and — optionally — the seen footprint size). + This removes the need to hand-tune map coordinates. + + NOTE: CLIP rerank (if enabled for the task) relabels the box to a grocery + candidate, so we DO NOT trust the detection's name — we take the biggest box. + + FALLBACK PATH (no table detected, or detect=False): use the configured + base_footprint box, transformed into MAP and locked there. + + Also publishes the detected table centre/size to the blackboard so a later + ApproachTable state can drive up to it. + + ROS 2 params (pick_and_place.table.collision): + detect : bool - run detection (default True) + head_tilt : float - head tilt while detecting (default -0.4; + use a shallower value e.g. -0.25 to see a + table that is further away) + frame_id : str - fallback input frame (default base_footprint) + size : [x, y, z] - fallback / minimum size + position : [x, y, z] - fallback box centre in frame_id + use_detected_size : bool - size box from seen footprint (default True) + size_margin : float - metres added per side of detected size (0.10) + + Blackboard outputs: + table_point : geometry_msgs/Point - table centre in map (None if unknown) + table_size : [x, y, z] - table box size used + + Outcomes: succeeded + """ + + HEAD_PAN_JOINT = "head_1_joint" + HEAD_TILT_JOINT = "head_2_joint" + HEAD_TILT_DOWN = -0.4 + + RGB_TOPIC = "/head_front_camera/rgb/image_raw" + DEPTH_TOPIC = "/head_front_camera/depth/image_raw" + INFO_TOPIC = "/head_front_camera/rgb/camera_info" + + DEFAULT_SIZE = [1.2, 1.6, 0.74] + DEFAULT_POSITION = [1.3, 0.0, 0.37] + + TABLE_QUERY = "box" + BOX_THRESHOLD = 0.15 # low: tables are big/obvious, keep recall high + TEXT_THRESHOLD = 0.10 + SURFACE_Z_BAND = 0.08 # m: points within this of the median z are "the top" + + def __init__(self, head_tilt: float = None): + """ + Args: + head_tilt: head tilt used while detecting. Overrides the ROS param. + Use a shallower value (e.g. -0.25) for the far DETECT_TABLE + instance, and the steeper default (-0.4) up close. + """ + super().__init__(outcomes=["succeeded"]) + self.add_output_key("table_point") + self.add_output_key("table_size") + self._head_tilt = head_tilt + self.node = yasmin_ros.logger_node + self.bridge = CvBridge() + + self._pub = self.node.create_publisher(CollisionObject, "/collision_object", 10) + self._tf = tf2_ros.Buffer(cache_time=rclpy.duration.Duration(seconds=30.0)) + self._tf_listener = tf2_ros.TransformListener(self._tf, self.node) + + self._rgb = None + self._depth = None + self._info = None + cam_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + ) + self.node.create_subscription(Image, self.RGB_TOPIC, self._rgb_cb, cam_qos) + self.node.create_subscription(Image, self.DEPTH_TOPIC, self._depth_cb, cam_qos) + self.node.create_subscription(CameraInfo, self.INFO_TOPIC, self._info_cb, cam_qos) + + self._ovd = self.node.create_client(OpenVocabDetect, "open_vocab/detect") + self._head = ActionClient( + self.node, FollowJointTrajectory, + "/head_controller/follow_joint_trajectory", + ) + + # camera callbacks + def _rgb_cb(self, m): + self._rgb = m + + def _depth_cb(self, m): + self._depth = m + + def _info_cb(self, m): + self._info = m + + # params + def _param(self, name, default): + try: + v = self.node.get_parameter(name).value + return v if v is not None else default + except Exception: + return default + + def _quat_param(self, base): + lst = self._param(base, None) + if isinstance(lst, (list, tuple)) and len(lst) == 4: + return [float(v) for v in lst] + x = self._param(base + ".x", None) + y = self._param(base + ".y", None) + z = self._param(base + ".z", None) + w = self._param(base + ".w", None) + if None not in (z, w): + return [float(x or 0.0), float(y or 0.0), float(z), float(w)] + return None + + def _table_orientation(self): + o = self._quat_param("pick_and_place.table.collision.orientation") + if o is None: + o = self._quat_param("pick_and_place.table.pose.orientation") + if o is None: + o = [0.0, 0.0, 0.0, 1.0] + q = Quaternion() + q.x, q.y, q.z, q.w = o[0], o[1], o[2], o[3] + return q + # head + def _look_down(self): + if not self._head.wait_for_server(timeout_sec=5.0): + yasmin.YASMIN_LOG_WARN("head controller unavailable; skipping look-down") + return + tilt = self._head_tilt + if tilt is None: + tilt = self._param("pick_and_place.table.collision.head_tilt", + self.HEAD_TILT_DOWN) + tilt = float(tilt) + pt = JointTrajectoryPoint() + pt.positions = [0.0, tilt] + pt.time_from_start = DurationMsg(sec=2) + traj = JointTrajectory() + traj.joint_names = [self.HEAD_PAN_JOINT, self.HEAD_TILT_JOINT] + traj.points = [pt] + goal = FollowJointTrajectory.Goal() + goal.trajectory = traj + self._head.send_goal_async(goal) + yasmin.YASMIN_LOG_INFO("Tilting head down to look at the table…") + time.sleep(3.0) + + @staticmethod + def _wait_future(future, timeout=30.0): + start = time.time() + while not future.done(): + if time.time() - start > timeout: + return None + time.sleep(0.05) + try: + return future.result() + except Exception: + return None + + # detection + def _detect_table_box(self): + """Return the largest detected box (cx, cy, w, h) for query 'table', + ignoring the (CLIP-reranked, unreliable) label. None on failure.""" + t0 = time.time() + while (self._rgb is None or self._info is None) and time.time() - t0 < 5.0: + time.sleep(0.1) + if self._rgb is None or self._info is None: + yasmin.YASMIN_LOG_WARN("No camera image/info — cannot detect table.") + return None + if not self._ovd.wait_for_service(timeout_sec=10.0): + yasmin.YASMIN_LOG_WARN("open_vocab/detect unavailable — cannot detect table.") + return None + + req = OpenVocabDetect.Request() + req.image = self._rgb + req.queries = [self.TABLE_QUERY] + req.box_threshold = float(self.BOX_THRESHOLD) + req.text_threshold = float(self.TEXT_THRESHOLD) + resp = self._wait_future(self._ovd.call_async(req), timeout=120.0) + if resp is None or not resp.detections: + yasmin.YASMIN_LOG_WARN("No 'table' detections returned.") + return None + + best = None + best_area = -1.0 + for d in resp.detections: + if len(d.xywh) < 4: + continue + cx, cy, w, h = d.xywh[0], d.xywh[1], d.xywh[2], d.xywh[3] + area = float(w) * float(h) + if area > best_area: + best_area = area + best = (float(cx), float(cy), float(w), float(h)) + if best is not None: + yasmin.YASMIN_LOG_INFO( + f"Largest 'table' box cxywh=({best[0]:.0f},{best[1]:.0f}," + f"{best[2]:.0f},{best[3]:.0f})" + ) + return best + + def _estimate_table_in_map(self, bbox): + """Grid-sample depth inside the box, project to MAP, robustly estimate + the table surface. Returns dict(cx, cy, top_z, sx, sy) or None.""" + if self._depth is None or self._info is None or self._rgb is None: + return None + try: + depth_img = self.bridge.imgmsg_to_cv2(self._depth, "32FC1") + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"depth convert failed: {e}") + return None + + H, W = depth_img.shape[:2] + cx, cy, w, h = bbox + x1, x2 = cx - w / 2.0, cx + w / 2.0 + y1, y2 = cy - h / 2.0, cy + h / 2.0 + # inset 12% so we sample the surface, not the edges/background + ix, iy = 0.12 * w, 0.12 * h + gxs = np.linspace(x1 + ix, x2 - ix, 11) + gys = np.linspace(y1 + iy, y2 - iy, 11) + + K = self._info.k + fx, fy, cxp, cyp = K[0], K[4], K[2], K[5] + cam_frame = self._rgb.header.frame_id + try: + tr = self._tf.lookup_transform( + "map", cam_frame, self._rgb.header.stamp, + timeout=ROS2Duration(seconds=0.5), + ) + except Exception: + try: + tr = self._tf.lookup_transform( + "map", cam_frame, ROS2Time(seconds=0), + timeout=ROS2Duration(seconds=0.5), + ) + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"TF map<-{cam_frame} failed: {e}") + return None + + pts = [] + for gy in gys: + py = int(np.clip(gy, 0, H - 1)) + for gx in gxs: + px = int(np.clip(gx, 0, W - 1)) + d = float(depth_img[py, px]) + if d <= 0.0 or np.isnan(d) or d > 6.0: + continue + ps = PointStamped() + ps.header.frame_id = cam_frame + ps.header.stamp = self._rgb.header.stamp + ps.point.x = (px - cxp) * d / fx + ps.point.y = (py - cyp) * d / fy + ps.point.z = d + try: + mp = do_transform_point(ps, tr).point + except Exception: + continue + pts.append((mp.x, mp.y, mp.z)) + + if len(pts) < 12: + yasmin.YASMIN_LOG_WARN(f"Too few valid depth points ({len(pts)}).") + return None + + arr = np.array(pts) + zmed = float(np.median(arr[:, 2])) + surf = arr[np.abs(arr[:, 2] - zmed) < self.SURFACE_Z_BAND] + if len(surf) < 8: + surf = arr + return { + "cx": float(np.median(surf[:, 0])), + "cy": float(np.median(surf[:, 1])), + "top_z": float(np.median(surf[:, 2])), + "sx": float(np.percentile(surf[:, 0], 95) - np.percentile(surf[:, 0], 5)), + "sy": float(np.percentile(surf[:, 1], 95) - np.percentile(surf[:, 1], 5)), + "n": int(len(surf)), + } + + # config fallback + def _lookup_map(self, frame): + t0 = time.time() + while time.time() - t0 < 5.0: + try: + return self._tf.lookup_transform( + "map", frame, ROS2Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + except Exception: + time.sleep(0.2) + return None + def _config_box(self): + """Configured box, ALWAYS published in 'map' so a hand-set box stays + locked in place. frame_id from config is intentionally IGNORED — a + base_footprint box is what made the box follow the robot.""" + size = list(self._param("pick_and_place.table.collision.size", self.DEFAULT_SIZE)) + pos = list(self._param("pick_and_place.table.collision.position", self.DEFAULT_POSITION)) + px, py, pz = float(pos[0]), float(pos[1]), float(pos[2]) + yasmin.YASMIN_LOG_INFO( + f"Configured table box in 'map' at ({px:.2f}, {py:.2f}, {pz:.2f})." + ) + + quat = self._table_orientation() + return "map", (px, py, pz), [float(size[0]), float(size[1]), float(size[2])], quat + + # publish + def _publish(self, frame, centre, size, orient=None): + co = CollisionObject() + co.header.frame_id = frame + co.id = "table" + co.operation = CollisionObject.ADD + + box = SolidPrimitive() + box.type = SolidPrimitive.BOX + box.dimensions = [float(size[0]), float(size[1]), float(size[2])] + co.primitives.append(box) + + p = Pose() + p.position.x, p.position.y, p.position.z = centre + if orient is not None: + p.orientation = orient + else: + p.orientation.w = 1.0 + co.primitive_poses.append(p) + co.pose.orientation.w = 1.0 + for _ in range(5): + self._pub.publish(co) + time.sleep(0.2) + yasmin.YASMIN_LOG_INFO( + f"Published 'table' box centre=({centre[0]:.2f},{centre[1]:.2f}," + f"{centre[2]:.2f}) size=[{size[0]:.2f},{size[1]:.2f},{size[2]:.2f}] " + f"frame='{frame}'." + ) + + + def _store_table(self, blackboard, cx, cy, cz, size): + """Expose the table centre/size for a later ApproachTable state.""" + pt = Point() + pt.x, pt.y, pt.z = float(cx), float(cy), float(cz) + blackboard["table_point"] = pt + blackboard["table_size"] = [float(size[0]), float(size[1]), float(size[2])] + + # main + def execute(self, blackboard) -> str: + detect = bool(self._param("pick_and_place.table.collision.detect", True)) + + if detect: + self._look_down() + bbox = self._detect_table_box() + est = self._estimate_table_in_map(bbox) if bbox is not None else None + if est is not None: + cfg_size = list( + self._param("pick_and_place.table.collision.size", self.DEFAULT_SIZE) + ) + use_det = bool( + self._param("pick_and_place.table.collision.use_detected_size", True) + ) + margin = float( + self._param("pick_and_place.table.collision.size_margin", 0.10) + ) + yasmin.YASMIN_LOG_INFO( + f"DETECTED table: centre=({est['cx']:.2f},{est['cy']:.2f}) " + f"top_z={est['top_z']:.2f} seen_size=({est['sx']:.2f}," + f"{est['sy']:.2f}) from {est['n']} pts." + ) + if use_det: + sx = float(np.clip(est["sx"] + 2 * margin, 0.40, 3.0)) + sy = float(np.clip(est["sy"] + 2 * margin, 0.40, 3.0)) + else: + sx, sy = float(cfg_size[0]), float(cfg_size[1]) + top_z = max(0.30, est["top_z"]) # box: floor -> table top + centre = (est["cx"], est["cy"], top_z / 2.0) + self._publish("map", centre, [sx, sy, top_z], orient=None) + # table CENTRE in map (surface height) for ApproachTable + self._store_table(blackboard, est["cx"], est["cy"], top_z, [sx, sy, top_z]) + return "succeeded" + yasmin.YASMIN_LOG_WARN("Table detection failed — using configured box.") + + frame, centre, size, orient = self._config_box() + self._publish(frame, centre, size, orient) + if frame == "map": + self._store_table(blackboard, centre[0], centre[1], centre[2], size) + else: + blackboard["table_point"] = None + blackboard["table_size"] = list(size) + return "succeeded" \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/approach_table.py b/tasks/pick_and_place/pick_and_place/states/approach_table.py new file mode 100644 index 000000000..ffbcbef65 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/approach_table.py @@ -0,0 +1,149 @@ +import time +import math + +import yasmin +import yasmin_ros + +import rclpy +import rclpy.duration +from rclpy.action import ActionClient +from rclpy.time import Time as ROS2Time + +from geometry_msgs.msg import Pose, PoseStamped +from std_msgs.msg import Header +from nav2_msgs.action import NavigateToPose + +import tf2_ros + + +def _wait_future(future, timeout): + """Wait on a future spun by the SM node's background executor.""" + deadline = time.time() + timeout + while not future.done() and time.time() < deadline: + time.sleep(0.02) + return future.result() if future.done() else None + + +class ApproachTable(yasmin.State): + """ + Drive to a standoff pose directly in FRONT of the detected table, facing it. + + Reads the table centre that AddTableCollision wrote to the blackboard + (table_point, in map), takes the line from the robot to the table, and places + a Nav2 goal `standoff` metres from the table centre along that line, oriented + to look at the table. If navigation is rejected/fails (e.g. the goal is inside + the table's costmap inflation), it backs off and retries at larger standoffs. + + Blackboard inputs: + table_point : geometry_msgs/Point - table centre in map + + Blackboard outputs: + table_pose : geometry_msgs/Pose - the approach pose actually reached + + ROS 2 params (pick_and_place.approach): + standoff : float - metres from table CENTRE (default 0.85). + Smaller => closer (better arm reach) but + Nav2 may refuse if too close to the table. + retry_increments : [float, ...] - extra standoff to add on retry + (default [0.0, 0.15, 0.30, 0.45]) + nav_timeout : float - per-attempt nav timeout (default 120) + + Outcomes: succeeded, failed + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("table_point") + self.add_output_key("table_pose") + + self.node = yasmin_ros.logger_node + self._tf = tf2_ros.Buffer(cache_time=rclpy.duration.Duration(seconds=30.0)) + self._tf_listener = tf2_ros.TransformListener(self._tf, self.node) + self._nav = ActionClient(self.node, NavigateToPose, "navigate_to_pose") + + def _param(self, name, default): + try: + v = self.node.get_parameter(name).value + return v if v is not None else default + except Exception: + return default + + def _robot_xy(self, timeout=5.0): + t0 = time.time() + while time.time() - t0 < timeout: + try: + tf = self._tf.lookup_transform( + "map", "base_footprint", ROS2Time(), + timeout=rclpy.duration.Duration(seconds=0.5), + ) + return tf.transform.translation.x, tf.transform.translation.y + except Exception: + time.sleep(0.2) + return None + + def _navigate(self, pose, timeout): + if not self._nav.wait_for_server(timeout_sec=5.0): + yasmin.YASMIN_LOG_ERROR("Nav2 navigate_to_pose server not available.") + return False + goal = NavigateToPose.Goal() + goal.pose = PoseStamped(header=Header(frame_id="map"), pose=pose) + gh = _wait_future(self._nav.send_goal_async(goal), 5.0) + if gh is None or not gh.accepted: + yasmin.YASMIN_LOG_WARN("Nav goal rejected.") + return False + res = _wait_future(gh.get_result_async(), timeout) + if res is None: + yasmin.YASMIN_LOG_WARN("Navigation timed out.") + return False + if res.status != 4: # 4 = SUCCEEDED (action_msgs/GoalStatus) + yasmin.YASMIN_LOG_WARN(f"Navigation failed (status {res.status}).") + return False + return True + + def execute(self, blackboard) -> str: + try: + tp = blackboard["table_point"] + except Exception: + tp = None + if tp is None: + yasmin.YASMIN_LOG_WARN("No table_point in blackboard — cannot approach.") + return "failed" + + standoff = float(self._param("pick_and_place.approach.standoff", 0.85)) + incs = list(self._param( + "pick_and_place.approach.retry_increments", [0.0, 0.15, 0.30, 0.45] + )) + nav_timeout = float(self._param("pick_and_place.approach.nav_timeout", 120.0)) + + rxy = self._robot_xy() + if rxy is None: + yasmin.YASMIN_LOG_ERROR("No robot pose (TF map->base_footprint).") + return "failed" + rx, ry = rxy + + dx, dy = rx - tp.x, ry - tp.y + dist = math.hypot(dx, dy) + ux, uy = (1.0, 0.0) if dist < 1e-3 else (dx / dist, dy / dist) + yasmin.YASMIN_LOG_INFO( + f"Table at map=({tp.x:.2f},{tp.y:.2f}); robot=({rx:.2f},{ry:.2f}); " + f"current gap={dist:.2f} m." + ) + + for inc in incs: + s = standoff + float(inc) + gx, gy = tp.x + ux * s, tp.y + uy * s + yaw = math.atan2(tp.y - gy, tp.x - gx) # face the table + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = gx, gy, 0.0 + pose.orientation.z = math.sin(yaw / 2.0) + pose.orientation.w = math.cos(yaw / 2.0) + yasmin.YASMIN_LOG_INFO( + f"Approach: standoff={s:.2f} goal=({gx:.2f},{gy:.2f}) yaw={yaw:.2f}" + ) + if self._navigate(pose, nav_timeout): + blackboard["table_pose"] = pose + yasmin.YASMIN_LOG_INFO("Reached table approach pose.") + return "succeeded" + + yasmin.YASMIN_LOG_WARN("All approach standoffs failed.") + return "failed" \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/choose_shelf.py b/tasks/pick_and_place/pick_and_place/states/choose_shelf.py index 2bb0ad0aa..0748f74d5 100644 --- a/tasks/pick_and_place/pick_and_place/states/choose_shelf.py +++ b/tasks/pick_and_place/pick_and_place/states/choose_shelf.py @@ -1,6 +1,13 @@ import yasmin import yasmin_ros +SHELF_POSITION_NAMES = { + "extra_bottom": "first shelf from the bottom", + "bottom": "second shelf from the bottom", + "middle": "third shelf from the bottom", + "top": "fourth shelf from the bottom", +} + class ChooseShelf(yasmin.State): """ @@ -40,9 +47,9 @@ def __init__(self): self.add_output_key("shelf_data") def execute(self, blackboard) -> str: - object_name = blackboard["selected_object_name"] + object_name = blackboard["selected_object_name"] object_category = blackboard["object_category"] - shelf_data = blackboard["shelf_data"] + shelf_data = blackboard["shelf_data"] yasmin.YASMIN_LOG_INFO( f"Choosing shelf for '{object_name}' (category: '{object_category}')." @@ -50,18 +57,19 @@ def execute(self, blackboard) -> str: yasmin.YASMIN_LOG_INFO(f"Current shelf data: {shelf_data}") if not shelf_data: - blackboard["chosen_shelf"] = f"the {object_category} shelf" - blackboard["chosen_shelf_str"] = f"the shelf for {object_category} items" + blackboard["chosen_shelf"] = "second shelf from the bottom" + blackboard["chosen_shelf_str"] = ( + f"near the {object_category} items if possible" + ) yasmin.YASMIN_LOG_WARN( - f"No shelf data (scan skipped) — defaulting to " - f"the {object_category} shelf." + "No shelf data (scan skipped) — defaulting to second shelf from the bottom." ) return "succeeded" - - chosen_shelf = None - chosen_shelf_str = "" - max_count = -1 - fallback_shelf = None + + chosen_shelf = None + chosen_shelf_str = "" + max_count = -1 + fallback_shelf = None min_total_objects = float("inf") # ── Pass 1: find best matching shelf ───────────────────────────────── @@ -78,7 +86,7 @@ def execute(self, blackboard) -> str: # Priority 2: shelf with the most items of this category count = shelf_info.get("category_counts", {}).get(object_category, 0) if count > max_count: - max_count = count + max_count = count chosen_shelf = shelf_name yasmin.YASMIN_LOG_INFO( f"Best category count so far ({count}) on '{shelf_name}'." @@ -88,7 +96,7 @@ def execute(self, blackboard) -> str: total_objects = len(shelf_info.get("objects", [])) if total_objects < min_total_objects: min_total_objects = total_objects - fallback_shelf = shelf_name + fallback_shelf = shelf_name # ── Pass 2: try an empty shelf ──────────────────────────────────────── if chosen_shelf is None or max_count == 0: @@ -111,14 +119,14 @@ def execute(self, blackboard) -> str: if chosen_shelf: shelf_info = shelf_data[chosen_shelf] - was_empty = shelf_info["category"] == "empty" + was_empty = shelf_info["category"] == "empty" category_previously_present = object_category in shelf_info.get( "category_counts", {} ) shelf_info.setdefault("objects", []).append(object_name) shelf_info.setdefault("category_counts", {})[object_category] = ( - shelf_info["category_counts"].get(object_category, 0) + 1 + shelf_info.get("category_counts", {}).get(object_category, 0) + 1 ) new_dominant = max( @@ -131,9 +139,14 @@ def execute(self, blackboard) -> str: else: chosen_shelf_str = "" - blackboard["chosen_shelf"] = chosen_shelf - blackboard["chosen_shelf_str"] = chosen_shelf_str - blackboard["shelf_data"] = shelf_data + # Convert internal shelf ID to human-readable position name + position_name = SHELF_POSITION_NAMES.get(chosen_shelf, chosen_shelf) + + blackboard["chosen_shelf"] = ( + position_name # e.g. "second shelf from the bottom" + ) + blackboard["chosen_shelf_str"] = chosen_shelf_str # e.g. "near the drinks" + blackboard["shelf_data"] = shelf_data yasmin.YASMIN_LOG_INFO( f"Chose shelf '{chosen_shelf}'. " @@ -142,4 +155,4 @@ def execute(self, blackboard) -> str: return "succeeded" yasmin.YASMIN_LOG_ERROR("No suitable shelf found.") - return "failed" \ No newline at end of file + return "failed" diff --git a/tasks/pick_and_place/pick_and_place/states/classify_category.py b/tasks/pick_and_place/pick_and_place/states/classify_category.py index 4ab71481d..b479f8bab 100644 --- a/tasks/pick_and_place/pick_and_place/states/classify_category.py +++ b/tasks/pick_and_place/pick_and_place/states/classify_category.py @@ -3,38 +3,38 @@ from yasmin_ros.yasmin_node import YasminNode import rclpy import time -from lasr_llm_interfaces.srv import StoringGroceriesQueryLlm +# NOTE: lasr_llm_interfaces is imported LAZILY inside _classify_with_llm so that a +# broken/stale typesupport (.so) never crashes state-machine construction. The LLM +# is only a 3rd-tier fallback after param lookup + CATEGORY_MAP. CATEGORY_MAP = { "fruit": { - "apple", "banana", "orange", "grape", "pineapple", "lemon", - "lime", "peach", "plum", "pear", "mango", "watermelon", - "strawberry", "blueberry", + "apple", "banana", "orange", }, "vegetable": { "carrot", "tomato", "cucumber", "lettuce", "onion", "broccoli", "cabbage", "pepper", "zucchini", "radish", "corn", "potato", "garlic", }, - "beverage": { - "bottle", "can", "water bottle", "juice box", "milk carton", - "soda can", "coffee cup", "energy drink", "thermos", + "drink": { + "bottle", "water bottle", "juice", "milk", + "soda can", "coffee cup", "energy drink", "thermos", "coke", "red bull", "iced tea", }, "snack": { "chips", "crackers", "candy", "chocolate bar", "cookie", - "snack bag", "biscuit", "granola bar", "popcorn", + "snack bag", "biscuit", "granola bar", "popcorn", "pringles", "crisps", }, "cleaning": { "soap", "sponge", "brush", "cleaner", "detergent", "tissue box", - "toilet paper", "broom", "mop", "spray bottle", "bucket", + "toilet paper", "broom", "mop", "spray bottle", "bucket", "toothpaste", }, "cereal": { "cereal", "cereal box", "oats", "muesli", }, "dish": { - "fork", "knife", "spoon", "plate", "bowl", "cup", "wine glass", - "mug", "chopsticks", + "fork", "knife", "spoon", "plate", "bowl", "wine glass", + "mug", "chopsticks", "cup", }, } @@ -52,7 +52,7 @@ class ClassifyCategory(yasmin.State): Classification priority: 1. ROS 2 param lookup (pick_and_place.objects..category) 2. Hardcoded CATEGORY_MAP - 3. LLM fallback via /lasr_llm/llm + 3. LLM fallback via /storing_groceries/query_llm Blackboard inputs: object_name : str — single object name (used when task="object") @@ -84,7 +84,9 @@ def __init__(self, task: str = "object"): self.add_output_key("shelf_category") self.node = yasmin_ros.logger_node - self._llm_client = self.node.create_client(StoringGroceriesQueryLlm, "/storing_groceries/query_llm") + # Created lazily on first LLM use (see _classify_with_llm). + self._llm_client = None + self._llm_srv_type = None def execute(self, blackboard) -> str: if self._task == "object": @@ -173,12 +175,28 @@ def _get_category(self, name: str) -> str | None: return self._classify_with_llm(name) def _classify_with_llm(self, name: str) -> str | None: + # Lazily import the interface + create the client on first use only. + # BOTH the import AND create_client are wrapped: the typesupport error + # for a broken lasr_llm_interfaces fires at create_client, so it must be + # inside the try. On any failure we skip the LLM tier instead of crashing. + if self._llm_client is None: + try: + from lasr_llm_interfaces.srv import StoringGroceriesQueryLlm + self._llm_srv_type = StoringGroceriesQueryLlm + self._llm_client = self.node.create_client( + StoringGroceriesQueryLlm, "/storing_groceries/query_llm" + ) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"lasr_llm_interfaces unavailable — skipping LLM tier ({e})." + ) + return None if not self._llm_client.wait_for_service(timeout_sec=5.0): yasmin.YASMIN_LOG_WARN("LLM service not available — skipping LLM tier.") return None - req = StoringGroceriesQueryLlm.Request() + req = self._llm_srv_type.Request() req.llm_input = [name] req.task = "ClassifyObject" diff --git a/tasks/pick_and_place/pick_and_place/states/decide_destination.py b/tasks/pick_and_place/pick_and_place/states/decide_destination.py new file mode 100644 index 000000000..c3ff88424 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/decide_destination.py @@ -0,0 +1,140 @@ +import yasmin +import yasmin_ros + +from geometry_msgs.msg import Point, Quaternion, Pose + + +# Object categories that belong in the dishwasher (dirty tableware + cutlery). +# In CATEGORY_MAP these all fall under "dish". +DISHWASHER_CATEGORIES = {"dish"} + + +class DecideDestination(yasmin.State): + """ + Decides where the selected object must go, based on its category — this is + the "task planning" core of the Pick and Place challenge: + + - tableware / cutlery (category "dish") → dishwasher + - the announced trash category → trash bin + - everything else → cabinet (then ChooseShelf) + + Manipulation is announce-only, so this state also loads the chosen + destination's nav pose into blackboard["location"] for the generic + GoToLocation state that follows. + + ROS 2 params: + pick_and_place.trash_category — category treated as trash ("" = none) + pick_and_place.dishwasher.pose.* — dishwasher nav pose + pick_and_place.trash_bin.pose.* — trash bin nav pose + pick_and_place.cabinet.pose.* — cabinet nav pose + + Blackboard inputs: + object_category : str + selected_object_name : str + + Blackboard outputs: + destination : str — "dishwasher" | "trash_bin" | "cabinet" + destination_str : str — human phrase e.g. "the dishwasher" + location : Pose — nav goal for GoToLocation + chosen_shelf : str — cleared ("") for non-cabinet destinations + chosen_shelf_str : str — cleared ("") for non-cabinet destinations + + Outcomes: + cabinet : object goes to the cabinet → run ChooseShelf next + other : object goes to dishwasher/trash → skip ChooseShelf + """ + + DEST_STR = { + "dishwasher": "the dishwasher", + "trash_bin": "the trash bin", + "cabinet": "the cabinet", + } + + def __init__(self): + super().__init__(outcomes=["cabinet", "other"]) + self.add_input_key("object_category") + self.add_input_key("selected_object_name") + self.add_output_key("destination") + self.add_output_key("destination_str") + self.add_output_key("location") + self.add_output_key("chosen_shelf") + self.add_output_key("chosen_shelf_str") + + self.node = yasmin_ros.logger_node + + def execute(self, blackboard) -> str: + name = blackboard["selected_object_name"] + category = (blackboard["object_category"] or "").lower() + + trash_category = self._get_str_param("pick_and_place.trash_category", "") + + # ── Route ───────────────────────────────────────────────────────────── + if category in DISHWASHER_CATEGORIES: + destination = "dishwasher" + elif trash_category and category == trash_category.lower(): + destination = "trash_bin" + else: + destination = "cabinet" + + yasmin.YASMIN_LOG_INFO( + f"'{name}' (category '{category}') -> {destination}." + ) + + # ── Load destination pose into blackboard["location"] ───────────────── + blackboard["location"] = self._load_pose( + f"pick_and_place.{destination}.pose" + ) + blackboard["destination"] = destination + blackboard["destination_str"] = self.DEST_STR.get(destination, destination) + + if destination == "cabinet": + # ChooseShelf fills in chosen_shelf / chosen_shelf_str + return "cabinet" + + # Non-cabinet: clear any stale shelf hint so InstructPlace omits it + blackboard["chosen_shelf"] = "" + blackboard["chosen_shelf_str"] = "" + return "other" + + # ── helpers ───────────────────────────────────────────────────────────── + + def _get_str_param(self, name: str, default: str) -> str: + try: + val = self.node.get_parameter(name).get_parameter_value().string_value + return val if val else default + except Exception: + return default + + def _load_pose(self, prefix: str) -> Pose: + """Reads .position.* / .orientation.* params into a Pose. + + Never raises: missing components default to 0.0, and a fully-zero + (invalid) quaternion is repaired to identity so navigation does not + crash if a destination pose has not been configured yet. + """ + def g(comp: str) -> float: + p = f"{prefix}.{comp}" + try: + if not self.node.has_parameter(p): + self.node.declare_parameter(p, 0.0) + return float(self.node.get_parameter(p).value) + except Exception: + return 0.0 + + pose = Pose( + position=Point(x=g("position.x"), y=g("position.y"), z=g("position.z")), + orientation=Quaternion( + x=g("orientation.x"), y=g("orientation.y"), + z=g("orientation.z"), w=g("orientation.w"), + ), + ) + + if (pose.orientation.x == 0.0 and pose.orientation.y == 0.0 + and pose.orientation.z == 0.0 and pose.orientation.w == 0.0): + yasmin.YASMIN_LOG_WARN( + f"Pose '{prefix}' looks unset (zero quaternion) — " + f"check config.yaml. Using identity orientation." + ) + pose.orientation.w = 1.0 + + return pose \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/detect_objects.py b/tasks/pick_and_place/pick_and_place/states/detect_objects.py index 6989d1186..d0215e3a2 100644 --- a/tasks/pick_and_place/pick_and_place/states/detect_objects.py +++ b/tasks/pick_and_place/pick_and_place/states/detect_objects.py @@ -1,258 +1,159 @@ -import time -import numpy as np - import yasmin import yasmin_ros +import os +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon +from ament_index_python.packages import get_package_share_directory +from lasr_skills import DetectAllInPolygon -from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy -from rclpy.duration import Duration as ROS2Duration -from rclpy.time import Time as ROS2Time -from rclpy.action import ActionClient - -import tf2_ros -from tf2_geometry_msgs import do_transform_point -from cv_bridge import CvBridge - -from sensor_msgs.msg import Image, CameraInfo -from geometry_msgs.msg import PointStamped - -from lasr_vision_interfaces.srv import OpenVocabDetect -from lasr_vision_interfaces.msg import Detection3D - -from control_msgs.action import FollowJointTrajectory -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from builtin_interfaces.msg import Duration as DurationMsg +_MODEL_PATH = os.path.join( + get_package_share_directory("lasr_vision_yolo"), "models", "best.pt" +) class DetectObjects(yasmin.State): """ - Detects groceries on the table using OPEN-VOCABULARY detection - (lasr_vision_open_vocabulary) instead of closed COCO-YOLO. + Looks at a configured surface and detects objects within its polygon + using YOLO, replacing the open-vocabulary detection approach. - 1. Tilt head down so the camera sees the table. - 2. Call open_vocab/detect with configured grocery queries + low thresholds. - 3. Clean labels: map returned phrase ("cup box") → matching query ("cup"). - 4. Class-agnostic NMS: drop overlapping duplicates (kills stacked boxes). - 5. Project each kept box centre → 3D via depth + TF (for manipulation later). + Uses DetectAllInPolygon (already ported to ROS 2 YASMIN in lasr_skills) + with a custom or generic YOLO model specified at construction time. - ROS 2 params: - pick_and_place.objects — query words (COMMON NOUNS). Empty → default. + The polygon is loaded from ROS 2 params. + + Constructor args: + location_param : str — config prefix, e.g. "table", + "extra_surface", "breakfast_surface" + object_filter : list | None — object class names to filter for. + None detects all known objects from + pick_and_place.objects param. + model : str — YOLO model filename, e.g. "robocup.pt" + or "yolo11n-seg.pt" for generic COCO + min_confidence : float — minimum detection confidence Blackboard outputs: detected_objects : List[Detection3D] """ - HEAD_PAN_JOINT = "head_1_joint" - HEAD_TILT_JOINT = "head_2_joint" - HEAD_TILT_DOWN = -0.4 - - RGB_TOPIC = "/head_front_camera/rgb/image_raw" - DEPTH_TOPIC = "/head_front_camera/depth/image_raw" - INFO_TOPIC = "/head_front_camera/rgb/camera_info" - - DEFAULT_QUERIES = ["cup", "can", "bottle", "bowl", "box"] - BOX_THRESHOLD = 0.25 # low — open-vocab scores are modest on sim models - TEXT_THRESHOLD = 0.10 - NMS_IOU = 0.5 - - def __init__(self): + def __init__( + self, + location_param: str = "table", + object_filter: list = None, + model: str = _MODEL_PATH, + min_confidence: float = 0.1, + ): super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detected_objects") self.node = yasmin_ros.logger_node - self.bridge = CvBridge() + self._model = model + self._min_confidence = min_confidence + # Load polygon from config try: - q = list( - self.node.get_parameter("pick_and_place.objects") - .get_parameter_value() - .string_array_value + polygon_points = [ + self.node.get_parameter( + f"pick_and_place.{location_param}.polygon.top_left" + ).value, + self.node.get_parameter( + f"pick_and_place.{location_param}.polygon.top_right" + ).value, + self.node.get_parameter( + f"pick_and_place.{location_param}.polygon.bottom_right" + ).value, + self.node.get_parameter( + f"pick_and_place.{location_param}.polygon.bottom_left" + ).value, + ] + self._polygon = ShapelyPolygon(polygon_points) + yasmin.YASMIN_LOG_INFO( + f"Loaded polygon for '{location_param}': {polygon_points}" ) - self._queries = q or list(self.DEFAULT_QUERIES) - except Exception: - self._queries = list(self.DEFAULT_QUERIES) - - self._rgb = None - self._depth = None - self._info = None - cam_qos = QoSProfile( - depth=10, - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, - ) - self.node.create_subscription(Image, self.RGB_TOPIC, self._rgb_cb, cam_qos) - self.node.create_subscription(Image, self.DEPTH_TOPIC, self._depth_cb, cam_qos) - self.node.create_subscription( - CameraInfo, self.INFO_TOPIC, self._info_cb, cam_qos - ) - - self._tf = tf2_ros.Buffer(cache_time=ROS2Duration(seconds=30)) - self._tf_listener = tf2_ros.TransformListener(self._tf, self.node) - - self._ovd = self.node.create_client(OpenVocabDetect, "open_vocab/detect") - self._head = ActionClient( - self.node, FollowJointTrajectory, - "/head_controller/follow_joint_trajectory", - ) - - # ── camera callbacks ── - def _rgb_cb(self, m): - self._rgb = m - - def _depth_cb(self, m): - self._depth = m - - def _info_cb(self, m): - self._info = m - - # ── head ── - def _look_down(self): - if not self._head.wait_for_server(timeout_sec=5.0): - yasmin.YASMIN_LOG_WARN("head controller unavailable; skipping look-down") - return - pt = JointTrajectoryPoint() - pt.positions = [0.0, self.HEAD_TILT_DOWN] - pt.time_from_start = DurationMsg(sec=2) - traj = JointTrajectory() - traj.joint_names = [self.HEAD_PAN_JOINT, self.HEAD_TILT_JOINT] - traj.points = [pt] - goal = FollowJointTrajectory.Goal() - goal.trajectory = traj - self._head.send_goal_async(goal) - yasmin.YASMIN_LOG_INFO("Tilting head down to look at the table…") - time.sleep(3.0) - - # ── helpers ── - @staticmethod - def _wait_future(future, timeout=30.0): - start = time.time() - while not future.done(): - if time.time() - start > timeout: - return None - time.sleep(0.05) - try: - return future.result() - except Exception: - return None - - def _clean_label(self, phrase): - p = phrase.lower() - for q in self._queries: - if q.lower() in p: - return q - return phrase - - @staticmethod - def _iou(a, b): # a,b = (cx,cy,w,h) midpoint format - ax1, ay1, ax2, ay2 = a[0]-a[2]/2, a[1]-a[3]/2, a[0]+a[2]/2, a[1]+a[3]/2 - bx1, by1, bx2, by2 = b[0]-b[2]/2, b[1]-b[3]/2, b[0]+b[2]/2, b[1]+b[3]/2 - iw = max(0.0, min(ax2, bx2) - max(ax1, bx1)) - ih = max(0.0, min(ay2, by2) - max(ay1, by1)) - inter = iw * ih - union = a[2]*a[3] + b[2]*b[3] - inter - return inter / union if union > 0 else 0.0 - - def _nms(self, dets): # class-agnostic, keep highest-confidence per region - kept = [] - for d in sorted(dets, key=lambda x: x[1], reverse=True): - if all(self._iou(d[2], k[2]) < self.NMS_IOU for k in kept): - kept.append(d) - return kept + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load polygon for '{location_param}': {e}. " + "Using empty polygon — detections will be unconstrained." + ) + self._polygon = ShapelyPolygon() - def _project_3d(self, cx, cy): - if self._depth is None or self._info is None or self._rgb is None: - return None - try: - depth_img = self.bridge.imgmsg_to_cv2(self._depth, "32FC1") - except Exception: - return None - h, w = depth_img.shape[:2] - px = int(np.clip(cx, 0, w - 1)) - py = int(np.clip(cy, 0, h - 1)) - d = float(depth_img[py, px]) - if d <= 0.0 or np.isnan(d): - return None - K = self._info.k - fx, fy, cxp, cyp = K[0], K[4], K[2], K[5] - cam_frame = self._rgb.header.frame_id - ps = PointStamped() - ps.header.frame_id = cam_frame - ps.header.stamp = self._rgb.header.stamp - ps.point.x = (px - cxp) * d / fx - ps.point.y = (py - cyp) * d / fy - ps.point.z = d + # Load look point from config try: - tr = self._tf.lookup_transform( - "map", cam_frame, self._rgb.header.stamp, - timeout=ROS2Duration(seconds=0.5), + lp = self.node.get_parameter( + f"pick_and_place.{location_param}.look_point" + ).value + self._look_point = PointStamped( + point=Point(x=float(lp[0]), y=float(lp[1]), z=float(lp[2])), + header=Header(frame_id="map"), ) - except Exception: + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load look_point for '{location_param}': {e}. " + "Skipping head orientation." + ) + self._look_point = None + + # ── Load object filter from config or use passed-in list ─────────────── + if object_filter is not None: + self._object_filter = object_filter + else: try: - tr = self._tf.lookup_transform( - "map", cam_frame, ROS2Time(seconds=0), - timeout=ROS2Duration(seconds=0.5), + self._object_filter = list( + self.node.get_parameter("pick_and_place.objects") + .get_parameter_value() + .string_array_value ) except Exception: - return None - try: - return do_transform_point(ps, tr).point - except Exception: - return None - - # ── main ── - def execute(self, blackboard): - self._look_down() + yasmin.YASMIN_LOG_WARN( + "Could not load object filter from params. " + "Detecting all objects." + ) + self._object_filter = None + + def execute(self, blackboard) -> str: + # 1. Look at configured point + if self._look_point is not None: + # TODO: call LookToPoint with self._look_point + # from lasr_skills import LookToPoint + # look = LookToPoint(pointstamped=self._look_point) + # look.execute(blackboard) + yasmin.YASMIN_LOG_INFO( + f"[TODO] LookToPoint at " + f"({self._look_point.point.x:.2f}, " + f"{self._look_point.point.y:.2f}, " + f"{self._look_point.point.z:.2f})" + ) - t0 = time.time() - while (self._rgb is None or self._info is None) and time.time() - t0 < 5.0: - time.sleep(0.1) - if self._rgb is None or self._info is None: - yasmin.YASMIN_LOG_ERROR("No camera image/info available.") - return "failed" + # 2. Detect objects within the polygon + try: + detector = DetectAllInPolygon( + polygon=self._polygon, + object_filter=self._object_filter, + min_confidence=self._min_confidence, + model=self._model, + ) - if not self._ovd.wait_for_service(timeout_sec=10.0): - yasmin.YASMIN_LOG_ERROR("open_vocab/detect service not available.") - return "failed" + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] - req = OpenVocabDetect.Request() - req.image = self._rgb - req.queries = list(self._queries) - req.box_threshold = float(self.BOX_THRESHOLD) - req.text_threshold = float(self.TEXT_THRESHOLD) - yasmin.YASMIN_LOG_INFO(f"open_vocab queries: {self._queries}") + outcome = detector(blackboard) - resp = self._wait_future(self._ovd.call_async(req), timeout=30.0) - if resp is None: - yasmin.YASMIN_LOG_ERROR("open_vocab/detect failed or timed out.") - return "failed" + if outcome == "failed": + yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed.") + return "failed" - raw = [ - (d.name, float(d.confidence), (d.xywh[0], d.xywh[1], d.xywh[2], d.xywh[3])) - for d in resp.detections - if len(d.xywh) >= 4 - ] - yasmin.YASMIN_LOG_INFO(f"Raw open-vocab detections ({len(raw)}):") - for n, c, b in raw: - yasmin.YASMIN_LOG_INFO(f" {n}: {c:.2f} cxywh={b}") + detected = blackboard["detected_objects"] - cleaned = [(self._clean_label(n), c, b) for n, c, b in raw] - kept = self._nms(cleaned) + if not detected: + yasmin.YASMIN_LOG_INFO("No objects detected.") + return "failed" - detected = [] - for name, conf, (cx, cy, w, h) in kept: - d3 = Detection3D() - d3.name = name - d3.confidence = float(conf) - # store TOP-LEFT xywh so SelectAndVisualise draws the box correctly - d3.xywh = [int(cx - w / 2), int(cy - h / 2), int(w), int(h)] - pt = self._project_3d(cx, cy) - if pt is not None: - d3.point = pt - detected.append(d3) + labels = [f"{obj.name} ({obj.confidence:.2f})" for obj in detected] + yasmin.YASMIN_LOG_INFO( + f"Detected {len(detected)} object(s): {', '.join(labels)}" + ) + return "succeeded" - blackboard["detected_objects"] = detected - yasmin.YASMIN_LOG_INFO( - f"Detected {len(detected)} object(s): " - f"{[(d.name, round(d.confidence, 2)) for d in detected]}" - ) - return "succeeded" if detected else "failed" \ No newline at end of file + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Detection failed: {e}") + return "failed" diff --git a/tasks/pick_and_place/pick_and_place/states/detect_trash_floor.py b/tasks/pick_and_place/pick_and_place/states/detect_trash_floor.py new file mode 100644 index 000000000..e3fea7b3f --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/detect_trash_floor.py @@ -0,0 +1,232 @@ +import math +import time + +import yasmin +import yasmin_ros + +import message_filters +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from rclpy.duration import Duration as ROS2Duration +from rclpy.time import Time as ROS2Time + +from sensor_msgs.msg import Image, CameraInfo +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header + +from lasr_skills import LookToPoint +from lasr_vision_interfaces.srv import YoloDetection3D + + +class DetectFloorTrash(yasmin.State): + """ + Sweeps the head across a small ring of floor-level points around the + trash bin's known position, since the trash item's exact location + "near the trash bin" is not guaranteed by the rulebook. + + Stops sweeping as soon as one floor-level object is found, since the + rulebook guarantees exactly one trash item on the floor. + + Uses YOLO (YoloDetection3D) instead of open-vocab detection since + YOLO already returns 3D points directly — no manual depth projection + or TF transform needed. + + Reads from ROS 2 params: + pick_and_place.trash_bin.pose.position.x / .y + pick_and_place.objects (nested dict — extracts object names) + + Blackboard outputs: + detected_objects : List[Detection3D] + """ + + FLOOR_Z_MAX = 0.3 # anything below this height counts as floor-level + SWEEP_RADIUS = 0.6 # metres around the trash bin to look at + SWEEP_POINTS_COUNT = 4 # how many points around the bin to check + + RGB_TOPIC = "/head_front_camera/rgb/image_raw" + DEPTH_TOPIC = "/head_front_camera/depth/image_raw" + INFO_TOPIC = "/head_front_camera/rgb/camera_info" + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("detected_objects") + + self.node = yasmin_ros.logger_node + + # ── Object query list from nested config dict ────────────────────────── + try: + objects_params = self.node.get_parameters_by_prefix( + "pick_and_place.objects" + ) + self._queries = list( + set(key.split(".")[0] for key in objects_params.keys()) + ) + if not self._queries: + raise ValueError("Empty object list") + except Exception: + self._queries = ["object", "item", "trash"] + + # ── Compute sweep points around the trash bin ────────────────────────── + try: + bin_x = self.node.get_parameter( + "pick_and_place.trash_bin.pose.position.x" + ).value + bin_y = self.node.get_parameter( + "pick_and_place.trash_bin.pose.position.y" + ).value + self._sweep_points = self._compute_sweep_points(bin_x, bin_y) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load trash_bin pose ({e}); using single forward look." + ) + self._sweep_points = [Point(x=0.0, y=0.0, z=0.1)] + + # ── Synchronized camera capture ──────────────────────────────────────── + self._latest = None # (rgb, depth, info) tuple, set by sync callback + + cam_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + ) + + img_sub = message_filters.Subscriber( + self.node, Image, self.RGB_TOPIC, qos_profile=cam_qos + ) + depth_sub = message_filters.Subscriber( + self.node, Image, self.DEPTH_TOPIC, qos_profile=cam_qos + ) + info_sub = message_filters.Subscriber( + self.node, CameraInfo, self.INFO_TOPIC, qos_profile=cam_qos + ) + + self._info_cache = message_filters.Cache(info_sub, 10) + self._ts = message_filters.ApproximateTimeSynchronizer( + [img_sub, depth_sub], queue_size=10, slop=1.0 + ) + self._ts.registerCallback(self._sync_cb) + + # ── YOLO 3D detection client ─────────────────────────────────────────── + self._yolo = self.node.create_client(YoloDetection3D, "/yolo/detect3d") + + # ── Sweep point generation ───────────────────────────────────────────────── + + def _compute_sweep_points(self, bin_x: float, bin_y: float) -> list: + """ + Generates floor-level points in a ring around the trash bin, so the + robot looks in several directions to find the trash item regardless + of exactly where near the bin it was placed. + """ + points = [] + for i in range(self.SWEEP_POINTS_COUNT): + angle = (2 * math.pi / self.SWEEP_POINTS_COUNT) * i + x = bin_x + self.SWEEP_RADIUS * math.cos(angle) + y = bin_y + self.SWEEP_RADIUS * math.sin(angle) + points.append(Point(x=x, y=y, z=0.1)) # floor height + return points + + # ── Camera sync callback ─────────────────────────────────────────────────── + + def _sync_cb(self, img, depth): + info = self._info_cache.getLast() + if info is None: + return + self._latest = (img, depth, info) + + def _wait_for_synced_frame(self, timeout: float = 3.0): + """Clears any stale frame and waits for a fresh synchronized capture.""" + self._latest = None + start = time.time() + while self._latest is None and time.time() - start < timeout: + time.sleep(0.05) + return self._latest + + # ── Detection at a single sweep point ───────────────────────────────────── + + @staticmethod + def _wait_future(future, timeout=15.0): + start = time.time() + while not future.done(): + if time.time() - start > timeout: + return None + time.sleep(0.05) + try: + return future.result() + except Exception: + return None + + def _detect_at_current_point(self): + """ + Captures a synchronized frame and runs YOLO 3D detection on it, + returning detections filtered to floor-level objects only. + YoloDetection3D already returns 3D points directly so no manual + depth projection or TF transform is needed. + """ + frame = self._wait_for_synced_frame(timeout=3.0) + if frame is None: + yasmin.YASMIN_LOG_WARN("No synchronized camera frame available.") + return [] + + rgb, depth, info = frame + + if not self._yolo.wait_for_service(timeout_sec=5.0): + yasmin.YASMIN_LOG_WARN("YOLO service not available.") + return [] + + req = YoloDetection3D.Request() + req.image_raw = rgb + req.depth_image = depth + req.depth_camera_info = info + req.model = "best.pt" # TODO: update to your trained model name + req.confidence = 0.25 + req.nms = 0.3 + + resp = self._wait_future(self._yolo.call_async(req), timeout=15.0) + if resp is None: + return [] + + # Filter to floor-level objects only via z-height + floor_objects = [ + d for d in resp.detected_objects if d.point.z < self.FLOOR_Z_MAX + ] + return floor_objects + + # ── Main execution ───────────────────────────────────────────────────────── + + def execute(self, blackboard) -> str: + yasmin.YASMIN_LOG_INFO( + f"Sweeping {len(self._sweep_points)} floor points around the trash bin." + ) + + for i, point in enumerate(self._sweep_points): + yasmin.YASMIN_LOG_INFO( + f"Looking at sweep point {i+1}/{len(self._sweep_points)}: " + f"({point.x:.2f}, {point.y:.2f}, {point.z:.2f})" + ) + + look = LookToPoint( + pointstamped=PointStamped( + point=point, + header=Header(frame_id="map"), + ) + ) + look.execute(blackboard) # best-effort; continue regardless of outcome + + time.sleep(1.0) # allow head/camera to settle + + floor_objects = self._detect_at_current_point() + + for obj in floor_objects: + yasmin.YASMIN_LOG_INFO( + f" Floor object: {obj.name} ({obj.confidence:.2f}) " + f"at ({obj.point.x:.2f}, {obj.point.y:.2f}, {obj.point.z:.2f})" + ) + + if floor_objects: + yasmin.YASMIN_LOG_INFO( + f"Floor trash found: {floor_objects[0].name}. Stopping sweep." + ) + blackboard["detected_objects"] = floor_objects[:1] + return "succeeded" + + yasmin.YASMIN_LOG_INFO("No object found on the floor after full sweep.") + return "failed" diff --git a/tasks/pick_and_place/pick_and_place/states/extra_surface_cleanup.py b/tasks/pick_and_place/pick_and_place/states/extra_surface_cleanup.py new file mode 100644 index 000000000..03a8225a4 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/extra_surface_cleanup.py @@ -0,0 +1,138 @@ +import yasmin +import yasmin_ros + +from lasr_skills import Say, GoToLocation + +from pick_and_place.states.detect_objects import DetectObjects +from pick_and_place.states.select_and_visualize_object import SelectAndVisualiseObject +from pick_and_place.states.classify_category import ClassifyCategory +from pick_and_place.states.choose_shelf import ChooseShelf +from pick_and_place.states.instruct_pick import InstructPick +from pick_and_place.states.instruct_place import InstructPlace + + +class ExtraSurfaceCleanup(yasmin.StateMachine): + """ + Clears the extra surface, which holds exactly two objects from the + common objects set per the rulebook. Both always go to the cabinet, + grouped by category or similarity — unlike dining table cleanup, + there is no dishwasher/trash routing here since extra surface items + are not tableware, cutlery, or designated trash. + + Sequence: + SAY_GOING_TO_EXTRA_SURFACE + -> GO_TO_EXTRA_SURFACE + -> DETECT_OBJECTS (done once, both objects) + ┌-> SELECT_OBJECT (empty -> succeeded) + │ -> CLASSIFY_CATEGORY + │ -> CHOOSE_SHELF + │ -> INSTRUCT_PICK + │ -> GO_TO_CABINET + │ -> INSTRUCT_PLACE + │ -> GO_TO_EXTRA_SURFACE (loop back for second object) + └─────(loop) + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + + # Announce and navigate to extra surface + self.add_state( + "SAY_GOING_TO_EXTRA_SURFACE", + Say(text="I am now going to check the extra surface."), + transitions={ + "succeeded": "GO_TO_EXTRA_SURFACE", + "aborted": "GO_TO_EXTRA_SURFACE", + "canceled": "GO_TO_EXTRA_SURFACE", + }, + ) + + self.add_state( + "GO_TO_EXTRA_SURFACE", + GoToLocation(location_param="pick_and_place.extra_surface.pose"), + transitions={ + "succeeded": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", + }, + ) + + # Detect both objects on the extra surface + self.add_state( + "DETECT_OBJECTS", + DetectObjects(location_param="extra_surface", model="best.pt"), + transitions={ + "succeeded": "SELECT_OBJECT", + "failed": "DETECT_OBJECTS", + }, + ) + + # Select next object and visualise for referee + self.add_state( + "SELECT_OBJECT", + SelectAndVisualiseObject(), + transitions={ + "succeeded": "CLASSIFY_CATEGORY", + "finished": "succeeded", # both objects processed + }, + ) + + # Classify selected object + self.add_state( + "CLASSIFY_CATEGORY", + ClassifyCategory(task="object"), + transitions={ + "succeeded": "CHOOSE_SHELF", + "failed": "CHOOSE_SHELF", # proceed with unknown category + "empty": "SELECT_OBJECT", # nothing to classify, next object + }, + ) + + # Always goes to the cabinet — choose which shelf + self.add_state( + "CHOOSE_SHELF", + ChooseShelf(), + transitions={ + "succeeded": "INSTRUCT_PICK", + "failed": "INSTRUCT_PICK", # announce anyway + }, + ) + + # Instruct operator to pick up object + self.add_state( + "INSTRUCT_PICK", + InstructPick(), + transitions={ + "succeeded": "GO_TO_CABINET", + "failed": "INSTRUCT_PICK", + }, + ) + + # Navigate to cabinet + self.add_state( + "GO_TO_CABINET", + GoToLocation(location_param="pick_and_place.cabinet.pose"), + transitions={ + "succeeded": "INSTRUCT_PLACE", + "failed": "INSTRUCT_PLACE", # announce even if nav failed + }, + ) + + # Instruct operator where to place object + self.add_state( + "INSTRUCT_PLACE", + InstructPlace(), + transitions={ + "succeeded": "GO_TO_EXTRA_SURFACE_LOOP", + "failed": "INSTRUCT_PLACE", + }, + ) + + # Navigate back to extra surface for the second object + self.add_state( + "GO_TO_EXTRA_SURFACE_LOOP", + GoToLocation(location_param="pick_and_place.extra_surface.pose"), + transitions={ + "succeeded": "SELECT_OBJECT", # loop back for next object + "failed": "GO_TO_EXTRA_SURFACE_LOOP", + }, + ) diff --git a/tasks/pick_and_place/pick_and_place/states/find_and_go_to_table.py b/tasks/pick_and_place/pick_and_place/states/find_and_go_to_table.py index 8301b4ada..1e6bbc06c 100644 --- a/tasks/pick_and_place/pick_and_place/states/find_and_go_to_table.py +++ b/tasks/pick_and_place/pick_and_place/states/find_and_go_to_table.py @@ -16,7 +16,6 @@ def _wait_future(future, timeout): - """Чекає future, який ЗАВЕРШИТЬ фоновий екзекютор (без повторного spin).""" deadline = time.time() + timeout while not future.done() and time.time() < deadline: time.sleep(0.02) @@ -134,7 +133,7 @@ def _go_to_table(self, blackboard) -> str: if result is None: yasmin.YASMIN_LOG_WARN("Navigation timed out, trying next pose.") continue - if result.status != 4: # 4 = SUCCEEDED (action_msgs/GoalStatus) + if result.status != 4: yasmin.YASMIN_LOG_WARN(f"Nav failed (status {result.status}), next pose.") continue diff --git a/tasks/pick_and_place/pick_and_place/states/grasp_object.py b/tasks/pick_and_place/pick_and_place/states/grasp_object.py new file mode 100644 index 000000000..5a00694a6 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/grasp_object.py @@ -0,0 +1,439 @@ +import time +import math +from threading import Thread + +import yasmin +import yasmin_ros + +import rclpy +import rclpy.duration +from rclpy.action import ActionClient +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.time import Time as ROS2Time + +from sensor_msgs.msg import JointState +from geometry_msgs.msg import Twist, PointStamped, Pose, Quaternion +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration + +from rcl_interfaces.srv import SetParameters +from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive +from std_srvs.srv import Empty + +import tf2_ros +from tf2_geometry_msgs import do_transform_point + +from pymoveit2 import MoveIt2 + + +ARM_JOINTS = [ + "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", + "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", +] +GRIPPER_JOINTS = ["gripper_left_finger_joint", "gripper_right_finger_joint"] +GRIPPER_OPEN = [0.044, 0.044] +GRIPPER_CLOSE = [0.010, 0.010] + +INIT_JOINTS_RIGHT = [ + 0.35, 42 * math.pi / 180, 16 * math.pi / 180, -109 * math.pi / 180, + 105 * math.pi / 180, -60 * math.pi / 180, -56 * math.pi / 180, -108 * math.pi / 180, +] +INIT_JOINTS_LEFT = [ + 0.35, -42 * math.pi / 180, 16 * math.pi / 180, 109 * math.pi / 180, + 105 * math.pi / 180, 60 * math.pi / 180, -56 * math.pi / 180, 108 * math.pi / 180, +] + +DEFAULT_TABLE_POS = [1.3, 0.0, 0.37] +DEFAULT_TABLE_SIZE = [1.2, 1.6, 0.74] + +TUCK_JOINTS = [ + 0.15, 0.20, -1.34, -0.20, 1.94, -1.57, 1.37, 0.00, +] + + +class GraspObject(yasmin.State): + """ + Arm grasp: drive the base into place, STOP, then move only the arm. + + Switched by pick_and_place.grasp.use_moveit: + * False (default, SIM): joint moves go STRAIGHT to /arm_controller + + /torso_controller (no collision check). The sim's MoveIt is broken for + grasping (no IK; the table box blocks the path to objects on it), so direct + execution of the fixed, table-safe poses is the only thing that works. + * True (REAL ROBOT): the same targets are PLANNED + executed by MoveIt + (collision-checked). If the table box blocks planning, set + pick_and_place.grasp.publish_box: false (or shrink the box). + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("selected_object") + self.node = yasmin_ros.logger_node + self._ready = False + self._use_moveit = False + self._moveit = None + self._mn = None + self._joints = {} + + def _setup(self): + if self._ready: + return + self._use_moveit = bool(self._param("pick_and_place.grasp.use_moveit", False)) + yasmin.YASMIN_LOG_INFO( + f"Grasp mode: {'MoveIt (collision-checked)' if self._use_moveit else 'direct controllers'}" + ) + + self._mn = rclpy.create_node("grasp_object_moveit") + + cb = ReentrantCallbackGroup() + self._moveit = MoveIt2( + node=self._mn, + joint_names=ARM_JOINTS, + base_link_name="base_footprint", + end_effector_name="arm_tool_link", + group_name="arm_torso", + callback_group=cb, + ) + self._moveit.planner_id = "RRTConnectkConfigDefault" + self._moveit.max_velocity = 0.3 + self._moveit.max_acceleration = 0.3 + + self._gripper = ActionClient( + self._mn, FollowJointTrajectory, + "/gripper_controller/follow_joint_trajectory", + ) + self._arm_ctrl = ActionClient( + self._mn, FollowJointTrajectory, + "/arm_controller/follow_joint_trajectory", + ) + self._torso_ctrl = ActionClient( + self._mn, FollowJointTrajectory, + "/torso_controller/follow_joint_trajectory", + ) + self._cmd_vel = self._mn.create_publisher(Twist, "/cmd_vel", 10) + self._coll_pub = self._mn.create_publisher(CollisionObject, "/collision_object", 10) + self._clear_octo_cli = self._mn.create_client(Empty, "/clear_octomap") + self._tf = tf2_ros.Buffer(cache_time=rclpy.duration.Duration(seconds=30.0)) + self._tf_listener = tf2_ros.TransformListener(self._tf, self._mn) + self._mn.create_subscription(JointState, "/joint_states", self._js_cb, 10) + + self._mexec = MultiThreadedExecutor() + self._mexec.add_node(self._mn) + self._mthread = Thread(target=self._mexec.spin, daemon=True) + self._mthread.start() + + if self._use_moveit: + self._disable_start_tolerance() + self._ready = True + + def _disable_start_tolerance(self): + try: + cli = self._mn.create_client(SetParameters, "/move_group/set_parameters") + if not cli.wait_for_service(timeout_sec=5.0): + yasmin.YASMIN_LOG_WARN( + "move_group params unavailable; set allowed_start_tolerance by hand." + ) + return + req = SetParameters.Request() + p = Parameter() + p.name = "trajectory_execution.allowed_start_tolerance" + p.value = ParameterValue( + type=ParameterType.PARAMETER_DOUBLE, double_value=0.0 + ) + req.parameters = [p] + cli.call_async(req) + time.sleep(1.0) + yasmin.YASMIN_LOG_INFO("move_group allowed_start_tolerance set to 0.0") + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"Could not set allowed_start_tolerance: {e}") + + def _publish_table_box(self): + pos = list(self._param("pick_and_place.table.collision.position", DEFAULT_TABLE_POS)) + size = list(self._param("pick_and_place.table.collision.size", DEFAULT_TABLE_SIZE)) + co = CollisionObject() + co.header.frame_id = "map" + co.id = "table" + co.operation = CollisionObject.ADD + box = SolidPrimitive() + box.type = SolidPrimitive.BOX + box.dimensions = [float(size[0]), float(size[1]), float(size[2])] + co.primitives.append(box) + p = Pose() + p.position.x, p.position.y, p.position.z = float(pos[0]), float(pos[1]), float(pos[2]) + p.orientation = self._table_orientation() + co.primitive_poses.append(p) + co.pose.orientation.w = 1.0 + for _ in range(3): + self._coll_pub.publish(co) + time.sleep(0.2) + yasmin.YASMIN_LOG_INFO( + f"Published table box (map) pos={pos} size={size} " + f"quat z={p.orientation.z:.3f} w={p.orientation.w:.3f}" + ) + + def _remove_table_box(self): + co = CollisionObject() + co.header.frame_id = "map" + co.id = "table" + co.operation = CollisionObject.REMOVE + for _ in range(3): + self._coll_pub.publish(co) + time.sleep(0.1) + yasmin.YASMIN_LOG_INFO("Removed table box for navigation.") + + def _clear_octomap(self): + try: + if self._clear_octo_cli.wait_for_service(timeout_sec=2.0): + self._clear_octo_cli.call_async(Empty.Request()) + time.sleep(0.4) + except Exception: + pass + + def _js_cb(self, msg): + for n, p in zip(msg.name, msg.position): + self._joints[n] = p + + def _arm_joints(self): + return [self._joints.get(j, 0.0) for j in ARM_JOINTS] + + def _param(self, name, default): + try: + v = self.node.get_parameter(name).value + return v if v is not None else default + except Exception: + return default + + def _quat_param(self, base): + lst = self._param(base, None) + if isinstance(lst, (list, tuple)) and len(lst) == 4: + return [float(v) for v in lst] + x = self._param(base + ".x", None) + y = self._param(base + ".y", None) + z = self._param(base + ".z", None) + w = self._param(base + ".w", None) + if None not in (z, w): + return [float(x or 0.0), float(y or 0.0), float(z), float(w)] + return None + + def _table_orientation(self): + o = self._quat_param("pick_and_place.table.collision.orientation") + if o is None: + o = self._quat_param("pick_and_place.table.pose.orientation") + if o is None: + o = [0.0, 0.0, 0.0, 1.0] + q = Quaternion() + q.x, q.y, q.z, q.w = o[0], o[1], o[2], o[3] + return q + + def _move(self, target, timeout=25.0): + if self._use_moveit: + return self._move_moveit(target, timeout) + return self._move_direct(target, timeout) + + def _move_moveit(self, target, timeout): + """Plan + execute via MoveIt (collision-checked). For the real robot.""" + self._clear_octomap() + self._moveit.move_to_configuration( + joint_positions=list(target), joint_names=ARM_JOINTS + ) + deadline = time.time() + timeout + stable = 0 + err = 99.0 + while time.time() < deadline: + cur = self._arm_joints() + err = max(abs(c - t) for c, t in zip(cur, target)) + if err < 0.06: + stable += 1 + if stable >= 3: + return True + else: + stable = 0 + time.sleep(0.1) + yasmin.YASMIN_LOG_WARN(f"move (moveit) timed out (err={err:.3f} rad)") + return False + + def _move_direct(self, target, timeout): + """Send straight to the controllers (NO collision check). For the sim.""" + self._send_traj(self._arm_ctrl, ARM_JOINTS[1:], target[1:], secs=4) + self._send_traj(self._torso_ctrl, ["torso_lift_joint"], [target[0]], secs=4) + deadline = time.time() + timeout + err = 99.0 + while time.time() < deadline: + cur = self._arm_joints() + err = max(abs(c - t) for c, t in zip(cur, target)) + if err < 0.06: + time.sleep(0.3) + return True + time.sleep(0.1) + yasmin.YASMIN_LOG_WARN(f"move (direct) timed out (err={err:.3f} rad)") + return False + + def _send_traj(self, client, joint_names, positions, secs=4): + if not client.wait_for_server(timeout_sec=5.0): + return False + goal = FollowJointTrajectory.Goal() + traj = JointTrajectory() + traj.joint_names = list(joint_names) + pt = JointTrajectoryPoint() + pt.positions = [float(v) for v in positions] + pt.time_from_start = Duration(sec=int(secs)) + traj.points = [pt] + goal.trajectory = traj + client.send_goal_async(goal) + return True + + def _gripper_cmd(self, positions): + if not self._gripper.wait_for_server(timeout_sec=5.0): + yasmin.YASMIN_LOG_WARN("gripper controller unavailable") + return False + goal = FollowJointTrajectory.Goal() + traj = JointTrajectory() + traj.joint_names = GRIPPER_JOINTS + pt = JointTrajectoryPoint() + pt.positions = list(positions) + pt.time_from_start = Duration(sec=2) + traj.points = [pt] + goal.trajectory = traj + self._gripper.send_goal_async(goal) + time.sleep(3.0) + return True + + def _wait_for_tf(self, timeout=10.0): + t0 = time.time() + while time.time() - t0 < timeout: + try: + self._tf.lookup_transform( + "base_footprint", "map", ROS2Time(), + timeout=rclpy.duration.Duration(seconds=0.5), + ) + return True + except Exception: + time.sleep(0.2) + return False + + def _target_in_base(self, point_map): + try: + tf = self._tf.lookup_transform( + "base_footprint", "map", ROS2Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + ps = PointStamped() + ps.header.frame_id = "map" + ps.header.stamp = tf.header.stamp + ps.point = point_map + return do_transform_point(ps, tf).point + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"TF map->base_footprint failed: {e}") + return None + + def _get_ee(self): + try: + tf = self._tf.lookup_transform( + "base_footprint", "gripper_grasping_frame", ROS2Time(), + timeout=rclpy.duration.Duration(seconds=1.0), + ) + return tf.transform.translation + except Exception: + return None + + def _drive(self, dist, speed=0.08): + if abs(dist) < 1e-3: + return + t = Twist() + t.linear.x = speed if dist > 0 else -speed + end = time.time() + abs(dist) / speed + while time.time() < end: + self._cmd_vel.publish(t) + time.sleep(0.05) + self._cmd_vel.publish(Twist()) + + def execute(self, blackboard) -> str: + if not bool(self._param("pick_and_place.grasp.enable", True)): + yasmin.YASMIN_LOG_INFO("grasp.enable=false — skipping grasp.") + return "succeeded" + + self._setup() + + obj = blackboard["selected_object"] + if obj is None or getattr(obj, "point", None) is None: + yasmin.YASMIN_LOG_WARN("No object/point to grasp.") + return "failed" + + t0 = time.time() + while not self._joints and time.time() - t0 < 5.0: + time.sleep(0.1) + if not self._wait_for_tf(): + yasmin.YASMIN_LOG_WARN("TF not ready — skipping grasp.") + return "failed" + + tb = self._target_in_base(obj.point) + if tb is None: + return "failed" + yasmin.YASMIN_LOG_INFO( + f"Grasp target (base_footprint): x={tb.x:.2f} y={tb.y:.2f} z={tb.z:.2f}" + ) + + reach = float(self._param("pick_and_place.grasp.reach", 0.60)) + max_fwd = float(self._param("pick_and_place.grasp.max_forward", 0.45)) + forward = min(tb.x - reach, max_fwd) + if forward > 0.05: + yasmin.YASMIN_LOG_INFO(f"Grasp: driving base forward {forward:.2f} m") + self._drive(forward) + time.sleep(0.8) + tb = self._target_in_base(obj.point) + if tb is None: + return "failed" + yasmin.YASMIN_LOG_INFO( + f"After approach: target x={tb.x:.2f} y={tb.y:.2f} z={tb.z:.2f}" + ) + + if bool(self._param("pick_and_place.grasp.publish_box", True)): + self._publish_table_box() + self._clear_octomap() + time.sleep(0.5) + + self._gripper_cmd(GRIPPER_OPEN) + config = INIT_JOINTS_LEFT if tb.y > 0.05 else INIT_JOINTS_RIGHT + yasmin.YASMIN_LOG_INFO("Grasp 1/4: pregrasp pose") + self._move(config, timeout=25.0) + + yasmin.YASMIN_LOG_INFO("Grasp 2/4: align sideways") + for _ in range(5): + ee = self._get_ee() + tb2 = self._target_in_base(obj.point) + if ee is None or tb2 is None: + break + dy = tb2.y - ee.y + if abs(dy) < 0.02: + break + joints = self._arm_joints() + joints[1] = max(-1.1, min(1.5, joints[1] + 1.5 * dy)) + self._move(joints, timeout=15.0) + + yasmin.YASMIN_LOG_INFO("Grasp 3/4: lower to object") + ee = self._get_ee() + tb3 = self._target_in_base(obj.point) or tb + if ee is not None: + dz = tb3.z - ee.z + joints = self._arm_joints() + joints[0] = max(0.0, min(0.35, joints[0] + dz)) + self._move(joints, timeout=15.0) + + yasmin.YASMIN_LOG_INFO("Grasp 4/4: close and lift") + self._gripper_cmd(GRIPPER_CLOSE) + time.sleep(1.0) + joints = self._arm_joints() + joints[0] = min(0.35, joints[0] + 0.10) + self._move(joints, timeout=15.0) + + yasmin.YASMIN_LOG_INFO("Grasp 5/5: tuck arm for navigation") + self._move(TUCK_JOINTS, timeout=20.0) + + self._remove_table_box() + + yasmin.YASMIN_LOG_INFO("Grasp complete.") + return "succeeded" \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/instruct_pick.py b/tasks/pick_and_place/pick_and_place/states/instruct_pick.py index 551f569e5..6bf3c408c 100644 --- a/tasks/pick_and_place/pick_and_place/states/instruct_pick.py +++ b/tasks/pick_and_place/pick_and_place/states/instruct_pick.py @@ -1,31 +1,37 @@ import yasmin -import yasmin_ros from lasr_skills import Say class InstructPick(yasmin.State): """ - Instructs the human operator to pick up the selected object. - - Reads selected_object_name from the blackboard and speaks a pick - instruction via TTS. + Instructs the human operator to pick up the selected object, and announces + the robot's classification + intended destination (Communicating Perception). Blackboard inputs: selected_object_name : str + object_category : str + destination_str : str — e.g. "the dishwasher" """ def __init__(self): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("selected_object_name") + self.add_input_key("object_category") + self.add_input_key("destination_str") def execute(self, blackboard) -> str: - name = blackboard["selected_object_name"] + name = blackboard["selected_object_name"] + category = blackboard["object_category"] or "unknown" + destination_str = blackboard["destination_str"] or "its place" - yasmin.YASMIN_LOG_INFO(f"Instructing pick: {name}") + yasmin.YASMIN_LOG_INFO( + f"Instructing pick: {name} ({category}) -> {destination_str}" + ) text = ( f"I have selected the {name}. " + f"I classified it as a {category} item, so it goes to {destination_str}. " f"Please pick it up and hold it ready." ) diff --git a/tasks/pick_and_place/pick_and_place/states/instruct_place.py b/tasks/pick_and_place/pick_and_place/states/instruct_place.py index baf678bd9..6e096fa91 100644 --- a/tasks/pick_and_place/pick_and_place/states/instruct_place.py +++ b/tasks/pick_and_place/pick_and_place/states/instruct_place.py @@ -1,5 +1,4 @@ import yasmin -import yasmin_ros from lasr_skills import Say @@ -8,12 +7,14 @@ class InstructPlace(yasmin.State): """ Instructs the human operator where to place the selected object. - Reads chosen_shelf and chosen_shelf_str from the blackboard. - chosen_shelf_str is a placement hint set by ChooseShelf e.g. - "near the cereal" — if empty the instruction omits the hint. + The destination phrase comes from DecideDestination (the dishwasher / the + trash bin / the cabinet). For cabinet placements, ChooseShelf additionally + provides a shelf id (chosen_shelf) and an optional hint (chosen_shelf_str + e.g. "near the cereal"). Dishwasher / trash placements carry no shelf hint. Blackboard inputs: selected_object_name : str + destination_str : str chosen_shelf : str chosen_shelf_str : str """ @@ -21,29 +22,31 @@ class InstructPlace(yasmin.State): def __init__(self): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("selected_object_name") + self.add_input_key("destination_str") self.add_input_key("chosen_shelf") self.add_input_key("chosen_shelf_str") def execute(self, blackboard) -> str: name = blackboard["selected_object_name"] + destination_str = blackboard["destination_str"] or "its place" chosen_shelf = blackboard["chosen_shelf"] chosen_shelf_str = blackboard["chosen_shelf_str"] + # Build an optional shelf hint (cabinet only). + hint_parts = [] + if chosen_shelf: + hint_parts.append(f"on {chosen_shelf}") + if chosen_shelf_str: + hint_parts.append(chosen_shelf_str) + hint = (", " + ", ".join(hint_parts)) if hint_parts else "" + yasmin.YASMIN_LOG_INFO( - f"Instructing place: {name} on {chosen_shelf} {chosen_shelf_str}" + f"Instructing place: {name} in {destination_str}{hint}" ) - if chosen_shelf_str: - text = ( - f"Please place the {name} on {chosen_shelf}, " - f"{chosen_shelf_str}. " - f"I will give you 5 seconds. 5.. 4.. 3.. 2.. 1.." - ) - else: - text = ( - f"Please place the {name} on {chosen_shelf}. " - f"I will give you 5 seconds. 5.. 4.. 3.. 2.. 1.." - ) + text = ( + f"Please place the {name} in {destination_str}{hint}. " + ) say = Say(text=text) outcome = say.execute(blackboard) diff --git a/tasks/pick_and_place/pick_and_place/states/scan_shelves.py b/tasks/pick_and_place/pick_and_place/states/scan_shelves.py index 87fc9188e..6aca61dd4 100644 --- a/tasks/pick_and_place/pick_and_place/states/scan_shelves.py +++ b/tasks/pick_and_place/pick_and_place/states/scan_shelves.py @@ -9,7 +9,7 @@ from lasr_skills import DetectAllInPolygon from pick_and_place.states.classify_category import ClassifyCategory - +import time class ScanShelves(yasmin.State): """ @@ -61,12 +61,9 @@ def __init__(self): def execute(self, blackboard) -> str: shelf_data = {} - - # Load shelf IDs from params - # TODO: confirm param name matches your yaml try: shelf_ids = ( - self.node.get_parameter("pick_and_place.cabinet.shelves") + self.node.get_parameter("pick_and_place.cabinet.shelf_order") .get_parameter_value() .string_array_value ) @@ -114,8 +111,9 @@ def execute(self, blackboard) -> str: shelf_category = blackboard["shelf_category"] shelf_data[shelf_id] = { - "objects": object_names, - "category": shelf_category, + "objects": [], + "category": "empty", + "category_counts": {}, } yasmin.YASMIN_LOG_INFO( @@ -149,13 +147,13 @@ def _get_shelf_config(self, shelf_id: str) -> bool: header=Header(frame_id="map"), ) - polygon_flat = ( - self.node.get_parameter(f"{prefix}.polygon") - .get_parameter_value() - .double_array_value - ) - coords = list(zip(polygon_flat[::2], polygon_flat[1::2])) - self._current_polygon = ShapelyPolygon(coords) + polygon_points = [ + self.node.get_parameter(f"{prefix}.polygon.top_left").value, + self.node.get_parameter(f"{prefix}.polygon.top_right").value, + self.node.get_parameter(f"{prefix}.polygon.bottom_right").value, + self.node.get_parameter(f"{prefix}.polygon.bottom_left").value, + ] + self._current_polygon = ShapelyPolygon(polygon_points) self._current_z_min = ( self.node.get_parameter(f"{prefix}.z_min") @@ -197,14 +195,16 @@ def _detect_objects(self, shelf_id: str, blackboard) -> list: polygon=self._current_polygon, min_confidence=0.1, # TODO: switch to robocup.pt or your competition model - model="yolo11n-seg.pt", + model="best.pt", ) + time.sleep(2.0) + # DetectAllInPolygon needs these keys initialised blackboard["detected_objects"] = [] - blackboard["debug_images"] = [] + blackboard["debug_images"] = [] - outcome = detector.execute(blackboard) + outcome = detector(blackboard) if outcome == "failed": yasmin.YASMIN_LOG_WARN( @@ -216,4 +216,4 @@ def _detect_objects(self, shelf_id: str, blackboard) -> list: except Exception as e: yasmin.YASMIN_LOG_WARN(f"Detection failed for shelf {shelf_id}: {e}") - return [] \ No newline at end of file + return [] diff --git a/tasks/pick_and_place/pick_and_place/states/scan_shelves_if_needed.py b/tasks/pick_and_place/pick_and_place/states/scan_shelves_if_needed.py new file mode 100644 index 000000000..7d1e8036e --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/scan_shelves_if_needed.py @@ -0,0 +1,41 @@ +import yasmin +import yasmin_ros + +from lasr_skills import Say, GoToLocation + +from pick_and_place.states.detect_objects import DetectObjects +from pick_and_place.states.select_and_visualize_object import SelectAndVisualiseObject +from pick_and_place.states.classify_category import ClassifyCategory +from pick_and_place.states.decide_destination import DecideDestination +from pick_and_place.states.choose_shelf import ChooseShelf +from pick_and_place.states.instruct_pick import InstructPick +from pick_and_place.states.instruct_place import InstructPlace +from pick_and_place.states.detect_trash_floor import DetectFloorTrash +from pick_and_place.states.scan_shelves import ScanShelves + + +class ScanShelvesIfNeeded(yasmin.State): + """ + Runs ScanShelves only on the first cabinet visit. + Skips if shelf_data is already populated or destination is not cabinet. + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "skipped"]) + self.add_input_key("shelf_data") + self.add_input_key("destination") + self._scanner = ScanShelves() + + def execute(self, blackboard) -> str: + # Only scan if going to cabinet + if blackboard["destination"] != "cabinet": + return "skipped" + + # Skip if already scanned on a previous visit + if blackboard["shelf_data"]: + yasmin.YASMIN_LOG_INFO("Shelf data already populated — skipping scan.") + return "skipped" + + yasmin.YASMIN_LOG_INFO("First cabinet visit — scanning shelves.") + outcome = self._scanner.execute(blackboard) + return "succeeded" if outcome == "succeeded" else "skipped" diff --git a/tasks/pick_and_place/pick_and_place/states/select_and_visualize_object.py b/tasks/pick_and_place/pick_and_place/states/select_and_visualize_object.py index 24b8845f2..48619ade3 100644 --- a/tasks/pick_and_place/pick_and_place/states/select_and_visualize_object.py +++ b/tasks/pick_and_place/pick_and_place/states/select_and_visualize_object.py @@ -1,8 +1,7 @@ import cv2 +import rclpy import yasmin import yasmin_ros -from yasmin_ros.yasmin_node import YasminNode -import rclpy from cv_bridge import CvBridge from sensor_msgs.msg import Image from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy @@ -10,78 +9,84 @@ class SelectAndVisualiseObject(yasmin.State): """ - Selects the first object from the detected_objects list, announces it - via TTS, and publishes a debug image with a bounding box to /referee_view - so the referee can confirm the robot's selection. + Pops the next object from the detected_objects list, announces it, and + publishes a debug image with a bounding box to /referee_view so the referee + can confirm the robot's selection. - Ported from ROS 1 SMACH SelectAndVisualiseObject. The three-state machine - (SELECT_OBJECT → SAY_OBJECT → VIS_OBJECT) collapses into a single - yasmin.State since there is no branching between them. + Announce-only: nothing is physically removed from the table, so the loop + iterates the detected list (pop) instead of re-detecting each round. When the + list is empty, every object has been processed → outcome "finished". Blackboard inputs: detected_objects : List[Detection3D] - Output of DetectAllInPolygon — each item has .name, .xywh, - .confidence, and the raw image stored at index [2]. Blackboard outputs: - selected_object : Detection3D — the chosen object - selected_object_name : str — its label, for use in Say format_str + selected_object : Detection3D + selected_object_name : str + object_name : str """ - def __init__(self): - super().__init__(outcomes=["succeeded", "failed"]) + def __init__(self, target_name: str = None): + super().__init__(outcomes=["succeeded", "finished"]) self.add_input_key("detected_objects") self.add_output_key("selected_object") self.add_output_key("selected_object_name") self.add_output_key("object_name") + self._target_name = target_name self.node = yasmin_ros.logger_node self._bridge = CvBridge() + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self._referee_pub = self.node.create_publisher(Image, "/referee_view", qos) - # Latched publisher so the referee view stays visible after publish - qos = QoSProfile( + self._last_image = None + cam_qos = QoSProfile( depth=1, - durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, ) - self._referee_pub = self.node.create_publisher(Image, "/referee_view", qos) - self._last_image = None - cam_qos = QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST) self.node.create_subscription( - Image, "/head_front_camera/rgb/image_raw", self._on_image, cam_qos) - + Image, "/head_front_camera/rgb/image_raw", self._on_image, cam_qos + ) def _on_image(self, msg): self._last_image = msg - - - def execute(self, blackboard) -> str: - # ── 1. Select object ───────────────────────────────────────────────── detected = blackboard["detected_objects"] - if not detected: - yasmin.YASMIN_LOG_WARN("No detected objects to select from.") - return "failed" - - # Always pick the first object — same behaviour as ROS 1 version - selected = detected[0] - blackboard["selected_object"] = selected + # Announce-only mode: nothing is physically removed from the table, + # so the loop iterates this list instead of re-detecting. Empty list + # means every detected object has been processed → we are done. + yasmin.YASMIN_LOG_INFO("No objects left to process — finished.") + return "finished" + + if self._target_name is not None: + selected = next( + (obj for obj in detected if obj.name == self._target_name), None + ) + if selected is None: + yasmin.YASMIN_LOG_WARN( + f"'{self._target_name}' not found in detected_objects." + ) + return "finished" + detected.remove(selected) + else: + # Default behaviour for cleanup loops — always take the first + selected = detected.pop(0) + + blackboard["detected_objects"] = detected + blackboard["selected_object"] = selected blackboard["selected_object_name"] = selected.name blackboard["object_name"] = selected.name - yasmin.YASMIN_LOG_INFO(f"Selected object: {selected.name}") + yasmin.YASMIN_LOG_INFO( + f"Selected object: {selected.name} ({len(detected)} remaining)." + ) # ── 2. Announce to referee ─────────────────────────────────────────── - # Say skill expects blackboard["text"] or is constructed with text= - # Using the node's TTS directly here to avoid needing a sub-state - # TODO: replace with Say skill call if your team prefers consistency yasmin.YASMIN_LOG_INFO( "[TTS] I have selected an object, and it is displayed on my screen. " "Please take a look." ) - # TODO: call Say skill — e.g. - # say = Say(text="I have selected an object...") - # say.execute(blackboard) # ── 3. Publish visualisation ───────────────────────────────────────── self._publish_visualisation(selected) @@ -90,23 +95,25 @@ def execute(self, blackboard) -> str: def _publish_visualisation(self, detection) -> None: try: - # Grab the latest RGB image directly from the camera topic - success, image_msg = rclpy.wait_for_message.wait_for_message( - msg_type=Image, - node=self.node, - topic="/head_front_camera/rgb/image_raw", - time_to_wait=5.0, - ) - - if not success: + # # Grab the latest RGB image directly from the camera topic + # success, image_msg = rclpy.wait_for_message.wait_for_message( + # msg_type=Image, + # node=self.node, + # topic="/head_front_camera/rgb/image_raw", + # time_to_wait=5.0, + # ) + + if self._last_image is None: yasmin.YASMIN_LOG_WARN("Could not get camera image for visualisation.") return - label = detection.name - xywh = detection.xywh + label = detection.name + xywh = detection.xywh confidence = detection.confidence - cv_im = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="rgb8") + cv_im = self._bridge.imgmsg_to_cv2( + self._last_image, desired_encoding="rgb8" + ) cv2.rectangle( cv_im, @@ -131,4 +138,4 @@ def _publish_visualisation(self, detection) -> None: yasmin.YASMIN_LOG_INFO("Published visualisation to /referee_view.") except Exception as e: - yasmin.YASMIN_LOG_WARN(f"Could not publish visualisation: {e}") \ No newline at end of file + yasmin.YASMIN_LOG_WARN(f"Could not publish visualisation: {e}") diff --git a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py new file mode 100644 index 000000000..c262d2071 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -0,0 +1,240 @@ +import yasmin +import yasmin_ros +from lasr_skills import GoToLocation, Say +from pick_and_place.states.detect_objects import DetectObjects +from pick_and_place.states.select_and_visualize_object import SelectAndVisualiseObject +from pick_and_place.states.instruct_pick import InstructPick + + +class ServeBreakfast(yasmin.StateMachine): + """ + Sets up breakfast on the dining table after table cleanup is complete. + + Each item is detected and instructed individually so that if one + item fails to be detected, only that item's detection is retried — + not the whole group. + + Sequence: + GO_TO_BREAKFAST_SURFACE + -> DETECT_BOWL -> SELECT_BOWL -> INSTRUCT_PICK_BOWL + -> DETECT_SPOON -> SELECT_SPOON -> INSTRUCT_PICK_SPOON + -> GO_TO_TABLE + -> INSTRUCT_PLACE_BOWL + -> INSTRUCT_PLACE_SPOON + -> GO_TO_CABINET + -> DETECT_CEREAL -> SELECT_CEREAL -> INSTRUCT_PICK_CEREAL + -> DETECT_MILK -> SELECT_MILK -> INSTRUCT_PICK_MILK + -> GO_TO_TABLE + -> INSTRUCT_PLACE_CEREAL + -> INSTRUCT_PLACE_MILK + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + + # Navigate to breakfast surface + self.add_state( + "GO_TO_BREAKFAST_SURFACE", + GoToLocation(location_param="pick_and_place.breakfast_surface.pose"), + transitions={ + "succeeded": "DETECT_BOWL", + "failed": "GO_TO_BREAKFAST_SURFACE", + }, + ) + + # Bowl + self.add_state( + "DETECT_BOWL", + DetectObjects( + location_param="breakfast_surface", + object_filter=["bowl"], + model="best.pt", + ), + transitions={ + "succeeded": "SELECT_BOWL", + "failed": "DETECT_BOWL", + }, + ) + + self.add_state( + "SELECT_BOWL", + SelectAndVisualiseObject(target_name="bowl"), + transitions={ + "succeeded": "INSTRUCT_PICK_BOWL", + "finished": "DETECT_BOWL", # not found, retry detection + }, + ) + + self.add_state( + "INSTRUCT_PICK_BOWL", + Say(text="I have detected a bowl. Please pick it up and hold it ready."), + transitions={ + "succeeded": "DETECT_SPOON", + "aborted": "INSTRUCT_PICK_BOWL", + "canceled": "INSTRUCT_PICK_BOWL", + }, + ) + + # Spoon + self.add_state( + "DETECT_SPOON", + DetectObjects( + location_param="breakfast_surface", + object_filter=["spoon"], + model="best.pt", + ), + transitions={ + "succeeded": "SELECT_SPOON", + "failed": "DETECT_SPOON", + }, + ) + + self.add_state( + "SELECT_SPOON", + SelectAndVisualiseObject(target_name="spoon"), + transitions={ + "succeeded": "INSTRUCT_PICK_SPOON", + "finished": "DETECT_SPOON", # not found, retry detection + }, + ) + + self.add_state( + "INSTRUCT_PICK_SPOON", + Say(text="I have detected a spoon. Please pick it up and hold it ready."), + transitions={ + "succeeded": "GO_TO_TABLE_1", + "canceled": "INSTRUCT_PICK_SPOON", + }, + ) + + # Navigate to table, place bowl and spoon + self.add_state( + "GO_TO_TABLE_1", + GoToLocation(location_param="pick_and_place.table.pose"), + transitions={ + "succeeded": "INSTRUCT_PLACE_BOWL", + "failed": "GO_TO_TABLE_1", + }, + ) + + self.add_state( + "INSTRUCT_PLACE_BOWL", + Say(text="Please place the bowl in the centre of the table."), + transitions={ + "succeeded": "INSTRUCT_PLACE_SPOON", + "aborted": "INSTRUCT_PLACE_SPOON", + "canceled": "INSTRUCT_PLACE_SPOON", + }, + ) + self.add_state( + "INSTRUCT_PLACE_SPOON", + Say(text="Please place the spoon next to the bowl."), + transitions={ + "succeeded": "GO_TO_CABINET", + "aborted": "GO_TO_CABINET", + "canceled": "GO_TO_CABINET", + }, + ) + + # Navigate to cabinet + self.add_state( + "GO_TO_CABINET", + GoToLocation(location_param="pick_and_place.cabinet.pose"), + transitions={ + "succeeded": "DETECT_CEREAL", + "failed": "GO_TO_CABINET", + }, + ) + + # Cereal + self.add_state( + "DETECT_CEREAL", + DetectObjects( + location_param="cabinet", + object_filter=["cereal"], + model="best.pt", + ), + transitions={ + "succeeded": "SELECT_CEREAL", + "failed": "DETECT_CEREAL", + }, + ) + self.add_state( + "SELECT_CEREAL", + SelectAndVisualiseObject(target_name="cereal"), + transitions={ + "succeeded": "INSTRUCT_PICK_CEREAL", + "finished": "DETECT_CEREAL", # not found, retry detection + }, + ) + self.add_state( + "INSTRUCT_PICK_CEREAL", + Say(text="I have detected cereal. Please pick it up and hold it ready."), + transitions={ + "succeeded": "DETECT_MILK", + "canceled": "INSTRUCT_PICK_CEREAL", + }, + ) + + # Milk + self.add_state( + "DETECT_MILK", + DetectObjects( + location_param="cabinet", object_filter=["milk"], model="best.pt" + ), + transitions={ + "succeeded": "SELECT_MILK", + "failed": "DETECT_MILK", + }, + ) + self.add_state( + "SELECT_MILK", + SelectAndVisualiseObject(target_name="milk"), + transitions={ + "succeeded": "INSTRUCT_PICK_MILK", + "finished": "DETECT_MILK", # not found, retry detection + }, + ) + self.add_state( + "INSTRUCT_PICK_MILK", + Say(text="I have detected milk. Please pick it up and hold it ready."), + transitions={ + "succeeded": "GO_TO_TABLE_2", + "canceled": "INSTRUCT_PICK_MILK", + }, + ) + + # Navigate to table, place cereal and milk + self.add_state( + "GO_TO_TABLE_2", + GoToLocation(location_param="pick_and_place.table.pose"), + transitions={ + "succeeded": "INSTRUCT_PLACE_CEREAL", + "failed": "GO_TO_TABLE_2", + }, + ) + + self.add_state( + "INSTRUCT_PLACE_CEREAL", + Say( + text="Please place the cereal next to the bowl, " + "with sufficient space between them." + ), + transitions={ + "succeeded": "INSTRUCT_PLACE_MILK", + "aborted": "INSTRUCT_PLACE_MILK", + "canceled": "INSTRUCT_PLACE_MILK", + }, + ) + self.add_state( + "INSTRUCT_PLACE_MILK", + Say( + text="Please place the milk next to the cereal, " + "with sufficient space between them." + ), + transitions={ + "succeeded": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", + }, + ) diff --git a/tasks/pick_and_place/pick_and_place/states/start.py b/tasks/pick_and_place/pick_and_place/states/start.py index 5a00f529c..6fe8ef0f4 100644 --- a/tasks/pick_and_place/pick_and_place/states/start.py +++ b/tasks/pick_and_place/pick_and_place/states/start.py @@ -2,7 +2,7 @@ import yasmin_ros from std_msgs.msg import Empty -from lasr_skills import Say, GoToLocation, DetectDoorOpening +from lasr_skills import Say, GoToLocation, StartDoorSM class Start(yasmin.StateMachine): @@ -42,8 +42,8 @@ def wait_cb(blackboard, msg): ), transitions={ "succeeded": "SAY_START", - "failed": "WAIT_START", - "canceled": "failed", + "failed": "WAIT_START", + "canceled": "failed", }, ) @@ -52,66 +52,50 @@ def wait_cb(blackboard, msg): "SAY_START", Say(text="Start of Pick and Place task."), transitions={ - "succeeded": "SAY_WAITING", - "aborted": "SAY_WAITING", - "canceled": "SAY_WAITING", - }, - ) - - # 3. Say waiting for door - self.add_state( - "SAY_WAITING", - Say(text="Waiting for the door to open."), - transitions={ - "succeeded": "SAY_GOING_TO_TABLE", - "aborted": "SAY_GOING_TO_TABLE", - "canceled": "SAY_GOING_TO_TABLE", + "succeeded": "WAIT_FOR_DOOR", + "aborted": "WAIT_FOR_DOOR", + "canceled": "WAIT_FOR_DOOR", }, ) # 4. Detect door opening self.add_state( "WAIT_FOR_DOOR", - DetectDoorOpening(timeout=1.0), + StartDoorSM(), transitions={ - "door_opened": "SAY_GOING_TO_TABLE", - "failed": "WAIT_FOR_DOOR", + "succeeded": "SAY_GOING_TO_TABLE", + "failed": "SAY_GOING_TO_TABLE", # FIX THIS ON THE REAL ROBOT }, ) - # 5. Announce navigation + # 5. Announce navigatGO_TO_TRASH_BIN_FLOORion self.add_state( "SAY_GOING_TO_TABLE", Say(text="I am going to the table."), transitions={ - "succeeded": "GO_TO_TABLE", - "aborted": "GO_TO_TABLE", - "canceled": "GO_TO_TABLE", + "succeeded": "ASK_OPEN_CABINET", + "aborted": "ASK_OPEN_CABINET", + "canceled": "ASK_OPEN_CABINET", }, ) - # 6. Navigate to table + # # 6. Navigate to table self.add_state( "GO_TO_TABLE", GoToLocation(location_param="pick_and_place.table.pose"), transitions={ "succeeded": "ASK_OPEN_CABINET", - "failed": "ASK_OPEN_CABINET", + "failed": "ASK_OPEN_CABINET", }, ) # 7. Ask referee to open cabinet self.add_state( "ASK_OPEN_CABINET", - Say( - text="Referee, I am unable to open the cabinet doors. " - "Please open them for me. " - "I will give you 5 seconds. 5.. 4.. 3.. 2.. 1.." - ), + Say(text=""), transitions={ "succeeded": "succeeded", - "aborted": "succeeded", + "aborted": "succeeded", "canceled": "succeeded", - }, - ) \ No newline at end of file + ) diff --git a/tasks/pick_and_place/pick_and_place/states/table_cleanup.py b/tasks/pick_and_place/pick_and_place/states/table_cleanup.py new file mode 100644 index 000000000..3f45acc88 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/table_cleanup.py @@ -0,0 +1,287 @@ +import yasmin +import yasmin_ros + +from lasr_skills import Say, GoToLocation + +from pick_and_place.states.detect_objects import DetectObjects +from pick_and_place.states.select_and_visualize_object import SelectAndVisualiseObject +from pick_and_place.states.classify_category import ClassifyCategory +from pick_and_place.states.decide_destination import DecideDestination +from pick_and_place.states.choose_shelf import ChooseShelf +from pick_and_place.states.instruct_pick import InstructPick +from pick_and_place.states.instruct_place import InstructPlace +from pick_and_place.states.detect_trash_floor import DetectFloorTrash +from pick_and_place.states.scan_shelves_if_needed import ScanShelvesIfNeeded + + +class TableCleanup(yasmin.StateMachine): + """ + Cleans the dining table: detects all objects once, then loops through + each one — classify, decide destination (dishwasher / trash bin / + cabinet), choose shelf if cabinet-bound, instruct pick, navigate, + instruct place, navigate back for the next object. + + After the table is clear, also checks the floor near the trash bin + for the optional floor trash item (rulebook +30 bonus). + + Sequence: + GO_TO_TABLE_FOR_PICK + -> DETECT_OBJECTS (done once) + ┌-> SELECT_OBJECT (empty -> SAY_CLEANUP_DONE) + │ -> CLASSIFY_CATEGORY + │ -> DECIDE_DESTINATION + │ ├ cabinet -> CHOOSE_SHELF + │ └ other ───────────┐ + │ -> INSTRUCT_PICK <───────┘ + │ -> GO_TO_DESTINATION + │ -> INSTRUCT_PLACE + │ -> GO_TO_TABLE + └─────(loop) + -> SAY_CLEANUP_DONE + -> GO_TO_TRASH_BIN_FLOOR + -> DETECT_FLOOR_TRASH (optional, skips if nothing found) + -> SELECT_FLOOR_TRASH -> INSTRUCT_PICK_FLOOR -> INSTRUCT_PLACE_FLOOR + -> succeeded + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + + # Navigate to table + self.add_state( + "GO_TO_TABLE_FOR_PICK", + GoToLocation(location_param="pick_and_place.table.pose"), + transitions={ + "succeeded": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", + }, + ) + + # Detect all objects on the table (done ONCE) + self.add_state( + "DETECT_OBJECTS", + DetectObjects(location_param="table", model="best.pt"), + transitions={ + "succeeded": "SELECT_OBJECT", + "failed": "DETECT_OBJECTS", + }, + ) + + # Select next object and visualise for referee + self.add_state( + "SELECT_OBJECT", + SelectAndVisualiseObject(), + transitions={ + "succeeded": "CLASSIFY_CATEGORY", + "finished": "CLOSE_DISHWASHER_IF_OPENED", # ← change from SAY_CLEANUP_DONE + }, + ) + + # Close dishwasher if it was opened + self.add_state( + "CLOSE_DISHWASHER_IF_OPENED", + CloseDishwasherIfOpened(), + transitions={ + "succeeded": "SAY_CLEANUP_DONE", + "skipped": "SAY_CLEANUP_DONE", + }, + ) + + # Classify selected object into a category + self.add_state( + "CLASSIFY_CATEGORY", + ClassifyCategory(task="object"), + transitions={ + "succeeded": "DECIDE_DESTINATION", + "failed": "DECIDE_DESTINATION", + "empty": "SELECT_OBJECT", + }, + ) + + # Decide destination: dishwasher / trash bin / cabinet + self.add_state( + "DECIDE_DESTINATION", + DecideDestination(), + transitions={ + "cabinet": "CHOOSE_SHELF", + "other": "INSTRUCT_PICK", + }, + ) + + # Choose which cabinet shelf to place object on + self.add_state( + "CHOOSE_SHELF", + ChooseShelf(), + transitions={ + "succeeded": "INSTRUCT_PICK", + "failed": "INSTRUCT_PICK", + }, + ) + + # Instruct operator to pick up object + self.add_state( + "INSTRUCT_PICK", + InstructPick(), + transitions={ + "succeeded": "GO_TO_DESTINATION", + "failed": "INSTRUCT_PICK", + }, + ) + + # Navigate to the chosen destination + self.add_state( + "GO_TO_DESTINATION", + GoToLocation(), # reads blackboard["location"] + transitions={ + "succeeded": "OPEN_DISHWASHER_IF_NEEDED", + "failed": "OPEN_DISHWASHER_IF_NEEDED", + }, + ) + + # Open dishwasher on first dish visit + self.add_state( + "OPEN_DISHWASHER_IF_NEEDED", + OpenDishwasherIfNeeded(), + transitions={ + "succeeded": "SCAN_SHELVES_IF_NEEDED", + "skipped": "SCAN_SHELVES_IF_NEEDED", + }, + ) + + # Scan shelves on first cabinet visit + self.add_state( + "SCAN_SHELVES_IF_NEEDED", + ScanShelvesIfNeeded(), + transitions={ + "succeeded": "INSTRUCT_PLACE", + "skipped": "INSTRUCT_PLACE", + }, + ) + + # Instruct operator where to place object + self.add_state( + "INSTRUCT_PLACE", + InstructPlace(), + transitions={ + "succeeded": "GO_TO_TABLE", + "failed": "INSTRUCT_PLACE", + }, + ) + + # Navigate back to table for next object + self.add_state( + "GO_TO_TABLE", + GoToLocation(location_param="pick_and_place.table.pose"), + transitions={ + "succeeded": "SELECT_OBJECT", + "failed": "GO_TO_TABLE", + }, + ) + + # Table done, check the floor near the trash bin + self.add_state( + "SAY_CLEANUP_DONE", + Say( + text="I have finished cleaning the table. " + "Let me check the floor near the trash bin." + ), + transitions={ + "succeeded": "GO_TO_TRASH_BIN_FLOOR", + "aborted": "GO_TO_TRASH_BIN_FLOOR", + "canceled": "GO_TO_TRASH_BIN_FLOOR", + }, + ) + + self.add_state( + "GO_TO_TRASH_BIN_FLOOR", + GoToLocation(location_param="pick_and_place.trash_bin.pose"), + transitions={ + "succeeded": "DETECT_FLOOR_TRASH", + "failed": "DETECT_FLOOR_TRASH", + }, + ) + + self.add_state( + "DETECT_FLOOR_TRASH", + DetectFloorTrash(), + transitions={ + "succeeded": "SET_FLOOR_TRASH_CONTEXT", + "failed": "succeeded", # nothing found, floor trash optional + }, + ) + + self.add_state( + "SELECT_FLOOR_TRASH", + SelectAndVisualiseObject(), + transitions={ + "succeeded": "INSTRUCT_PICK_FLOOR", + "finished": "succeeded", + }, + ) + + self.add_state( + "SET_FLOOR_TRASH_CONTEXT", + yasmin.CbState( + outcomes=["succeeded"], + callback=lambda bb: [ + bb.__setitem__("object_category", "trash"), + bb.__setitem__("destination_str", "the trash bin"), + bb.__setitem__("chosen_shelf", ""), + bb.__setitem__("chosen_shelf_str", ""), + ] + and "succeeded", + ), + transitions={"succeeded": "INSTRUCT_PICK_FLOOR"}, + ) + + self.add_state( + "INSTRUCT_PICK_FLOOR", + InstructPick(), + transitions={ + "succeeded": "INSTRUCT_PLACE_FLOOR", + "failed": "INSTRUCT_PICK_FLOOR", + }, + ) + + self.add_state( + "INSTRUCT_PLACE_FLOOR", + Say(text="Please place it in the trash bin."), + transitions={ + "succeeded": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", + }, + ) + + +class OpenDishwasherIfNeeded(yasmin.State): + """Says open dishwasher only on first dish item visit.""" + + def __init__(self): + super().__init__(outcomes=["succeeded", "skipped"]) + self.add_input_key("destination") + self.add_input_key("dishwasher_opened") + self.add_output_key("dishwasher_opened") + + def execute(self, blackboard) -> str: + if blackboard["destination"] != "dishwasher": + return "skipped" + if blackboard["dishwasher_opened"]: + return "skipped" + say = Say(text="Please open the dishwasher.") + say.execute(blackboard) + blackboard["dishwasher_opened"] = True + return "succeeded" + + +class CloseDishwasherIfOpened(yasmin.State): + def __init__(self): + super().__init__(outcomes=["succeeded", "skipped"]) + self.add_input_key("dishwasher_opened") + + def execute(self, blackboard) -> str: + if not blackboard["dishwasher_opened"]: + return "skipped" + say = Say(text="Please close the dishwasher.") + say.execute(blackboard) + return "succeeded" diff --git a/tasks/pick_and_place/pick_and_place/test_detect.py b/tasks/pick_and_place/pick_and_place/test_detect.py deleted file mode 100644 index cf0349983..000000000 --- a/tasks/pick_and_place/pick_and_place/test_detect.py +++ /dev/null @@ -1,25 +0,0 @@ -import rclpy -import yasmin -import yasmin_ros -from pick_and_place.states.detect_objects import DetectObjects - -def main(): - rclpy.init() - yasmin_ros.set_ros_loggers() - - bb = yasmin.Blackboard() - bb["detected_objects"] = [] - bb["debug_images"] = [] - - state = DetectObjects() - outcome = state.execute(bb) - - print(f"Outcome: {outcome}") - print(f"Detected {len(bb['detected_objects'])} objects:") - for obj in bb["detected_objects"]: - print(f" - {obj.name} (confidence: {obj.confidence:.2f})") - - rclpy.shutdown() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py b/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py new file mode 100644 index 000000000..6de1203c6 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +import rclpy +import yasmin +import yasmin_ros +from pick_and_place.states.serve_breakfast import ServeBreakfast + + +from pick_and_place.state_machine import PickAndPlaceNode + +def main(): + + rclpy.init() + + node = PickAndPlaceNode() # has allow_undeclared_parameters=True + + yasmin_ros.set_ros_loggers(node) + + bb = yasmin.Blackboard() + + bb["detected_objects"] = [] + + bb["selected_object"] = None + + bb["selected_object_name"] = "" + + bb["object_name"] = "" + + bb["object_category"] = "breakfast" + + bb["destination_str"] = "the dining table" + + bb["chosen_shelf"] = "" + + bb["chosen_shelf_str"] = "" + + bb["last_rgb_image"] = None + + bb["debug_images"] = [] + + sm = ServeBreakfast() + + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(f"ServeBreakfast finished with outcome: {outcome}") + + if rclpy.ok(): + + node.destroy_node() + + rclpy.shutdown() + \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/vlm_classifier.py b/tasks/pick_and_place/pick_and_place/vlm_classifier.py new file mode 100644 index 000000000..273d2b40e --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/vlm_classifier.py @@ -0,0 +1,79 @@ +"""Name a detected object crop with a local VLM (Ollama, e.g. gemma3:4b). +Talks to Ollama HTTP API via stdlib urllib — no extra deps, no lasr_vlm venv. +Ollama must be running at `host`; the model auto-pulls on first use.""" +import json +import base64 +import urllib.request + +import cv2 + +# Specific product labels the VLM must choose from — EDIT for your items. +CANDIDATES = [ + "iced tea", "water bottle", "coke can", "sprite can", "pringles", "fork", "knife", "spoon", + "red bull", "apple", "banana", "cup", "mug", "cereal", "bowl", "sponge", "unknown", +] + +_PROMPT = ( + "You are labelling ONE grocery item for a robot. Look at the image and reply " + "with EXACTLY ONE label from this list, lowercase, and nothing else:\n" + "{labels}\n" + "If none clearly fits, reply 'unknown'." +) + +def classify_crop( + rgb_bgr, + box_xywh_center, + candidates=None, + *, + model="gemma3:4b", + host="http://localhost:11434", + timeout=60.0, + pad=0.12, +): + """VLM label for the crop at box (cx,cy,w,h) CENTRE in rgb_bgr, or None.""" + cands = candidates or CANDIDATES + h_img, w_img = rgb_bgr.shape[:2] + cx, cy, w, h = box_xywh_center + px, py = w * pad, h * pad + x1 = int(max(0, cx - w / 2 - px)) + y1 = int(max(0, cy - h / 2 - py)) + x2 = int(min(w_img, cx + w / 2 + px)) + y2 = int(min(h_img, cy + h / 2 + py)) + if x2 <= x1 or y2 <= y1: + return None + crop = rgb_bgr[y1:y2, x1:x2] + + ok, buf = cv2.imencode(".jpg", crop) + if not ok: + return None + img_b64 = base64.b64encode(buf.tobytes()).decode("utf-8") + payload = { + "model": model, + "messages": [{ + "role": "user", + "content": _PROMPT.format(labels=", ".join(cands)), + "images": [img_b64], + }], + "stream": False, + "options": {"temperature": 0.0}, + } + req = urllib.request.Request( + host.rstrip("/") + "/api/chat", + data=json.dumps(payload).encode("utf-8"), + headers={"Content-Type": "application/json"}, + ) + try: + with urllib.request.urlopen(req, timeout=timeout) as r: + resp = json.loads(r.read().decode("utf-8")) + text = (resp.get("message", {}).get("content", "") or "").strip().lower() + except Exception: + return None + if not text: + return None + for c in cands: + if text == c: + return c + for c in cands: + if c in text or text in c: + return c + return text.split("\n")[0][:40] \ No newline at end of file diff --git a/tasks/pick_and_place/setup.py b/tasks/pick_and_place/setup.py index e82bef3c2..157b3b9f3 100644 --- a/tasks/pick_and_place/setup.py +++ b/tasks/pick_and_place/setup.py @@ -1,33 +1,36 @@ from setuptools import find_packages, setup -package_name = 'pick_and_place' +package_name = "pick_and_place" setup( name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), + version="0.0.0", + packages=find_packages(exclude=["test"]), data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/config', ['config/config.yaml']), - ('share/' + package_name + '/launch', ['launch/pick_and_place.launch.py']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", ["config/config.yaml"]), + ("share/" + package_name + "/launch", ["launch/pick_and_place.launch.py"]), + ("share/" + package_name + "/launch", ["launch/serve_breakfast.launch.py"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='yara', - maintainer_email='yaralkhelaiwi@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + maintainer="yara", + maintainer_email="yaralkhelaiwi@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", extras_require={ - 'test': [ - 'pytest', + "test": [ + "pytest", ], }, entry_points={ - 'console_scripts': [ + "console_scripts": [ "state_machine = pick_and_place.state_machine:main", - "test_detect = pick_and_place.test_detect:main", + "test_serve_breakfast = pick_and_place.test_serve_breakfast:main", "point_head_stub = pick_and_place.point_head_stub:main", + "detect_tuner = pick_and_place.detect_tuner:main", + "test_shelf_logic = pick_and_place.shelf:main", ], }, )