From 5a2f7b686977fdf3ea9e9d001023ddea64cee371 Mon Sep 17 00:00:00 2001 From: Fadi <130671609+Fadi-Mostefai@users.noreply.github.com> Date: Sun, 14 Jun 2026 12:39:32 +0100 Subject: [PATCH 01/21] HRI Task - SM1-3 fully tested and working on the robot (#443) * Testing HRI SM2 * LLM package with venv * Fixed SM3 * RVIZ debug for HRI testing * Fixed lasr_llm by removing broken llama_cpp_python * Successfull SM2 of HRI * WIP - SM2 to SM3 * WIP: CHECKS * WIP - SM1 -> SM3 Final checks to see if it works consistently needs to be carried out * Ported recieve Object * Temporary detect3d fix * Working SM1-3 of HRI --------- Co-authored-by: Aldrich-Fernandes --- common/language/lasr_llm/See | 0 .../lasr_llm/lasr_llm/llm_inference.py | 10 +- .../lasr_llm/nodes/hri_task_service.py | 2 +- .../language/lasr_llm/lasr_llm/nodes/llm.py | 80 -- common/language/lasr_llm/package.xml | 6 +- common/language/lasr_llm/requirements.in | 4 +- common/language/lasr_llm/requirements.txt | 152 +--- common/language/lasr_llm/setup.py | 24 +- .../lasr_llm_interfaces/CMakeLists.txt | 1 - .../language/lasr_llm_interfaces/srv/Llm.srv | 5 - .../transcribe_microphone_server.py | 2 +- .../cropped_detection.py | 55 +- .../eye_tracker_action_server.py | 45 +- .../action/EyeTracker.action | 2 + .../lasr_vision_reid/relay_3d.py | 2 +- common/vision/lasr_vision_yolo/setup.py | 2 - skills/setup.py | 2 + skills/src/lasr_skills/ask_and_listen.py | 18 + skills/src/lasr_skills/describe_people.py | 38 +- skills/src/lasr_skills/detect_3d.py | 55 +- skills/src/lasr_skills/detect_3d_in_area.py | 18 +- .../src/lasr_skills/detect_all_in_polygon.py | 139 ++- skills/src/lasr_skills/detect_door_opening.py | 7 +- skills/src/lasr_skills/eye_tracker.py | 22 +- skills/src/lasr_skills/go_to_location.py | 12 +- skills/src/lasr_skills/look_to_point.py | 7 +- skills/src/lasr_skills/receive_object.py | 383 ++++---- .../src/lasr_skills/vision/crop_image_3d.py | 46 +- skills/src/lasr_skills/vision/get_image.py | 61 +- .../lasr_skills/wait_for_person_in_area.py | 7 +- tasks/HRI/HRI/state_machine.py | 107 ++- tasks/HRI/HRI/states/get_attributes.py | 6 +- tasks/HRI/HRI/states/get_name_and_drink.py | 20 +- tasks/HRI/HRI/states/greet.py | 3 +- tasks/HRI/HRI/states/hri_learn_faces.py | 4 +- tasks/HRI/HRI/states/learn_host_face.py | 2 +- tasks/HRI/HRI/states/seat_guest.py | 82 +- tasks/HRI/HRI/states/start_door_sm.py | 2 +- tasks/HRI/config/debug.rviz | 829 ++++++++++++++++++ tasks/HRI/config/lab.yaml | 54 +- tasks/HRI/launch/HRI.launch.py | 17 + 41 files changed, 1615 insertions(+), 718 deletions(-) delete mode 100644 common/language/lasr_llm/See delete mode 100644 common/language/lasr_llm/lasr_llm/nodes/llm.py delete mode 100644 common/language/lasr_llm_interfaces/srv/Llm.srv create mode 100644 tasks/HRI/config/debug.rviz diff --git a/common/language/lasr_llm/See b/common/language/lasr_llm/See deleted file mode 100644 index e69de29bb..000000000 diff --git a/common/language/lasr_llm/lasr_llm/llm_inference.py b/common/language/lasr_llm/lasr_llm/llm_inference.py index e6ef7f33c..84d51a55a 100644 --- a/common/language/lasr_llm/lasr_llm/llm_inference.py +++ b/common/language/lasr_llm/lasr_llm/llm_inference.py @@ -19,7 +19,6 @@ ) import torch -import os import json from datetime import datetime @@ -29,6 +28,10 @@ parse_llm_output_to_dict, ) +import os + +here = os.path.dirname(os.path.abspath(__file__)) + @dataclass class ModelConfig: @@ -68,7 +71,8 @@ def __init__(self, model_config: ModelConfig): print(f"Using device: {self.device}") self.model_name = self.config.model_name - self.tokenizer = AutoTokenizer.from_pretrained(self.model_name) + cache_dir='/home/fadi/.cache/huggingface/hub' + self.tokenizer = AutoTokenizer.from_pretrained(self.model_name, local_files_only=True) self.logger = logging.getLogger(__name__) @@ -140,7 +144,7 @@ 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 + self.model_name, low_cpu_mem_usage=True, local_files_only=True ) kwargs = {"low_cpu_mem_usage": True} 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 0315ef1ce..1a7f3a189 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") + config = ModelConfig(model_name="Qwen/Qwen2.5-1.5B", model_type="llm", quantize=False) self.llm_inference = LLMInference(config) self.get_logger().info("HRI Task Query LLM service started") diff --git a/common/language/lasr_llm/lasr_llm/nodes/llm.py b/common/language/lasr_llm/lasr_llm/nodes/llm.py deleted file mode 100644 index 087688d50..000000000 --- a/common/language/lasr_llm/lasr_llm/nodes/llm.py +++ /dev/null @@ -1,80 +0,0 @@ -from typing import Dict, Any - -import rclpy -from rclpy.node import Node -from llama_cpp import Llama -from lasr_llm_interfaces.srv import Llm -import timeit - - -# From https://stackoverflow.com/questions/14452145/how-to-measure-time-taken-between-lines-of-code-in-python -class CodeTimer: - def __init__(self, name=None): - self.name = " '" + name + "'" if name else "" - - def __enter__(self): - self.start = timeit.default_timer() - - def __exit__(self, exc_type, exc_value, traceback): - self.took = (timeit.default_timer() - self.start) * 1000.0 - print("Code block" + self.name + " took: " + str(self.took) + " ms") - - -class LLMService(Node): - - _model: Llama - - def __init__(self): - super().__init__("lasr_llm") - - self._model = Llama.from_pretrained( - repo_id="microsoft/Phi-3-mini-4k-instruct-gguf", - verbose=False, - filename="*q4.gguf", - n_ctx=4096, # Context length - n_gpu_layers=-1, # Use all available GPU layers - ) - - # Warm up the model - with CodeTimer("LLM Warmup"): - self._model( - "You are a robot acting as a party host. You are tasked with identifying the name and interest belonging to a guest. The possible names are John, Charlie, Axel, Matt, Jared, Ben,, Siyao, Albert, Robert, Grace, Freya, George, Siyao. You will receive input such as my name is john and I like robotics. Output only the name and interest, e.g., john, robotics. Make sure that the interest is only one or two words. If you cant identify the name or interest output unkown, e.g. john, unkown. The user says:", - max_tokens=10, - stop=["<|end|>"], - echo=False, - ) - - self._service = self.create_service(Llm, "/lasr_llm/llm", self._llm) - self.get_logger().info("/lasr_llm/llm service is ready!") - - def _llm(self, request, response): - with CodeTimer("LLM Request"): - prompt = f"{request.system_prompt} The user says: {request.prompt}" - self.get_logger().info(f"Prompting LLM with prompt:\n {prompt}") - llm_output = self._model( - f"<|user|>\n{prompt}<|end|>\n<|assistant|>", - max_tokens=request.max_tokens, - stop=["<|end|>"], - echo=False, - ) - self.get_logger().info(f"LLM Output:\n {llm_output}") - response.output = llm_output["choices"][0]["text"] - - return response - - -def main(args=None): - rclpy.init(args=args) - node = LLMService() - - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/common/language/lasr_llm/package.xml b/common/language/lasr_llm/package.xml index 6b9e4cea1..01193c1fe 100644 --- a/common/language/lasr_llm/package.xml +++ b/common/language/lasr_llm/package.xml @@ -11,16 +11,14 @@ rclpy std_msgs - - rosidl_default_generators - rosidl_default_runtime - ament_copyright ament_flake8 ament_pep257 python3-pytest + ament_virtualenv ament_python + requirements.txt diff --git a/common/language/lasr_llm/requirements.in b/common/language/lasr_llm/requirements.in index 872a1f45a..98e4207f3 100644 --- a/common/language/lasr_llm/requirements.in +++ b/common/language/lasr_llm/requirements.in @@ -1,7 +1,5 @@ -numpy +numpy<2.0 transformers>=4.44,<4.46 accelerate>=0.33,<1.0 bitsandbytes>=0.49 torch -llama_cpp_python==0.3.9 -huggingface-hub \ No newline at end of file diff --git a/common/language/lasr_llm/requirements.txt b/common/language/lasr_llm/requirements.txt index 89f763a96..98e4207f3 100644 --- a/common/language/lasr_llm/requirements.txt +++ b/common/language/lasr_llm/requirements.txt @@ -1,147 +1,5 @@ -# -# This file is autogenerated by pip-compile with Python 3.13 -# by the following command: -# -# pip-compile requirements.in -# -accelerate==0.34.2 - # via -r requirements.in -bitsandbytes==0.49.2 - # via -r requirements.in -certifi==2025.4.26 - # via requests -charset-normalizer==3.4.2 - # via requests -cuda-bindings==12.9.4 - # via torch -cuda-pathfinder==1.4.2 - # via cuda-bindings -diskcache==5.6.3 - # via llama-cpp-python -filelock==3.18.0 - # via - # huggingface-hub - # torch - # transformers -fsspec==2025.5.1 - # via - # huggingface-hub - # torch -hf-xet==1.1.3 - # via huggingface-hub -huggingface-hub==0.33.0 - # via - # -r requirements.in - # accelerate - # tokenizers - # transformers -idna==3.10 - # via requests -jinja2==3.1.6 - # via - # llama-cpp-python - # torch -llama-cpp-python==0.3.9 - # via -r requirements.in -markupsafe==3.0.2 - # via jinja2 -mpmath==1.3.0 - # via sympy -networkx==3.6.1 - # via torch -numpy==2.2.6 - # via - # -r requirements.in - # accelerate - # bitsandbytes - # llama-cpp-python - # transformers -nvidia-cublas-cu12==12.8.4.1 - # via - # nvidia-cudnn-cu12 - # nvidia-cusolver-cu12 - # torch -nvidia-cuda-cupti-cu12==12.8.90 - # via torch -nvidia-cuda-nvrtc-cu12==12.8.93 - # via torch -nvidia-cuda-runtime-cu12==12.8.90 - # via torch -nvidia-cudnn-cu12==9.10.2.21 - # via torch -nvidia-cufft-cu12==11.3.3.83 - # via torch -nvidia-cufile-cu12==1.13.1.3 - # via torch -nvidia-curand-cu12==10.3.9.90 - # via torch -nvidia-cusolver-cu12==11.7.3.90 - # via torch -nvidia-cusparse-cu12==12.5.8.93 - # via - # nvidia-cusolver-cu12 - # torch -nvidia-cusparselt-cu12==0.7.1 - # via torch -nvidia-nccl-cu12==2.27.5 - # via torch -nvidia-nvjitlink-cu12==12.8.93 - # via - # nvidia-cufft-cu12 - # nvidia-cusolver-cu12 - # nvidia-cusparse-cu12 - # torch -nvidia-nvshmem-cu12==3.4.5 - # via torch -nvidia-nvtx-cu12==12.8.90 - # via torch -packaging==25.0 - # via - # accelerate - # bitsandbytes - # huggingface-hub - # transformers -psutil==7.2.2 - # via accelerate -pyyaml==6.0.2 - # via - # accelerate - # huggingface-hub - # transformers -regex==2026.2.28 - # via transformers -requests==2.32.4 - # via - # huggingface-hub - # transformers -safetensors==0.7.0 - # via - # accelerate - # transformers -sympy==1.14.0 - # via torch -tokenizers==0.20.3 - # via transformers -torch==2.10.0 - # via - # -r requirements.in - # accelerate - # bitsandbytes -tqdm==4.67.1 - # via - # huggingface-hub - # transformers -transformers==4.45.2 - # via -r requirements.in -triton==3.6.0 - # via torch -typing-extensions==4.14.0 - # via - # huggingface-hub - # llama-cpp-python - # torch -urllib3==2.4.0 - # via requests - -# The following packages are considered to be unsafe in a requirements file: -# setuptools +numpy<2.0 +transformers>=4.44,<4.46 +accelerate>=0.33,<1.0 +bitsandbytes>=0.49 +torch diff --git a/common/language/lasr_llm/setup.py b/common/language/lasr_llm/setup.py index 4afd703a5..72724b82f 100644 --- a/common/language/lasr_llm/setup.py +++ b/common/language/lasr_llm/setup.py @@ -1,14 +1,35 @@ +import os from setuptools import find_packages, setup +import setuptools.command.install +import ament_virtualenv.install + + +_here = os.path.dirname(os.path.abspath(__file__)) + package_name = "lasr_llm" + +class InstallCommand(setuptools.command.install.install): + def run(self): + super().run() + ament_virtualenv.install.install_venv( + install_base=self.install_base, + scripts_base=self.install_scripts, + package_name=package_name, + python_version="3", + source_dir=_here, + ) + return + setup( 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"]), + ("share/" + package_name, ["package.xml", "requirements.txt"]), ], install_requires=["setuptools"], zip_safe=True, @@ -23,7 +44,6 @@ }, entry_points={ "console_scripts": [ - "llm = lasr_llm.nodes.llm:main", "receptionist_service = lasr_llm.nodes.receptionist_service:main", "hri_task_service = lasr_llm.nodes.hri_task_service:main", "storing_groceries_service = lasr_llm.nodes.storing_groceries_service:main", diff --git a/common/language/lasr_llm_interfaces/CMakeLists.txt b/common/language/lasr_llm_interfaces/CMakeLists.txt index 3bca6bbd5..f87f6df38 100644 --- a/common/language/lasr_llm_interfaces/CMakeLists.txt +++ b/common/language/lasr_llm_interfaces/CMakeLists.txt @@ -27,7 +27,6 @@ set(msg_files # Specify service files set(srv_files - "srv/Llm.srv" "srv/ReceptionistQueryLlm.srv" "srv/HRITaskQueryLlm.srv" "srv/SentenceEmbedding.srv" diff --git a/common/language/lasr_llm_interfaces/srv/Llm.srv b/common/language/lasr_llm_interfaces/srv/Llm.srv deleted file mode 100644 index 54973230f..000000000 --- a/common/language/lasr_llm_interfaces/srv/Llm.srv +++ /dev/null @@ -1,5 +0,0 @@ -string system_prompt -string prompt -int32 max_tokens # Max tokens for the LLM to generate ---- -string output \ No newline at end of file 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 7714193fe..1b28a1ca1 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 @@ -48,7 +48,7 @@ class speech_model_params: sample_rate: int = 16000 mic_device: Optional[str] = None timer_duration: Optional[int] = 20 - warmup: bool = True + warmup: bool = False energy_threshold: Optional[int] = None pause_threshold: Optional[float] = 2.0 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..620fa09c8 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,40 @@ 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 +379,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 +424,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 +434,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 807765e34..70994cd5a 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 @@ -5,6 +5,7 @@ MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup, ) +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy from rclpy.executors import MultiThreadedExecutor import message_filters import threading @@ -30,6 +31,7 @@ PointStamped, Point, PoseWithCovarianceStamped, + Vector3, ) from sensor_msgs.msg import Image, CameraInfo from std_msgs.msg import Header @@ -50,11 +52,25 @@ def __init__(self, max_eye_distance: float = 1.5): self._max_eye_distance: float = max_eye_distance self._move_up_count: float = 0.0 self._max_move_up_count: int = 2 + + 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, - "/robot_pose", + "/amcl_pose", self._robot_pose_callback, - qos_profile=10, + qos_profile=amcl_qos, callback_group=self._work_cb_group, ) self._yolo_keypoint_client = self.create_client( @@ -204,6 +220,14 @@ def _execute_callback(self, goal_handle): self.get_logger().info("Beginning eye tracking...") goal = goal_handle.request + + if goal.cancel: + self.get_logger().info('Cancelling eye tracker') + self._done = True + goal_handle.succeed() + # self.destroy_node() + return EyeTrackerAction.Result() + if self._robot_point is None: self.get_logger().warn( "No /robot_pose received yet; continuing and waiting asynchronously." @@ -217,8 +241,8 @@ def _execute_callback(self, goal_handle): g = PointHead.Goal( pointing_frame="head_2_link", - pointing_axis=Point(x=1.0, y=0.0, z=0.0), - max_velocity=1.0, + pointing_axis=Vector3(x=1.0, y=0.0, z=0.0), + max_velocity=2.0, target=PointStamped( header=Header(frame_id="map"), point=goal.person_point, @@ -304,16 +328,20 @@ def detect_cb(image: Image, depth_image: Image, depth_camera_info: CameraInfo): 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 @@ -331,8 +359,8 @@ def detect_cb(image: Image, depth_image: Image, depth_camera_info: CameraInfo): else: g = PointHead.Goal( pointing_frame="head_2_link", - pointing_axis=Point(x=1.0, y=0.0, z=0.0), - max_velocity=1.0, + pointing_axis=Vector3(x=1.0, y=0.0, z=0.0), + max_velocity=2.0, target=PointStamped( header=Header(frame_id="map"), point=self._eyes, @@ -362,9 +390,6 @@ def detect_cb(image: Image, depth_image: Image, depth_camera_info: CameraInfo): self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.25)) goal_handle.succeed() - image_sub.unregister() - depth_sub.unregister() - depth_camera_info_sub.unregister() return EyeTrackerAction.Result() @@ -376,7 +401,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..3fa40fa2f 100644 --- a/common/vision/lasr_vision_interfaces/action/EyeTracker.action +++ b/common/vision/lasr_vision_interfaces/action/EyeTracker.action @@ -1,5 +1,7 @@ # goal geometry_msgs/Point person_point +# cancel +bool cancel --- # result bool person_lost 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..55c647e38 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 @@ -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_yolo/setup.py b/common/vision/lasr_vision_yolo/setup.py index e304b8d9c..4f9c5b2c2 100755 --- a/common/vision/lasr_vision_yolo/setup.py +++ b/common/vision/lasr_vision_yolo/setup.py @@ -8,7 +8,6 @@ package_name = "lasr_vision_yolo" - class InstallCommand(setuptools.command.install.install): def run(self): super().run() @@ -21,7 +20,6 @@ def run(self): ) return - setup( name=package_name, version="0.0.0", diff --git a/skills/setup.py b/skills/setup.py index 6eab8ee70..2987048c0 100755 --- a/skills/setup.py +++ b/skills/setup.py @@ -60,6 +60,8 @@ 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", + "receive_object = lasr_skills.receive_object:main", ], }, ) diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 87daa7045..9cfe4e365 100644 --- a/skills/src/lasr_skills/ask_and_listen.py +++ b/skills/src/lasr_skills/ask_and_listen.py @@ -1,4 +1,6 @@ import yasmin +import rclpy +import yasmin_ros from lasr_skills import Listen from lasr_skills import Say @@ -78,3 +80,19 @@ def __init__( }, remapping={"sequence": "transcribed_speech"}, ) + + +def main(): + rclpy.init() + + yasmin_ros.set_ros_loggers() + + 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}') + + rclpy.shutdown() + + \ No newline at end of file diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py index adc6fa82f..6bd0f991e 100755 --- a/skills/src/lasr_skills/describe_people.py +++ b/skills/src/lasr_skills/describe_people.py @@ -16,7 +16,7 @@ def __init__(self): self.add_state( "GET_IMAGE", GetImage(), - transitions={"succeeded": "GET_CLIP_ATTRIBUTES", "failed": "failed"}, + transitions={"succeeded": "LOOP_ATTR_STATE", "failed": "failed"}, ) loop_state = yasmin.CbState( @@ -39,14 +39,16 @@ def __init__(self): ) def _get_attr(self, blackboard): - if blackboard["clip_index"] is None: - blackboard["clip_index"] = 0 - return "continue" - elif blackboard["clip_index"] < 3: - blackboard["clip_index"] += 1 - return "continue" - else: - return "succeeded" + 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): @@ -58,7 +60,7 @@ def __init__(self): response_handler=self._handle_resp, ) - self.add_input_key("img_raw") + self.add_input_key("image_raw") self.add_output_key("clip_detection_dict") self.glasses_questions = [ @@ -82,42 +84,42 @@ 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["img_raw"] + 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["img_raw"] + 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["img_raw"] + 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["img_raw"] + t_shirt_request.image_raw = blackboard["image_raw"] return t_shirt_request def _handle_resp(self, blackboard, response): if blackboard["clip_index"] == 0: - yasmin.YASMIN_LOG_INFO(f"Glasses: {response.answer}") + # 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}") + # 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}") + # 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}") + # 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} diff --git a/skills/src/lasr_skills/detect_3d.py b/skills/src/lasr_skills/detect_3d.py index 7c3485e51..861349494 100644 --- a/skills/src/lasr_skills/detect_3d.py +++ b/skills/src/lasr_skills/detect_3d.py @@ -3,13 +3,14 @@ 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 -from time import sleep +import time from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy @@ -32,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, @@ -53,6 +53,8 @@ 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, @@ -60,82 +62,85 @@ def __init__( ) self.cam_info = None + self.data = None + self.image_msg = None + self.node.create_subscription( CameraInfo, self.depth_camera_info_topic, self._cache_camera_info, qos_profile=camera_qos, ) - + 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 ) - + 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 + + self.ts.registerCallback(self.callback) + + def callback(self, image_msg, depth_msg): + if self.data is None: + self.data = (image_msg, depth_msg) def _cache_camera_info(self, msg: CameraInfo) -> None: if self.cam_info is None: self.cam_info = msg def _create_req(self, blackboard): + self.data = None + self.image_msg = None + if self.cam_info is None: deadline = time.time() + 5.0 while self.cam_info is None and time.time() < deadline: - rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.25) if self.cam_info is None: - self.node.get_logger().error( + yasmin.YASMIN_LOG_ERROR( f"Timed out waiting for camera info on {self.depth_camera_info_topic}" ) return "failed" - 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) - deadline = time.time() + 30.0 - while not self.data: + 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" - rclpy.spin_once(self.node, timeout_sec=0.1) - - image_msg, depth_msg, cam_info_msg = self.data + time.sleep(0.25) + + 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.cam_info, model=self.model, confidence=self.confidence, filter=self.filter, target_frame=self.target_frame, ) self.image_msg = image_msg - self.pcl = pcl_msg + return req def response_handler(self, blackboard, response): yasmin.YASMIN_LOG_INFO(f"Got {len(response.detected_objects)} detections") for det in response.detected_objects: - yasmin.YASMIN_LOG_INFO( + self.node.get_logger().info( f" {det.name} at ({det.point.x:.2f}, {det.point.y:.2f}, {det.point.z:.2f})" ) + blackboard["detections_3d"] = response blackboard["image_raw"] = self.image_msg diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py index 32844c7b6..2c03a4a0e 100644 --- a/skills/src/lasr_skills/detect_3d_in_area.py +++ b/skills/src/lasr_skills/detect_3d_in_area.py @@ -24,6 +24,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,11 +35,12 @@ 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 - self.debug_publisher = yasmin_ros.logger_node.create_publisher( + self.node = yasmin_ros.logger_node + self.debug_publisher = self.node.create_publisher( PolygonStamped, debug_publisher, 1 ) @@ -66,12 +69,13 @@ 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 detection.point.x == "nan": #CHECK: Potential broken? float vs string? + yasmin.YASMIN_LOG_WARN("NAN detection check work") # Remove line if works continue yasmin.YASMIN_LOG_INFO( f"Detected a {detection.name} at x:{detection.point.x}, y:{detection.point.y}, z:{detection.point.z}" @@ -79,7 +83,7 @@ def execute(self, blackboard): pub.publish( PointStamped( header=Header( - frame_id="head_front_camera_color_optical_frame", + frame_id="map", stamp=Time().to_msg(), ), point=Point( @@ -122,6 +126,8 @@ def __init__( z_min: Optional[float] = None, z_max: Optional[float] = None, ): + + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) if area_polygon is None: self.add_input_key("polygon") if z_min is None and z_max is None: @@ -131,8 +137,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 ed6c866e4..a3c720f6d 100644 --- a/skills/src/lasr_skills/detect_all_in_polygon.py +++ b/skills/src/lasr_skills/detect_all_in_polygon.py @@ -149,12 +149,24 @@ def __init__( self._min_coverage = min_coverage self._z_axis = z_axis self._fov_depth = fov_depth + + qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.VOLATILE, + reliability=ReliabilityPolicy.BEST_EFFORT, + depth=10, + ) self.node = yasmin_ros.logger_node + self.msg = None + self.node.create_subscription(CameraInfo, "/head_front_camera/depth/camera_info", self.info_cb, qos_profile=qos) self._tf_buffer = tf2_ros.Buffer(Duration(seconds=10.0)) self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self.node) + def info_cb(self, msg): + self.msg = msg + def _get_camera_fov_polygon(self) -> ShapelyPolygon: """ Projects the camera's FOV to the ground plane using intrinsics and TF. @@ -163,27 +175,17 @@ 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=5, - ) - - if success is False: - yasmin.YASMIN_LOG_INFO("No camera info received, ending state") - self.cancel_state() + attempt = 0 + while self.msg is None: + if attempt < 5: + sleep(0.5) + attempt += 0.5 + else: + yasmin.YASMIN_LOG_INFO("No camera info received, ending state") + self.cancel_state() model = PinholeCameraModel() - model.fromCameraInfo(msg) + model.fromCameraInfo(self.msg) # Define pixel corners (image boundaries) corners = [ @@ -202,7 +204,7 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon: 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 @@ -214,7 +216,7 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon: try: transform = self._tf_buffer.lookup_transform( "map", - msg.header.frame_id, + self.msg.header.frame_id, Time(), timeout=Duration(seconds=5.0), ) @@ -307,7 +309,9 @@ def _calculate_sweep_points(self) -> List[PointStamped]: # Optional: visualize FOV - qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ) pub = self.node.create_publisher(PolygonStamped, "projected_fov_polygon", qos) @@ -399,17 +403,17 @@ def __init__( "LOOK_POINT", LookToPoint(), transitions={ - "succeeded": "DETECT_OBJECTS", - "aborted": "DETECT_OBJECTS", + "succeeded": "SLEEP", + "aborted": "SLEEP", "canceled": "failed", - "timeout": "DETECT_OBJECTS", + "timeout": "SLEEP", }, ) - # self.add_state( - # 'SLEEP', - # Wait(wait_time=4), - # transitions={'succeeded': 'DETECT_OBJECTS', 'failed': 'failed'} - # ) + self.add_state( + 'SLEEP', + Wait(wait_time=2), + transitions={'succeeded': 'DETECT_OBJECTS', 'failed': 'failed'} + ) self.add_state( "DETECT_OBJECTS", Detect3DInArea( @@ -422,11 +426,21 @@ def __init__( target_frame="map", ), transitions={"succeeded": "PROCESS_DETECTIONS", "failed": "failed"}, + remappings={ + "detections_3d": "detections_3d", + "image_raw": "image_raw", + }, ) self.add_state( "PROCESS_DETECTIONS", ProcessDetections(min_new_object_dist=min_new_object_dist), transitions={"succeeded": "GET_LOOK_POINT", "failed": "failed"}, + remappings={ + "detections_3d": "detections_3d", + "detected_objects": "detected_objects", + "image_raw": "image_raw", + "debug_images": "debug_images", + }, ) def _get_look_point(self, blackboard) -> str: @@ -512,11 +526,14 @@ def __init__( self._model = model 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 ) @@ -586,6 +603,20 @@ def build_state_machine(self): for input in ["debug_images", "detected_objects"]: publish_detected_objects.add_input_key(input) + def _init_bb(blackboard): + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] + return "succeeded" + + init_state = yasmin.CbState(outcomes=["succeeded"], callback=_init_bb) + init_state.add_output_key("detected_objects") + init_state.add_output_key("debug_images") + self.add_state( + "INIT_BLACKBOARD", + init_state, + transitions={"succeeded": "CALCULATE_SWEEP_POINTS"}, + ) + self.add_state( "CALCULATE_SWEEP_POINTS", CalculateSweepPoints( @@ -593,6 +624,7 @@ def build_state_machine(self): min_coverage=self._min_coverage, ), transitions={"succeeded": "LOOK_AND_DETECT", "failed": "failed"}, + remappings={"sweep_points": "sweep_points"}, ) self.add_state( "LOOK_AND_DETECT", @@ -604,39 +636,51 @@ def build_state_machine(self): min_new_object_dist=self._min_new_object_dist, ), transitions={"succeeded": "PUBLISH_DETECTED_OBJECTS", "failed": "failed"}, + remappings={ + "sweep_points": "sweep_points", + "detected_objects": "detected_objects", + }, ) self.add_state( "PUBLISH_DETECTED_OBJECTS", publish_detected_objects, transitions={"succeeded": "succeeded"}, + remappings={ + "debug_images": "debug_images", + "detected_objects": "detected_objects", + }, + ) + +class Detect_node(Node): + def __init__(self): + super().__init__( + node_name="detect_all_in_polygon", ) + self._executor = Executor() + self._executor.add_node(self) + self._spin_thread = Thread(target=self._executor.spin) + self._spin_thread.start() def main(): seat_area = [ - [0.9422937035560608, -1.9376981258392334], - [-0.01625092327594757, -1.1312360763549805], - [-0.5108118057250977, -1.6913851499557495], - [0.4300234913825989, -2.5222253799438477], + [0.37787461280822754, -3.057680130004883], + [-1.3188365697860718, -1.687604546546936], + [-0.07020854949951172, -0.43113774061203003], + [1.5865206718444824, -1.74715256690979], ] seat_polygon = ShapelyPolygon(seat_area) rclpy.init() - - # node = Node('Detect_All_In_Polygon') - # executor = Executor() - # executor.add_node(node) - - # thread = Thread(target=executor.spin()) - # thread.start() - yasmin_ros.set_ros_loggers() + + node = Detect_node() + + yasmin_ros.set_ros_loggers(node) bb = Blackboard() bb["sweep_points"] = [] - bb["detected_objects"] = [] - bb["debug_images"] = [] bb["pointstamped"] = PointStamped() bb["sweep_point_index"] = 0 @@ -660,10 +704,13 @@ def main(): yasmin.YASMIN_LOG_INFO(f"SM finished with outcome: {outcome}") except Exception as e: yasmin.YASMIN_LOG_WARN(e) - - if rclpy.ok(): + node.destroy_node() rclpy.shutdown() + + node.destroy_node() + rclpy.shutdown() + if __name__ == "__main__": main() 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/eye_tracker.py b/skills/src/lasr_skills/eye_tracker.py index d107257ec..70dd79653 100644 --- a/skills/src/lasr_skills/eye_tracker.py +++ b/skills/src/lasr_skills/eye_tracker.py @@ -8,11 +8,14 @@ class StartEyeTracker(yasmin_ros.ActionState): def __init__(self): super().__init__( - action_name="/lasr_vision_eye_tacker/track_eyes", + action_name="/lasr_vision_eye_tracker/track_eyes", action_type=EyeTrackerAction, create_goal_handler=self.create_goal, response_timeout=1.0, + maximum_retry=0, ) + + self.add_input_key('person_point') def create_goal(self, blackboard): goal_msg = EyeTrackerAction.Goal() @@ -24,10 +27,15 @@ def create_goal(self, blackboard): class StopEyeTracker(yasmin_ros.ActionState): def __init__(self): super().__init__( - action_name="/lasr_vision_eye_tacker/track_eyes", - action_spec=EyeTrackerAction, - goal_cb=self.create_goal, - create_goal_handler=self.cancel_goal, + action_name="/lasr_vision_eye_tracker/track_eyes", + action_type=EyeTrackerAction, + create_goal_handler=self._create_goal, + response_timeout=1.0, + maximum_retry=0, ) - - super().cancel_state() + + + def _create_goal(self, blackboard): + return EyeTrackerAction.Goal(cancel=True) + + diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index 0d7e1b08f..db4e75c09 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,14 +37,6 @@ def execute(self, blackboard): node = yasmin_ros.logger_node - node.declare_parameter(f"{self.location_param}.position.x", 0.0) - node.declare_parameter(f"{self.location_param}.position.y", 0.0) - node.declare_parameter(f"{self.location_param}.position.z", 0.0) - node.declare_parameter(f"{self.location_param}.orientation.x", 0.0) - node.declare_parameter(f"{self.location_param}.orientation.y", 0.0) - node.declare_parameter(f"{self.location_param}.orientation.z", 0.0) - node.declare_parameter(f"{self.location_param}.orientation.w", 0.0) - goal_pose = Pose( position=Point( x=float( @@ -83,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" diff --git a/skills/src/lasr_skills/look_to_point.py b/skills/src/lasr_skills/look_to_point.py index 927e3c6e6..c94d526b9 100644 --- a/skills/src/lasr_skills/look_to_point.py +++ b/skills/src/lasr_skills/look_to_point.py @@ -23,8 +23,7 @@ def __init__( action_type=PointHead, create_goal_handler=self._create_goal, response_timeout=5.0, - callback_group=ros_client_group, - maximum_retry=1, + maximum_retry=0, ) if pointstamped is None: self.add_input_key("pointstamped") @@ -38,8 +37,8 @@ def _create_goal(self, blackboard): ) goal = PointHead.Goal() - goal.pointing_frame = "head_front_camera_depth_optical_frame" - goal.pointing_axis = Vector3(x=0.0, y=0.0, z=1.0) + goal.pointing_frame = "head_2_link" + goal.pointing_axis = Vector3(x=1.0, y=0.0, z=0.0) goal.max_velocity = 1.0 goal.target = target diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py index c24ca0f45..7cd293392 100755 --- a/skills/src/lasr_skills/receive_object.py +++ b/skills/src/lasr_skills/receive_object.py @@ -1,225 +1,238 @@ #!/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 + 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 ClearOctomap(ServiceState): + def __init__(self): + super().__init__( + srv_type=Empty, + srv_name="/clear_octomap", + create_request_handler=self._create_request, + ) + + def _create_request(self, blackboard): + return Empty.Request() + +#TODO: Do we need to detect object or just assume that the second guest is holding a bag. +class ReceiveObject(StateMachine): + def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): + + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + if object_name is None: + self.add_input_key("object_name") + + self.add_state( + "CLEAR_OCTOMAP", + ClearOctomap(), + transitions={ + "succeeded": "LOOK_LEFT", + "aborted": "failed" + }, + ) -class ReceiveObject(smach.StateMachine): - def __init__( - self, node: Node, object_name: Union[str, None] = None, vertical: bool = True - ): + self.add_state( + "LOOK_LEFT", + PlayMotion(motion_name="look_left"), + transitions={ + "succeeded": "LOOK_DOWN_LEFT", + "aborted": "failed", + "canceled": "failed", + }, + ) - 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") + self.add_state( + "LOOK_DOWN_LEFT", + PlayMotion(motion_name="look_down_left"), + transitions={ + "succeeded": "LOOK_RIGHT", + "aborted": "failed", + "canceled": "failed", + }, ) - 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", - }, - ) + self.add_state( + "LOOK_RIGHT", + PlayMotion(motion_name="look_right"), + transitions={ + "succeeded": "LOOK_DOWN_RIGHT", + "aborted": "failed", + "canceled": "failed", + }, + ) - smach.StateMachine.add( - "LOOK_DOWN_LEFT", - PlayMotion(node=Node, motion_name="look_down_left"), - transitions={ - "succeeded": "LOOK_RIGHT", - "aborted": "failed", - "preempted": "failed", - }, - ) + self.add_state( + "LOOK_DOWN_RIGHT", + PlayMotion(motion_name="look_down_right"), + transitions={ + "succeeded": "LOOK_DOWN_CENTRE", + "aborted": "failed", + "canceled": "failed", + }, + ) - smach.StateMachine.add( - "LOOK_RIGHT", - PlayMotion(motion_name="look_right"), - transitions={ - "succeeded": "LOOK_DOWN_RIGHT", - "aborted": "failed", - "preempted": "failed", - }, - ) + self.add_state( + "LOOK_DOWN_CENTRE", + PlayMotion(motion_name="look_centre"), + transitions={ + "succeeded": "LOOK_CENTRE", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "LOOK_CENTRE", + PlayMotion(motion_name="look_centre"), + transitions={ + "succeeded": "SAY_REACH_ARM", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "SAY_REACH_ARM", + Say(text="Please step back, I am going to reach my arm out."), + transitions={ + "succeeded": "REACH_ARM", + "aborted": "REACH_ARM", + "canceled": "REACH_ARM", + }, + ) - smach.StateMachine.add( - "LOOK_DOWN_RIGHT", - PlayMotion(node=Node, motion_name="look_down_right"), + 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"), - transitions={ - "succeeded": "SAY_REACH_ARM", - "aborted": "failed", - "preempted": "failed", - }, - ) + self.add_state( + "OPEN_GRIPPER", + PlayMotion(motion_name="open_gripper"), + transitions={ + "succeeded": "SAY_PLACE", + "aborted": "failed", + "canceled": "failed", + }, + ) - smach.StateMachine.add( - "SAY_REACH_ARM", + if object_name is not None: + self.add_state( + "SAY_PLACE", Say( - node=Node, text="Please step back, I am going to reach my arm out." + text=f"Please place the {object_name} in my hand. I will wait for a few seconds.", ), transitions={ - "succeeded": "REACH_ARM", - "aborted": "REACH_ARM", - "preempted": "REACH_ARM", + "succeeded": "WAIT_5", + "aborted": "failed", + "canceled": "failed", }, ) - - 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"), + else: + self.add_state( + "SAY_PLACE", + Say( + format_str="Please place the {} in my hand. I will wait for a few seconds.", + ), transitions={ - "succeeded": "SAY_PLACE", + "succeeded": "WAIT_5", "aborted": "failed", - "preempted": "failed", + "canceled": "failed", }, + remapping={"placeholders": "object_name"}, ) - 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", - }, - ) + self.add_state( + "WAIT_5", + Wait(5), + transitions={ + "succeeded": "CLOSE_HALF_GRIPPER", + "failed": "CLOSE_HALF_GRIPPER", + }, + ) - 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", - }, - ) + #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 + + # 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 - using half to not jam an item between gripper + PlayMotion(motion_name="close_half"), + transitions={ + "succeeded": "FOLD_ARM", + "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) + self.add_state( + "FOLD_ARM", + PlayMotion(motion_name="cml_arm_away"), + transitions={ + "succeeded": "succeeded", + "aborted": "failed", + "canceled": "failed", + }, + ) -if __name__ == "__main__": +def 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() + diff --git a/skills/src/lasr_skills/vision/crop_image_3d.py b/skills/src/lasr_skills/vision/crop_image_3d.py index 84b542496..54b7c9d89 100644 --- a/skills/src/lasr_skills/vision/crop_image_3d.py +++ b/skills/src/lasr_skills/vision/crop_image_3d.py @@ -1,7 +1,6 @@ import rclpy from rclpy.node import Node -from rclpy.wait_for_message import wait_for_message -from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy import yasmin from yasmin import State, StateMachine @@ -14,6 +13,8 @@ from typing import Optional, List +import time + from geometry_msgs.msg import PoseWithCovarianceStamped from cv_bridge import CvBridge @@ -64,8 +65,10 @@ def __init__( self.crop_logic = crop_logic self.crop_type = crop_type self._bridge = CvBridge() + + self.node = yasmin_ros.logger_node - self.debug_publisher = yasmin_ros.logger_node.create_publisher( + self.debug_publisher = self.node.create_publisher( Image, "/skills/crop_image_3d/debug", QoSProfile( @@ -74,6 +77,17 @@ def __init__( reliability=ReliabilityPolicy.BEST_EFFORT, ), ) + + 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( @@ -84,25 +98,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): 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 - success, robot_pose_msg = wait_for_message( - PoseWithCovarianceStamped, yasmin_ros.logger_node, self.robot_pose_topic - ) - 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 14421c9a5..89f81abe5 100755 --- a/skills/src/lasr_skills/vision/get_image.py +++ b/skills/src/lasr_skills/vision/get_image.py @@ -3,7 +3,8 @@ from yasmin import State, StateMachine import rclpy -from rclpy.wait_for_message import wait_for_message +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy + from typing import Optional from sensor_msgs.msg import Image, PointCloud2 @@ -14,42 +15,39 @@ 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") - 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.camera_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, ) + + self.node = yasmin_ros.logger_node + + self.msg = None + + self.node.create_subscription(Image, topic, self.image_cb, qos_profile=self.camera_qos) + + def image_cb(self, msg): + self.msg = msg def execute(self, blackboard): - # if not rclpy.ok(): - # rclpy.init() try: - msg = wait_for_message(Image, yasmin_ros.logger_node, self.topic) - 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.msg + return 'failed' if self.msg is None else 'succeeded' except Exception as e: yasmin.YASMIN_LOG_ERROR(str(e)) return "failed" - return "succeeded" +# UNUSED THROUGHOUT WHOLE REPO, MAYBE DELETE????? + class GetPointCloud(State): """ State for acquiring a PointCloud2 message. @@ -60,6 +58,12 @@ 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, + history=HistoryPolicy.KEEP_LAST, + ) yasmin_ros.logger_node.declare_parameter( "image_topic", "/head_front_camera/rgb/image_raw" @@ -78,7 +82,7 @@ def execute(self, blackboard): try: blackboard["pcl_msg"] = None blackboard["pcl_msg"] = wait_for_message( - PointCloud2, yasmin_ros.logger_node, self.topic + PointCloud2, yasmin_ros.logger_node, self.topic, qos_profile=self.camera_qos ) if blackboard["pcl_msg"] is None: return "failed" @@ -88,6 +92,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"]) @@ -96,6 +101,12 @@ 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, + history=HistoryPolicy.KEEP_LAST, + ) self.topic1 = "/head_front_camera/rgb/image_raw" self.topic2 = "/head_front_camera/depth/points" @@ -108,10 +119,10 @@ def execute(self, blackboard): # rclpy.init() try: blackboard["img_msg"] = wait_for_message( - Image, yasmin_ros.logger_node, self.topic1 + Image, yasmin_ros.logger_node, self.topic1, self.camera_qos ) blackboard["pcl_msg"] = wait_for_message( - PointCloud2, yasmin_ros.logger_node, self.topic2 + PointCloud2, yasmin_ros.logger_node, self.topic2, self.camera_qos ) if blackboard["img_msg"] is None or blackboard["pcl_msg"] is None: 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 44f7ac78d..dfdd007a7 100644 --- a/skills/src/lasr_skills/wait_for_person_in_area.py +++ b/skills/src/lasr_skills/wait_for_person_in_area.py @@ -5,7 +5,8 @@ from lasr_skills import Detect3DInArea -from shapely.geometry.polygon import Polygon +from shapely import Polygon as ShapelyPolygon + class CheckForPerson(State): @@ -21,7 +22,7 @@ def execute(self, blackboard): class WaitForPersonInArea(StateMachine): - def __init__(self, area_polygon_param: Polygon): + def __init__(self): super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) self.add_output_key("detections_3d") @@ -40,7 +41,7 @@ def __init__(self, area_polygon_param: Polygon): node.get_parameter("door_polygon.bottom_right").get_parameter_value() ) - door_polygon = Polygon([top_left, top_right, bottom_left, bottom_right]) + door_polygon = ShapelyPolygon([top_left, top_right, bottom_right, bottom_left]) self.add_state( "DETECT_PEOPLE_3D", diff --git a/tasks/HRI/HRI/state_machine.py b/tasks/HRI/HRI/state_machine.py index 5c092cd5a..d9663209a 100644 --- a/tasks/HRI/HRI/state_machine.py +++ b/tasks/HRI/HRI/state_machine.py @@ -10,7 +10,7 @@ from geometry_msgs.msg import Point, PointStamped, Pose -from lasr_skills import Say, GoToLocation +from lasr_skills import Say, GoToLocation, StopEyeTracker, PlayMotion from HRI.states import * @@ -27,6 +27,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" @@ -49,37 +51,103 @@ 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", + GoToLocation(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"}, + transitions={"succeeded": "STOP_EYE_TRACKER", "failed": "failed"}, + ) + + self.add_state( + "STOP_EYE_TRACKER", + StopEyeTracker(), + transitions={ + "succeeded": "LOOK_CENTRE", + "aborted": "failed", + "canceled": "failed", + "timeout": "failed", + }, + ) + + self.add_state( + "LOOK_CENTRE", + PlayMotion("look_centre"), + transitions={ + "succeeded": "SAY_FOLLOW", + "aborted": "failed", + "canceled": "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( + "SAY_FOLLOW", + Say(text="Welcome. Follow me to the seating area."), + transitions={ + "succeeded": "GUIDE_TO_SEAT", + "aborted": "failed", + "canceled": "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( "SEAT_GUEST", # SM3: Locates and seats guest in free seat SeatGuest(learn_host=False), - transitions={"succeeded": "succeeded", "failed": "failed"}, + transitions={"succeeded": "CHECK", "failed": "failed"}, + ) + + self.add_state( + "CHECK", + yasmin.CbState(outcomes=["succeeded", 'GO_TO_DOOR_2'], callback=self.check), + transitions={"succeeded": 'succeeded', 'GO_TO_DOOR_2': 'GO_TO_DOOR_2'}, + ) + + self.add_state( + "GO_TO_DOOR_2", + GoToLocation(location_param="door_pose"), + transitions={"succeeded": "GREET_2", "failed": "failed"}, + ) + + self.add_state( + "GREET_2", # SM2: Greets guest + LookAndGreetGuest(last_resort=False, guest_id="guest2"), + transitions={"succeeded": "STOP_EYE_TRACKER", "failed": "failed"}, ) + def check(self, blackboard): + guest = blackboard["guest_data"][f"guest{self.guest_id}"] + yasmin.YASMIN_LOG_INFO(f'{self.guest_id}') + + for key in guest.keys(): + value = guest[key] + yasmin.YASMIN_LOG_INFO(f"{key}: {value}") + + self.guest_id += 1 + return "GO_TO_DOOR_2" 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", @@ -122,11 +190,9 @@ def main(): sm = HRI() bb = yasmin.Blackboard() - host_data = {} face_detection_confidence = 0.2 bb["guest_data"] = { - "host": host_data, "guest1": { "name": "", "drink": "", @@ -148,6 +214,8 @@ def main(): bb["dataset"] = "hri" bb["drink_position"] = PointStamped() + + outcome = sm(bb) yasmin.YASMIN_LOG_INFO(f"State machine has ended with outcome {outcome}") @@ -155,9 +223,8 @@ def main(): # except Exception as e: # yasmin.YASMIN_LOG_WARN(e) - if rclpy.ok(): - node.destroy_node() - rclpy.shutdown() + node.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/tasks/HRI/HRI/states/get_attributes.py b/tasks/HRI/HRI/states/get_attributes.py index 839cb8d1f..bdde91c35 100644 --- a/tasks/HRI/HRI/states/get_attributes.py +++ b/tasks/HRI/HRI/states/get_attributes.py @@ -37,9 +37,7 @@ def __init__(self, guest_id: str): def execute(self, blackboard) -> str: try: - blackboard["guest_data"][self._guest_id]["attributes"] = blackboard[ - "clip_detection_dict" - ] + blackboard["guest_data"][self._guest_id]["attributes"] = blackboard["clip_detection_dict"] blackboard["guest_data"][self._guest_id]["detection"] = True return "succeeded" except Exception as e: @@ -61,7 +59,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 360dd0245..ebe464e2f 100755 --- a/tasks/HRI/HRI/states/get_name_and_drink.py +++ b/tasks/HRI/HRI/states/get_name_and_drink.py @@ -30,21 +30,17 @@ def __init__(self, task, guest_id): self.task = task self.guest_id = guest_id - def _create_req(self, userdata, request): + def _create_req(self, blackboard): request = HRITaskQueryLlm.Request( - string=userdata.guest_transcription, task=self.task + llm_input=blackboard['guest_transcription'], task=self.task ) return request def _handle_resp(self, blackboard, result): - ( - blackboard["guest_data"].update({self.guest_id: {"name": result.name}}) - if self.task == "name" - else blackboard["guest_data"].update( - {self.guest_id: {"drink": result.favoutrite_drink}} - ) - ) + result = result.response + blackboard["guest_data"][self.guest_id][self.task] = result.name if self.task == "name" else result.favourite_drink + return "succeeded" @@ -64,7 +60,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" @@ -80,8 +76,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__( diff --git a/tasks/HRI/HRI/states/greet.py b/tasks/HRI/HRI/states/greet.py index b88d5fe55..b8fc8e46b 100644 --- a/tasks/HRI/HRI/states/greet.py +++ b/tasks/HRI/HRI/states/greet.py @@ -97,7 +97,7 @@ def __init__(self, last_resort, guest_id): ) self.add_state( "WAIT_FOR_GUEST", - WaitForPersonInArea(area_polygon_param="door_polygon"), + WaitForPersonInArea(), transitions={ "succeeded": "GET_PERSON_POINT", "failed": "SAY_WAITING_FOR_GUEST", @@ -119,6 +119,7 @@ def __init__(self, last_resort, guest_id): "succeeded": "GREET_AND_ASK_GUEST", "aborted": "SAY_WAITING_FOR_GUEST", "canceled": "failed", + 'timeout': 'GREET_AND_ASK_GUEST', }, ) self.add_state( diff --git a/tasks/HRI/HRI/states/hri_learn_faces.py b/tasks/HRI/HRI/states/hri_learn_faces.py index 82cd14bd7..c70d7b539 100644 --- a/tasks/HRI/HRI/states/hri_learn_faces.py +++ b/tasks/HRI/HRI/states/hri_learn_faces.py @@ -93,7 +93,7 @@ def _handle_resp(self, blackboard, response): try: blackboard["num_images"] += 1 except: - blackboard["num_images"] = 0 + blackboard["num_images"] = 1 except Exception as e: yasmin.YASMIN_LOG_ERROR(f"Service call failed: {e}") return "failed" @@ -108,7 +108,7 @@ def __init__(self, dataset_size: int): self._dataset_size = dataset_size def execute(self, blackboard): - if blackboard.get("num_images", 0) >= self._dataset_size: + if blackboard['num_images'] >= self._dataset_size: yasmin.YASMIN_LOG_INFO("Collected enough images for the guest.") return "succeeded" else: diff --git a/tasks/HRI/HRI/states/learn_host_face.py b/tasks/HRI/HRI/states/learn_host_face.py index a4743d473..ddb40f786 100644 --- a/tasks/HRI/HRI/states/learn_host_face.py +++ b/tasks/HRI/HRI/states/learn_host_face.py @@ -31,7 +31,7 @@ def execute(self, blackboard): return "failed" point = blackboard["seated_guest_locs"][0] blackboard["pointstamped"] = PointStamped( - header=Header(frame_id="base_footprint"), point=point + header=Header(frame_id="map"), point=point ) # TODO: Change to 'map' when 2dnav is fixed return "succeeded" diff --git a/tasks/HRI/HRI/states/seat_guest.py b/tasks/HRI/HRI/states/seat_guest.py index d2da86cbd..0989860f5 100644 --- a/tasks/HRI/HRI/states/seat_guest.py +++ b/tasks/HRI/HRI/states/seat_guest.py @@ -10,7 +10,6 @@ import numpy as np import tf2_ros as tf -import threading from typing import Optional from shapely.geometry import Polygon as ShapelyPolygon @@ -29,6 +28,7 @@ Say, Wait, DetectAllInPolygon, + StopEyeTracker ) from yasmin_viewer import YasminViewerPub @@ -103,6 +103,7 @@ def execute(self, blackboard): blackboard["sofa_detections"] (List[Detection3D]): List of detected objects on the sofa """ + yasmin.YASMIN_LOG_WARN("Finding seat in seat guest") seat_sofa = True seated_guests_loc = [ detection.point @@ -259,13 +260,7 @@ def __init__( # TODO: Update to allow local paramters overriding ros param seating_area_minus_sofa = self.seating_area.difference(self.sofa_area) - - # 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( "SAY_FINDING_SEAT", Say(text="I will now find a seat for you."), @@ -279,7 +274,7 @@ def __init__( "LOOK_TO_SOFA", LookToPoint( pointstamped=PointStamped( - header=Header(frame_id="base_footprint"), + header=Header(frame_id="map"), point=self.sofa_point, # TODO: Change to 'map' when 2dnav is fixed ) ), @@ -311,31 +306,31 @@ def __init__( "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", - # DetectAllInPolygon( - # polygon=seating_area_minus_sofa, # TODO: Verify Potential type mismatch (BaseGeometry vs accepted ShapelyPolygon) - # object_filter=["person", "chair"], - # min_coverage=1.0, - # min_new_object_dist=0.50, - # min_confidence=0.5, + # 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={"detected_objects": "non_sofa_detections"}, + # remappings={"detections_3d": "non_sofa_detections"}, # ) + + self.add_state( + "DETECT_NON_SOFA", + DetectAllInPolygon( + polygon=seating_area_minus_sofa, # TODO: Verify Potential type mismatch (BaseGeometry vs accepted ShapelyPolygon) + object_filter=["person", "chair"], + min_coverage=1.0, + min_new_object_dist=0.50, + min_confidence=0.5, + ), + transitions={"succeeded": "PROCESS_DETECTIONS", "failed": "failed"}, + remappings={"detected_objects": "non_sofa_detections"}, + ) # Process detections if learn_host: detection_transition = "SAY_AND_LEARN_HOST_FACE" @@ -391,6 +386,7 @@ def __init__( "succeeded": "SAY_SEAT_GUEST", "aborted": "SAY_SEAT_GUEST", "canceled": "SAY_SEAT_GUEST", + "timeout": "SAY_SEAT_GUEST" }, remappings={"pointstamped": "guest_seat_point"}, ) @@ -502,15 +498,34 @@ 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__( + node_name="hri", + allow_undeclared_parameters=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.start() def main(): rclpy.init() - node = rclpy.create_node("hri") + node = HRI_node() yasmin_ros.set_ros_loggers(node) try: + #TODO: Try with learn_host=True sm = SeatGuest(learn_host=False) bb = Blackboard() @@ -527,8 +542,15 @@ def main(): "detection": False, "seating_detection": False, }, + "guest2": { + "name": "", + "drink": "", + "detection": False, + "seating_detection": False, + }, } + YasminViewerPub(sm, "HRI_SM3") outcome = sm(bb) diff --git a/tasks/HRI/HRI/states/start_door_sm.py b/tasks/HRI/HRI/states/start_door_sm.py index 193165cd6..6794c458b 100644 --- a/tasks/HRI/HRI/states/start_door_sm.py +++ b/tasks/HRI/HRI/states/start_door_sm.py @@ -21,7 +21,7 @@ def __init__( location_param: Union[str, None] = "start_pose", ): super().__init__( - outcomes=["succeeded", "failed"], + outcomes=["succeeded", "failed"], handle_sigint=True ) self.add_state( diff --git a/tasks/HRI/config/debug.rviz b/tasks/HRI/config/debug.rviz new file mode 100644 index 000000000..e517c2c1e --- /dev/null +++ b/tasks/HRI/config/debug.rviz @@ -0,0 +1,829 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Image1 + - /Image1/Topic1 + - /PointCloud21/Topic1 + - /Polygon1 + - /Polygon2 + - /PointStamped1 + - /Marker1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 645 + - 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: false + 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: 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/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 + - 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: /projected_fov_polygon + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 255; 0; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /skills/detect3d_in_area/debug + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 10 + Name: PointStamped + Radius: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sweep_points + 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: /yolo/detect3d/yolo11n_seg_pt + 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.055266380310059 + 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 + 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 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 0.9223852157592773 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1131 + Hide Left Dock: true + Hide Right Dock: true + Image: + collapsed: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000415fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000002c0000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000001b1000001230000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000fb0000000a0049006d00610067006500000003010000014f0000002800ffffff000000010000010f00000415fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000415000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003c00000041500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 960 + X: 960 + Y: 32 diff --git a/tasks/HRI/config/lab.yaml b/tasks/HRI/config/lab.yaml index 9792022ce..3aa21f7bb 100644 --- a/tasks/HRI/config/lab.yaml +++ b/tasks/HRI/config/lab.yaml @@ -1,65 +1,67 @@ hri: # the `hri` Node's Parameters ros__parameters: + # Start location after door start_pose: position: - x: 6.945671558380127 - y: 2.1550674438476562 + x: 3.045619723531395 + y: 0.4649833631759533 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.8876724994180473 - w: 0.46047533460210094 + z: 0.9927799557187154 + w: 0.11994982085499546 + # Where to wait for guests door_pose: position: - x: 7.2768144607543945 - y: -2.1811742782592773 + x: 2.133728259050112 + y: 0.9460632470337332 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.5462976677684211 - w: 0.8375911044123998 + z: 0.9378581866285763 + w: 0.34701876285549516 door_polygon: - top_left: [0.6513739824295044, 0.6461964249610901] - top_right: [1.5877039432525635, -0.14666156470775604] - bottom_right: [2.010641574859619, 0.6358721256256104] - bottom_left: [1.1054649353027344, 1.3228563070297241] + top_left: [1.4293599128723145, 1.1347585916519165] + top_right: [1.8499776124954224, 1.6542631387710571] + bottom_right: [1.4631035327911377, 2.074192762374878] + bottom_left: [0.940778374671936, 1.5865740776062012] # Where to position self for seating guests seat_pose: position: - x: 0.7253542411729228 - y: -0.41696111136241965 + x: 1.6410613033142212 + y: 0.029216336819556613 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.7903108029987692 - w: 0.6127061568675809 + z: -0.9565993334920534 + w: 0.29140644324132453 # Where the robot looks at the general sofa sofa_point: - x: 2.175074577331543 - y: -0.014040738344192505 + x: 0.4285953640937805 + y: -1.1452689170837402 z: 0.5 # From robot POV: [top left, top right,bottom right, bottom left ] # General area to perform detections in seat_area: - top_left: [2.9270116806030273, 0.9738638401031494] - top_right: [2.923091411590576, -1.54178786277771] - bottom_right: [0.4543781280517578, -1.3588223457336426] - bottom_left: [0.9297175407409668,1.0230729579925537] + top_left: [0.7586971521377563, -1.8606892824172974] + top_right: [-0.9268304109573364, -0.5480098724365234] + bottom_right: [0.03138554096221924, 0.5941693782806396] + bottom_left: [1.653577566146850, -0.5811513662338257] # 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.728126049041748, -1.872155785560608] + top_right: [-0.356697678565979, -1.0435458421707153] + bottom_right: [0.16386330127716064, -0.47996777296066284] + bottom_left: [1.1935354471206665, -1.1827179193496704] diff --git a/tasks/HRI/launch/HRI.launch.py b/tasks/HRI/launch/HRI.launch.py index 3fa1b04cb..add28aea0 100644 --- a/tasks/HRI/launch/HRI.launch.py +++ b/tasks/HRI/launch/HRI.launch.py @@ -40,6 +40,13 @@ def generate_launch_description(): name="lasr_vision_reid", output="screen", ) + + llm_service = Node( + package='lasr_llm', + executable='hri_task_service', + name='llm', + output='screen' + ) eye_tracker = Node( package="lasr_vision_eye_tracker", @@ -48,6 +55,14 @@ def generate_launch_description(): output="screen", ) + transcribe_speech = Node( + package='lasr_speech_recognition_whisper', + executable='transcribe_microphone_server', + name='whisper_mic_server', + output='screen', + + ) + state_machine = Node( package="HRI", executable="sm", @@ -66,5 +81,7 @@ def generate_launch_description(): vision_clip, eye_tracker, state_machine, + transcribe_speech, + llm_service, ] ) From 914bf96a85b76b989b2d109e92326696e26c943e Mon Sep 17 00:00:00 2001 From: Fadi <130671609+Fadi-Mostefai@users.noreply.github.com> Date: Sun, 14 Jun 2026 13:12:04 +0100 Subject: [PATCH 02/21] New VLM service to be used in HRI (#444) * vlm service * service update * Added back license and readme * Formatted files with black --------- Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi --- LICENSE | 0 README.md | 0 .../lasr_vlm/lasr_vlm/nodes/vlm_service.py | 97 ++++++++++ .../lasr_vlm/lasr_vlm/test_vlm_service.py | 182 ++++++++++++++++++ .../lasr_vlm/lasr_vlm/vlm_inference.py | 8 +- .../lasr_vlm/requiremenets.in | 1 - .../lasr_vlm/requiremenets.txt | 37 ---- common/foundation_models/lasr_vlm/setup.py | 3 +- .../lasr_vlm_interfaces/CMakeLists.txt | 40 ++++ .../lasr_vlm_interfaces/package.xml | 22 +++ .../srv/VlmDescribePeople.srv | 7 + .../lasr_llm/lasr_llm/llm_inference.py | 6 +- .../lasr_llm/nodes/hri_task_service.py | 4 +- common/language/lasr_llm/setup.py | 2 +- .../cropped_detection.py | 38 ++-- .../eye_tracker_action_server.py | 28 +-- common/vision/lasr_vision_yolo/setup.py | 2 + skills/src/lasr_skills/ask_and_listen.py | 16 +- skills/src/lasr_skills/describe_people.py | 14 +- skills/src/lasr_skills/detect_3d.py | 17 +- skills/src/lasr_skills/detect_3d_in_area.py | 12 +- .../src/lasr_skills/detect_all_in_polygon.py | 26 +-- skills/src/lasr_skills/eye_tracker.py | 9 +- skills/src/lasr_skills/receive_object.py | 22 +-- .../src/lasr_skills/vision/crop_image_3d.py | 14 +- skills/src/lasr_skills/vision/get_image.py | 24 ++- .../lasr_skills/wait_for_person_in_area.py | 1 - tasks/HRI/HRI/state_machine.py | 18 +- tasks/HRI/HRI/states/get_attributes.py | 4 +- tasks/HRI/HRI/states/get_name_and_drink.py | 7 +- tasks/HRI/HRI/states/greet.py | 2 +- tasks/HRI/HRI/states/hri_learn_faces.py | 2 +- tasks/HRI/HRI/states/seat_guest.py | 12 +- tasks/HRI/HRI/states/start_door_sm.py | 4 +- tasks/HRI/launch/HRI.launch.py | 16 +- 35 files changed, 508 insertions(+), 189 deletions(-) mode change 100755 => 100644 LICENSE mode change 100755 => 100644 README.md create mode 100644 common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py create mode 100644 common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py delete mode 100644 common/foundation_models/lasr_vlm/requiremenets.in delete mode 100644 common/foundation_models/lasr_vlm/requiremenets.txt create mode 100644 common/foundation_models/lasr_vlm_interfaces/CMakeLists.txt create mode 100644 common/foundation_models/lasr_vlm_interfaces/package.xml create mode 100644 common/foundation_models/lasr_vlm_interfaces/srv/VlmDescribePeople.srv 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..e7deb4e5c --- /dev/null +++ b/common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +import os +import tempfile + +import cv2 +import rclpy +from cv_bridge import CvBridge +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=True) + + self.bridge = CvBridge() + self.get_logger().info("VLM Describe People service started") + + 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.bridge.imgmsg_to_cv2(request.image_raw, "bgr8") + + 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..dab7e32f4 --- /dev/null +++ b/common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py @@ -0,0 +1,182 @@ +#!/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 + +# ─── 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 cv_bridge import CvBridge + from lasr_vlm_interfaces.srv import VlmDescribePeople + + class VlmTestClient(Node): + def __init__(self): + super().__init__("vlm_test_client") + self.bridge = CvBridge() + self.client = self.create_client(VlmDescribePeople, "/vlm/describe_people") + + 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.bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") + + 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/setup.py b/common/foundation_models/lasr_vlm/setup.py index a166096ff..8b0325839 100644 --- a/common/foundation_models/lasr_vlm/setup.py +++ b/common/foundation_models/lasr_vlm/setup.py @@ -22,6 +22,7 @@ def run(self): setup( + cmdclass={"install": InstallCommand}, name=package_name, version="0.0.0", packages=find_packages(exclude=["test"]), @@ -42,6 +43,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..e5ddd4a8b 100644 --- a/common/language/lasr_llm/lasr_llm/llm_inference.py +++ b/common/language/lasr_llm/lasr_llm/llm_inference.py @@ -71,8 +71,10 @@ def __init__(self, model_config: ModelConfig): print(f"Using device: {self.device}") 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) + cache_dir = "/home/fadi/.cache/huggingface/hub" + self.tokenizer = AutoTokenizer.from_pretrained( + self.model_name, local_files_only=True + ) self.logger = logging.getLogger(__name__) 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..87fc8ccf1 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,9 @@ 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", quantize=False + ) self.llm_inference = LLMInference(config) self.get_logger().info("HRI Task Query LLM service started") 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/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 620fa09c8..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 @@ -326,40 +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, - ) - + 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, - ) - + 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( + 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 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 70994cd5a..ce9d0a870 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 @@ -52,20 +52,20 @@ def __init__(self, max_eye_distance: float = 1.5): self._max_eye_distance: float = max_eye_distance self._move_up_count: float = 0.0 self._max_move_up_count: int = 2 - + 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", @@ -220,14 +220,14 @@ def _execute_callback(self, goal_handle): self.get_logger().info("Beginning eye tracking...") goal = goal_handle.request - + if goal.cancel: - self.get_logger().info('Cancelling eye tracker') + self.get_logger().info("Cancelling eye tracker") self._done = True goal_handle.succeed() # self.destroy_node() return EyeTrackerAction.Result() - + if self._robot_point is None: self.get_logger().warn( "No /robot_pose received yet; continuing and waiting asynchronously." @@ -325,23 +325,13 @@ def detect_cb(image: Image, depth_image: Image, depth_camera_info: CameraInfo): self._eyes = closest_eye_midpoint image_sub = message_filters.Subscriber( - self, - Image, - "/head_front_camera/rgb/image_raw", - self.camera_qos - + 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 + 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 + 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 diff --git a/common/vision/lasr_vision_yolo/setup.py b/common/vision/lasr_vision_yolo/setup.py index 4f9c5b2c2..e304b8d9c 100755 --- a/common/vision/lasr_vision_yolo/setup.py +++ b/common/vision/lasr_vision_yolo/setup.py @@ -8,6 +8,7 @@ package_name = "lasr_vision_yolo" + class InstallCommand(setuptools.command.install.install): def run(self): super().run() @@ -20,6 +21,7 @@ def run(self): ) return + setup( name=package_name, version="0.0.0", diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 9cfe4e365..4ab9cb0d7 100644 --- a/skills/src/lasr_skills/ask_and_listen.py +++ b/skills/src/lasr_skills/ask_and_listen.py @@ -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/describe_people.py b/skills/src/lasr_skills/describe_people.py index 6bd0f991e..35cf7e93e 100755 --- a/skills/src/lasr_skills/describe_people.py +++ b/skills/src/lasr_skills/describe_people.py @@ -40,15 +40,15 @@ def __init__(self): def _get_attr(self, blackboard): try: - if blackboard['clip_index'] < 3: - blackboard['clip_index'] += 1 - return 'continue' + if blackboard["clip_index"] < 3: + blackboard["clip_index"] += 1 + return "continue" else: - return 'succeeded' + return "succeeded" except RuntimeError: - blackboard['clip_index'] = 0 - blackboard['clip_detection_dict'] = {} - return 'continue' + blackboard["clip_index"] = 0 + blackboard["clip_detection_dict"] = {} + return "continue" class GetClipAttributes(yasmin_ros.ServiceState): diff --git a/skills/src/lasr_skills/detect_3d.py b/skills/src/lasr_skills/detect_3d.py index 861349494..cbcce7b5b 100644 --- a/skills/src/lasr_skills/detect_3d.py +++ b/skills/src/lasr_skills/detect_3d.py @@ -64,28 +64,28 @@ def __init__( self.cam_info = None self.data = None self.image_msg = None - + self.node.create_subscription( CameraInfo, self.depth_camera_info_topic, self._cache_camera_info, qos_profile=camera_qos, ) - + 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 ) - + 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) @@ -97,7 +97,7 @@ def _cache_camera_info(self, msg: CameraInfo) -> None: def _create_req(self, blackboard): self.data = None self.image_msg = None - + if self.cam_info is None: deadline = time.time() + 5.0 while self.cam_info is None and time.time() < deadline: @@ -117,7 +117,7 @@ def _create_req(self, blackboard): ) return "failed" time.sleep(0.25) - + image_msg, depth_msg = self.data req = YoloDetection3D.Request( @@ -130,7 +130,6 @@ def _create_req(self, blackboard): target_frame=self.target_frame, ) self.image_msg = image_msg - return req diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py index 2c03a4a0e..6d7ccd540 100644 --- a/skills/src/lasr_skills/detect_3d_in_area.py +++ b/skills/src/lasr_skills/detect_3d_in_area.py @@ -69,13 +69,17 @@ def execute(self, blackboard): PolygonStamped(polygon=polygon_msg, header=Header(frame_id="map")) ) - pub = yasmin_ros.logger_node.create_publisher( #CHECK: New publisher each time? declare in __init__ instead? + 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": #CHECK: Potential broken? float vs string? - yasmin.YASMIN_LOG_WARN("NAN detection check work") # Remove line if works + if ( + detection.point.x == "nan" + ): # CHECK: Potential broken? float vs string? + yasmin.YASMIN_LOG_WARN( + "NAN detection check work" + ) # Remove line if works continue yasmin.YASMIN_LOG_INFO( f"Detected a {detection.name} at x:{detection.point.x}, y:{detection.point.y}, z:{detection.point.z}" @@ -126,7 +130,7 @@ def __init__( z_min: Optional[float] = None, z_max: Optional[float] = None, ): - + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) if area_polygon is None: self.add_input_key("polygon") diff --git a/skills/src/lasr_skills/detect_all_in_polygon.py b/skills/src/lasr_skills/detect_all_in_polygon.py index a3c720f6d..575fd95c8 100644 --- a/skills/src/lasr_skills/detect_all_in_polygon.py +++ b/skills/src/lasr_skills/detect_all_in_polygon.py @@ -149,7 +149,7 @@ def __init__( self._min_coverage = min_coverage self._z_axis = z_axis self._fov_depth = fov_depth - + qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, durability=DurabilityPolicy.VOLATILE, @@ -159,7 +159,12 @@ def __init__( self.node = yasmin_ros.logger_node self.msg = None - self.node.create_subscription(CameraInfo, "/head_front_camera/depth/camera_info", self.info_cb, qos_profile=qos) + self.node.create_subscription( + CameraInfo, + "/head_front_camera/depth/camera_info", + self.info_cb, + qos_profile=qos, + ) self._tf_buffer = tf2_ros.Buffer(Duration(seconds=10.0)) self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self.node) @@ -309,9 +314,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) @@ -410,9 +413,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", @@ -527,7 +530,7 @@ 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, @@ -652,6 +655,7 @@ def _init_bb(blackboard): }, ) + class Detect_node(Node): def __init__(self): super().__init__( @@ -663,6 +667,7 @@ def __init__(self): self._spin_thread = Thread(target=self._executor.spin) self._spin_thread.start() + def main(): seat_area = [ [0.37787461280822754, -3.057680130004883], @@ -674,9 +679,9 @@ def main(): seat_polygon = ShapelyPolygon(seat_area) rclpy.init() - + node = Detect_node() - + yasmin_ros.set_ros_loggers(node) bb = Blackboard() @@ -707,7 +712,6 @@ def main(): node.destroy_node() rclpy.shutdown() - node.destroy_node() rclpy.shutdown() diff --git a/skills/src/lasr_skills/eye_tracker.py b/skills/src/lasr_skills/eye_tracker.py index 70dd79653..45123f5c4 100644 --- a/skills/src/lasr_skills/eye_tracker.py +++ b/skills/src/lasr_skills/eye_tracker.py @@ -14,8 +14,8 @@ def __init__(self): response_timeout=1.0, maximum_retry=0, ) - - self.add_input_key('person_point') + + self.add_input_key("person_point") def create_goal(self, blackboard): goal_msg = EyeTrackerAction.Goal() @@ -33,9 +33,6 @@ def __init__(self): response_timeout=1.0, maximum_retry=0, ) - - + def _create_goal(self, blackboard): return EyeTrackerAction.Goal(cancel=True) - - diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py index 7cd293392..c2d656746 100755 --- a/skills/src/lasr_skills/receive_object.py +++ b/skills/src/lasr_skills/receive_object.py @@ -3,7 +3,7 @@ import yasmin from yasmin import StateMachine, Blackboard -import yasmin_ros +import yasmin_ros from yasmin_ros import ServiceState, ActionState from std_srvs.srv import Empty @@ -12,18 +12,20 @@ from typing import Union + class ClearOctomap(ServiceState): - def __init__(self): + def __init__(self): super().__init__( srv_type=Empty, srv_name="/clear_octomap", create_request_handler=self._create_request, ) - + def _create_request(self, blackboard): return Empty.Request() -#TODO: Do we need to detect object or just assume that the second guest is holding a bag. + +# TODO: Do we need to detect object or just assume that the second guest is holding a bag. class ReceiveObject(StateMachine): def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): @@ -34,10 +36,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): self.add_state( "CLEAR_OCTOMAP", ClearOctomap(), - transitions={ - "succeeded": "LOOK_LEFT", - "aborted": "failed" - }, + transitions={"succeeded": "LOOK_LEFT", "aborted": "failed"}, ) self.add_state( @@ -176,8 +175,8 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): }, ) - #TODO: No longer a gripper server for this smach_ros.ServiceState("/parallel_gripper_controller/grasp", Empty) - # Alternatively: + # 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 @@ -192,7 +191,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): # }, # ) self.add_state( - "CLOSE_HALF_GRIPPER", # TEMPORARY REPLACEMENT - using half to not jam an item between gripper + "CLOSE_HALF_GRIPPER", # TEMPORARY REPLACEMENT - using half to not jam an item between gripper PlayMotion(motion_name="close_half"), transitions={ "succeeded": "FOLD_ARM", @@ -235,4 +234,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/skills/src/lasr_skills/vision/crop_image_3d.py b/skills/src/lasr_skills/vision/crop_image_3d.py index 54b7c9d89..958ffc6c7 100644 --- a/skills/src/lasr_skills/vision/crop_image_3d.py +++ b/skills/src/lasr_skills/vision/crop_image_3d.py @@ -65,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,17 +77,19 @@ def __init__( reliability=ReliabilityPolicy.BEST_EFFORT, ), ) - + 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) + + self.node.create_subscription( + PoseWithCovarianceStamped, "amcl_pose", self.pose_cb, qos_profile=amcl_qos + ) if self.crop_type not in ["masked", "bbox"]: raise ValueError( @@ -106,7 +108,7 @@ def execute(self, blackboard): if not detections: yasmin.YASMIN_LOG_WARN("No 3D detections found.") return "failed" - + attempt = 0 while self.robot_pose_msg is None: if attempt > 5: diff --git a/skills/src/lasr_skills/vision/get_image.py b/skills/src/lasr_skills/vision/get_image.py index 89f81abe5..f0515b143 100755 --- a/skills/src/lasr_skills/vision/get_image.py +++ b/skills/src/lasr_skills/vision/get_image.py @@ -15,7 +15,7 @@ class GetImage(State): State for reading an sensor_msgs Image message """ - def __init__(self, topic = 'head_front_camera/rgb/image_raw'): + def __init__(self, topic="head_front_camera/rgb/image_raw"): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("img_msg") @@ -26,12 +26,14 @@ def __init__(self, topic = 'head_front_camera/rgb/image_raw'): reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, ) - + self.node = yasmin_ros.logger_node - + self.msg = None - - self.node.create_subscription(Image, topic, self.image_cb, qos_profile=self.camera_qos) + + self.node.create_subscription( + Image, topic, self.image_cb, qos_profile=self.camera_qos + ) def image_cb(self, msg): self.msg = msg @@ -40,7 +42,7 @@ def execute(self, blackboard): try: blackboard["img_msg"] = self.msg - return 'failed' if self.msg is None else 'succeeded' + return "failed" if self.msg is None else "succeeded" except Exception as e: yasmin.YASMIN_LOG_ERROR(str(e)) return "failed" @@ -48,6 +50,7 @@ def execute(self, blackboard): # UNUSED THROUGHOUT WHOLE REPO, MAYBE DELETE????? + class GetPointCloud(State): """ State for acquiring a PointCloud2 message. @@ -58,7 +61,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, @@ -82,7 +85,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" @@ -101,7 +107,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_in_area.py b/skills/src/lasr_skills/wait_for_person_in_area.py index dfdd007a7..bf69620de 100644 --- a/skills/src/lasr_skills/wait_for_person_in_area.py +++ b/skills/src/lasr_skills/wait_for_person_in_area.py @@ -8,7 +8,6 @@ from shapely import Polygon as ShapelyPolygon - class CheckForPerson(State): def __init__(self): super().__init__(outcomes=["done", "not_done"]) diff --git a/tasks/HRI/HRI/state_machine.py b/tasks/HRI/HRI/state_machine.py index d9663209a..d85a6d911 100644 --- a/tasks/HRI/HRI/state_machine.py +++ b/tasks/HRI/HRI/state_machine.py @@ -36,7 +36,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, @@ -114,19 +114,19 @@ def wait_cb(blackboard, msg): SeatGuest(learn_host=False), transitions={"succeeded": "CHECK", "failed": "failed"}, ) - + self.add_state( "CHECK", - yasmin.CbState(outcomes=["succeeded", 'GO_TO_DOOR_2'], callback=self.check), - transitions={"succeeded": 'succeeded', 'GO_TO_DOOR_2': 'GO_TO_DOOR_2'}, + yasmin.CbState(outcomes=["succeeded", "GO_TO_DOOR_2"], callback=self.check), + transitions={"succeeded": "succeeded", "GO_TO_DOOR_2": "GO_TO_DOOR_2"}, ) - + self.add_state( "GO_TO_DOOR_2", GoToLocation(location_param="door_pose"), transitions={"succeeded": "GREET_2", "failed": "failed"}, ) - + self.add_state( "GREET_2", # SM2: Greets guest LookAndGreetGuest(last_resort=False, guest_id="guest2"), @@ -135,14 +135,14 @@ def wait_cb(blackboard, msg): def check(self, blackboard): guest = blackboard["guest_data"][f"guest{self.guest_id}"] - yasmin.YASMIN_LOG_INFO(f'{self.guest_id}') + yasmin.YASMIN_LOG_INFO(f"{self.guest_id}") for key in guest.keys(): value = guest[key] yasmin.YASMIN_LOG_INFO(f"{key}: {value}") self.guest_id += 1 - return "GO_TO_DOOR_2" if self.guest_id == 2 else 'succeeded' + return "GO_TO_DOOR_2" if self.guest_id == 2 else "succeeded" def setup(self): start_con_sm = yasmin.Concurrence( @@ -214,8 +214,6 @@ def main(): bb["dataset"] = "hri" bb["drink_position"] = PointStamped() - - outcome = sm(bb) yasmin.YASMIN_LOG_INFO(f"State machine has ended with outcome {outcome}") diff --git a/tasks/HRI/HRI/states/get_attributes.py b/tasks/HRI/HRI/states/get_attributes.py index bdde91c35..d89d879bb 100644 --- a/tasks/HRI/HRI/states/get_attributes.py +++ b/tasks/HRI/HRI/states/get_attributes.py @@ -37,7 +37,9 @@ def __init__(self, guest_id: str): def execute(self, blackboard) -> str: try: - blackboard["guest_data"][self._guest_id]["attributes"] = blackboard["clip_detection_dict"] + blackboard["guest_data"][self._guest_id]["attributes"] = blackboard[ + "clip_detection_dict" + ] blackboard["guest_data"][self._guest_id]["detection"] = True return "succeeded" except Exception as e: diff --git a/tasks/HRI/HRI/states/get_name_and_drink.py b/tasks/HRI/HRI/states/get_name_and_drink.py index ebe464e2f..3a81a9683 100755 --- a/tasks/HRI/HRI/states/get_name_and_drink.py +++ b/tasks/HRI/HRI/states/get_name_and_drink.py @@ -32,15 +32,16 @@ def __init__(self, task, 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 + ) return "succeeded" diff --git a/tasks/HRI/HRI/states/greet.py b/tasks/HRI/HRI/states/greet.py index b8fc8e46b..0a1fad174 100644 --- a/tasks/HRI/HRI/states/greet.py +++ b/tasks/HRI/HRI/states/greet.py @@ -119,7 +119,7 @@ def __init__(self, last_resort, guest_id): "succeeded": "GREET_AND_ASK_GUEST", "aborted": "SAY_WAITING_FOR_GUEST", "canceled": "failed", - 'timeout': 'GREET_AND_ASK_GUEST', + "timeout": "GREET_AND_ASK_GUEST", }, ) self.add_state( diff --git a/tasks/HRI/HRI/states/hri_learn_faces.py b/tasks/HRI/HRI/states/hri_learn_faces.py index c70d7b539..4104316c3 100644 --- a/tasks/HRI/HRI/states/hri_learn_faces.py +++ b/tasks/HRI/HRI/states/hri_learn_faces.py @@ -108,7 +108,7 @@ def __init__(self, dataset_size: int): self._dataset_size = dataset_size def execute(self, blackboard): - if blackboard['num_images'] >= self._dataset_size: + if blackboard["num_images"] >= self._dataset_size: yasmin.YASMIN_LOG_INFO("Collected enough images for the guest.") return "succeeded" else: diff --git a/tasks/HRI/HRI/states/seat_guest.py b/tasks/HRI/HRI/states/seat_guest.py index 0989860f5..286a7542d 100644 --- a/tasks/HRI/HRI/states/seat_guest.py +++ b/tasks/HRI/HRI/states/seat_guest.py @@ -28,7 +28,7 @@ Say, Wait, DetectAllInPolygon, - StopEyeTracker + StopEyeTracker, ) from yasmin_viewer import YasminViewerPub @@ -260,7 +260,7 @@ def __init__( # TODO: Update to allow local paramters overriding ros param seating_area_minus_sofa = self.seating_area.difference(self.sofa_area) - + self.add_state( "SAY_FINDING_SEAT", Say(text="I will now find a seat for you."), @@ -386,7 +386,7 @@ def __init__( "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"}, ) @@ -498,12 +498,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__( @@ -517,6 +519,7 @@ def __init__(self): self._spin_thread = Thread(target=self._executor.spin) self._spin_thread.start() + def main(): rclpy.init() @@ -525,7 +528,7 @@ def main(): yasmin_ros.set_ros_loggers(node) try: - #TODO: Try with learn_host=True + # TODO: Try with learn_host=True sm = SeatGuest(learn_host=False) bb = Blackboard() @@ -550,7 +553,6 @@ def main(): }, } - YasminViewerPub(sm, "HRI_SM3") outcome = sm(bb) diff --git a/tasks/HRI/HRI/states/start_door_sm.py b/tasks/HRI/HRI/states/start_door_sm.py index 6794c458b..7a2e3369c 100644 --- a/tasks/HRI/HRI/states/start_door_sm.py +++ b/tasks/HRI/HRI/states/start_door_sm.py @@ -20,9 +20,7 @@ def __init__( location: Union[Pose, None] = None, location_param: Union[str, None] = "start_pose", ): - super().__init__( - outcomes=["succeeded", "failed"], handle_sigint=True - ) + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) self.add_state( "DETECT_DOOR_OPENING", diff --git a/tasks/HRI/launch/HRI.launch.py b/tasks/HRI/launch/HRI.launch.py index add28aea0..5ed6c13da 100644 --- a/tasks/HRI/launch/HRI.launch.py +++ b/tasks/HRI/launch/HRI.launch.py @@ -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( From ed93815bc7540b083a9db90282d8fd6542b553c1 Mon Sep 17 00:00:00 2001 From: Fadi <130671609+Fadi-Mostefai@users.noreply.github.com> Date: Sun, 14 Jun 2026 14:44:02 +0100 Subject: [PATCH 03/21] VLM service for HRI task (#445) * Working VLM Replaced old vqa for describing people with new vlm service * Formatted files with black --- .../lasr_vlm/lasr_vlm/nodes/vlm_service.py | 25 +++- .../lasr_vlm/lasr_vlm/test_vlm_service.py | 16 ++- .../lasr_vlm/requirements.txt | 1 + common/foundation_models/lasr_vlm/setup.py | 1 - skills/src/lasr_skills/describe_people.py | 113 ++++-------------- skills/src/lasr_skills/vision/get_image.py | 12 +- tasks/HRI/HRI/state_machine.py | 2 + tasks/HRI/HRI/states/get_attributes.py | 4 +- tasks/HRI/launch/HRI.launch.py | 12 +- 9 files changed, 75 insertions(+), 111 deletions(-) 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 index e7deb4e5c..9073875d6 100644 --- a/common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py +++ b/common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py @@ -3,8 +3,8 @@ import tempfile import cv2 +import numpy as np import rclpy -from cv_bridge import CvBridge from rclpy.node import Node from lasr_vlm_interfaces.srv import VlmDescribePeople @@ -31,11 +31,26 @@ def __init__(self): ) model_config = ModelConfig(model_name="moondream") - self.vlm = VLMInference(model_config, new_model=True) - - self.bridge = CvBridge() + 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. @@ -44,7 +59,7 @@ def describe_people_callback(self, request, response): self.get_logger().info("Received request to describe person") try: - cv_image = self.bridge.imgmsg_to_cv2(request.image_raw, "bgr8") + cv_image = self._image_msg_to_bgr8(request.image_raw) with tempfile.NamedTemporaryFile(suffix=".jpg", delete=False) as f: tmp_path = f.name 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 index dab7e32f4..a9363f8bb 100644 --- a/common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py +++ b/common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py @@ -15,6 +15,8 @@ import sys import argparse +import numpy as np + # ─── Layer 1: unit-test the parser ──────────────────────────────────────────── @@ -93,15 +95,23 @@ def test_service(image_path: str): import cv2 import rclpy from rclpy.node import Node - from cv_bridge import CvBridge + 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.bridge = CvBridge() 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): @@ -116,7 +126,7 @@ def run(self, image_path: str): self.get_logger().info(f"Image loaded: {cv_image.shape} from {image_path}") request = VlmDescribePeople.Request() - request.image_raw = self.bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") + request.image_raw = self._cv2_to_image_msg(cv_image) self.get_logger().info( "Sending request (Ollama inference may take ~10-30s)..." 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 8b0325839..339dee702 100644 --- a/common/foundation_models/lasr_vlm/setup.py +++ b/common/foundation_models/lasr_vlm/setup.py @@ -26,7 +26,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"]), diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py index 35cf7e93e..a99127198 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,21 @@ 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, + "shirt_color": response.shirt_color, + } + + blackboard["attributes"] = dict + + return "succeeded" diff --git a/skills/src/lasr_skills/vision/get_image.py b/skills/src/lasr_skills/vision/get_image.py index f0515b143..b03339570 100755 --- a/skills/src/lasr_skills/vision/get_image.py +++ b/skills/src/lasr_skills/vision/get_image.py @@ -9,6 +9,8 @@ from typing import Optional from sensor_msgs.msg import Image, PointCloud2 +import time + class GetImage(State): """ @@ -36,13 +38,19 @@ def __init__(self, topic="head_front_camera/rgb/image_raw"): ) def image_cb(self, msg): - self.msg = msg + if self.msg is None: + self.msg = msg def execute(self, blackboard): + self.msg = None + + while self.msg is None: + yasmin.YASMIN_LOG_INFO("Waiting for rgb frame") + time.sleep(1) try: blackboard["img_msg"] = self.msg - return "failed" if self.msg is None else "succeeded" + return "succeeded" except Exception as e: yasmin.YASMIN_LOG_ERROR(str(e)) return "failed" diff --git a/tasks/HRI/HRI/state_machine.py b/tasks/HRI/HRI/state_machine.py index d85a6d911..60bfa7581 100644 --- a/tasks/HRI/HRI/state_machine.py +++ b/tasks/HRI/HRI/state_machine.py @@ -198,12 +198,14 @@ def main(): "drink": "", "detection": False, "seating_detection": False, + "attributes": {}, }, "guest2": { "name": "", "drink": "", "detection": False, "seating_detection": False, + "attributes": {}, }, } diff --git a/tasks/HRI/HRI/states/get_attributes.py b/tasks/HRI/HRI/states/get_attributes.py index d89d879bb..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" diff --git a/tasks/HRI/launch/HRI.launch.py b/tasks/HRI/launch/HRI.launch.py index 5ed6c13da..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", ) @@ -74,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, ] ) From 7fc01ffe6018f35d50fab087385a4f704dee317a Mon Sep 17 00:00:00 2001 From: Illia Putintsev Date: Tue, 16 Jun 2026 16:00:36 +0100 Subject: [PATCH 04/21] and dishwasher and trash bin --- skills/src/lasr_skills/say.py | 2 +- tasks/pick_and_place/config/config.yaml | 55 ++++++- .../launch/pick_and_place.launch.py | 65 +++++++- .../pick_and_place/state_machine.py | 124 +++++++++------- .../pick_and_place/states/__init__.py | 1 + .../pick_and_place/states/choose_shelf.py | 2 +- .../states/decide_destination.py | 140 ++++++++++++++++++ .../pick_and_place/states/instruct_pick.py | 20 ++- .../pick_and_place/states/instruct_place.py | 36 +++-- .../states/select_and_visualize_object.py | 45 +++--- 10 files changed, 381 insertions(+), 109 deletions(-) create mode 100644 tasks/pick_and_place/pick_and_place/states/decide_destination.py 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/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index c7acf2288..ccf0d06b1 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -1,17 +1,62 @@ /**: ros__parameters: pick_and_place: + + # Category the referee designates as trash (announced on Setup Days). + # Objects of this category are routed to the trash bin. "" disables it. + trash_category: "snack" + + # Open-vocab query words for table detection (common nouns). + objects: ["cup", "can", "bottle", "bowl", "box"] + table: 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] + position: {x: 3.0, y: -2.7, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.23} + look_point: [5.25, 2.27, 0.78] 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] z_min: 0.7 z_max: 1.5 + + # ── DESTINATION 1: dishwasher (tableware + cutlery) ── + dishwasher: + pose: + position: {x: 1.1, y: 3.0, z: 0.0} # <-- ВСТАВ pose (ros2 topic echo --once /amcl_pose) + orientation: {x: 0.0, y: 0.0, z: -0.62, w: 0.78} + + # ── DESTINATION 2: trash bin ── + trash_bin: + pose: + position: {x: 0.0, y: 0.0, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + + # ── DESTINATION 3: cabinet (fake: boxes with objects) ── cabinet: pose: - position: {x: 0.0, y: 0.0, z: 0.0} + position: {x: 0.0, y: 0.0, z: 0.0} # <-- ВСТАВ pose 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 + + # Shelves — used by ScanShelves in the NEXT deliverable. + # shelf_order is the iteration list; shelves. holds per-shelf config. + # (Tune look_point / polygon / z when the fake cabinet is built.) + shelf_order: ["top", "middle", "bottom"] + shelves: + top: + torso_lift_joint: 0.30 + look_point: [0.0, 0.0, 1.0] + polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + z_min: 0.9 + z_max: 1.3 + middle: + torso_lift_joint: 0.15 + look_point: [0.0, 0.0, 0.7] + polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + z_min: 0.6 + z_max: 0.9 + bottom: + torso_lift_joint: 0.0 + look_point: [0.0, 0.0, 0.4] + polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + z_min: 0.3 + z_max: 0.6 \ 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..608e559a7 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,73 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +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" + """ + One-shot launch for the Pick and Place task. + + Brings up everything that used to be started in separate terminals EXCEPT + the robot platform itself: + - open-vocabulary detection service (open_vocab/detect) ← required + - detection visualiser (comes with the above) + - LLM category-fallback service (optional, use_llm:=true) + - the task state machine (state_machine) + - point-head stub (/head_controller/point_head_action) + + Still launch SEPARATELY (the platform, unchanged between runs): + - the simulator / robot bringup (camera, TF, controllers) + - nav2 + localisation (map, /amcl_pose) — GoToLocation needs this + + Start the task after open_vocab has finished loading its model: + 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") + + open_vocab_launch = os.path.join( + get_package_share_directory("lasr_vision_open_vocabulary"), + "launch", + "open_vocab.launch.py", ) + + use_llm = LaunchConfiguration("use_llm") + return LaunchDescription([ + DeclareLaunchArgument( + "use_llm", + default_value="false", + description="Also start the storing_groceries LLM service " + "(category fallback). Forced onto CPU to avoid GPU OOM. " + "Most groceries resolve via CATEGORY_MAP, so default off.", + ), + + # ── Perception: open-vocabulary detection (open_vocab/detect) ───────── + # Reuses lasr_vision_open_vocabulary's own params.yaml (model / device / + # weights). Keep your local fix there: grounding_dino_weights: '' and + # model_device set for your GPU. + IncludeLaunchDescription( + PythonLaunchDescriptionSource(open_vocab_launch), + ), + + # ── Optional: LLM category-fallback service (CPU-forced) ───────────── + Node( + condition=IfCondition(use_llm), + package="lasr_llm", + executable="storing_groceries_service", + name="storing_groceries_query_llm_service", + output="screen", + additional_env={"CUDA_VISIBLE_DEVICES": ""}, + ), + + # ── Task: state machine ────────────────────────────────────────────── Node( package="pick_and_place", executable="state_machine", @@ -16,6 +75,8 @@ def generate_launch_description(): output="screen", parameters=[config], ), + + # ── Head stub: serves /head_controller/point_head_action ───────────── Node( package="pick_and_place", executable="point_head_stub", 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..94afbd4ec 100644 --- a/tasks/pick_and_place/pick_and_place/state_machine.py +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -11,11 +11,10 @@ from pick_and_place.states import ( Start, - ScanShelves, - FindAndGoToTable, DetectObjects, SelectAndVisualiseObject, ClassifyCategory, + DecideDestination, ChooseShelf, InstructPick, InstructPlace, @@ -25,25 +24,36 @@ class PickAndPlace(yasmin.StateMachine): """ - Main state machine for the Pick and Place task. + Main state machine for the Pick and Place task (announce-only). - Physical manipulation is delegated to a human operator - via verbal instructions — the robot perceives, reasons, and speaks. + Physical manipulation is delegated to a human operator via verbal + instructions — the robot perceives, reasons, routes, and speaks. + + Each detected table object is routed to one of THREE destinations + (the task-planning core of the challenge): + - tableware / cutlery → dishwasher + - the trash category → trash bin + - everything else → cabinet (matched to a shelf) 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) + START (start signal, door, drive to table) + → DETECT_OBJECTS (open-vocab detect all table objects, ONCE) + ┌→ SELECT_OBJECT (pop next object; empty → FINISH) + │ → CLASSIFY_CATEGORY (determine object category) + │ → DECIDE_DESTINATION(dishwasher / trash bin / cabinet) + │ ├ cabinet → CHOOSE_SHELF + │ └ other ─────────────┐ + │ → INSTRUCT_PICK ←───────────┘ + │ → GO_TO_DESTINATION (drive to chosen destination pose) + │ → INSTRUCT_PLACE + │ → GO_TO_TABLE (drive back to table) + └───(loop) + → FINISH (announce completion) → succeeded + + NOTE: ScanShelves (perceiving the cabinet shelves and announcing their + categories) is added in the next iteration; until then shelf_data is {} + and ChooseShelf uses its category-name fallback. """ def __init__(self): @@ -53,34 +63,13 @@ def __init__(self): self.add_state( "START", Start(), - transitions={ - "succeeded": "SCAN_SHELVES", - "failed": "failed", - }, - ) - - # ── Scan cabinet shelves (done once) ────────────────────────────────── - self.add_state( - "SCAN_SHELVES", - ScanShelves(), transitions={ "succeeded": "DETECT_OBJECTS", "failed": "failed", }, ) - # ── Find and navigate to table ──────────────────────────────────────── - self.add_state( - "FIND_AND_GO_TO_TABLE", - FindAndGoToTable(), - transitions={ - "succeeded": "DETECT_OBJECTS", - "failed": "DETECT_OBJECTS", # proceed even if table not found - }, - ) - - # ── Detect all objects on table ─────────────────────────────────────── - # Re-entered at the top of every loop iteration + # ── Detect all objects on the table (done ONCE) ─────────────────────── self.add_state( "DETECT_OBJECTS", DetectObjects(), @@ -90,13 +79,13 @@ def __init__(self): }, ) - # ── Select object and visualise for referee ─────────────────────────── + # ── Select next object and visualise for referee ────────────────────── self.add_state( "SELECT_OBJECT", SelectAndVisualiseObject(), transitions={ "succeeded": "CLASSIFY_CATEGORY", - "failed": "DETECT_OBJECTS", # re-scan if nothing to select + "finished": "FINISH", # all objects processed }, ) @@ -105,19 +94,29 @@ def __init__(self): "CLASSIFY_CATEGORY", ClassifyCategory(task="object"), transitions={ - "succeeded": "CHOOSE_SHELF", - "failed": "CHOOSE_SHELF", # proceed with unknown category - "empty": "DETECT_OBJECTS", # nothing to classify, re-scan + "succeeded": "DECIDE_DESTINATION", + "failed": "DECIDE_DESTINATION", # proceed with unknown category + "empty": "SELECT_OBJECT", # nothing to classify, next object + }, + ) + + # ── Decide destination: dishwasher / trash bin / cabinet ────────────── + self.add_state( + "DECIDE_DESTINATION", + DecideDestination(), + transitions={ + "cabinet": "CHOOSE_SHELF", + "other": "INSTRUCT_PICK", }, ) - # ── Choose which shelf to place object on ───────────────────────────── + # ── Choose which cabinet shelf to place object on ───────────────────── self.add_state( "CHOOSE_SHELF", ChooseShelf(), transitions={ "succeeded": "INSTRUCT_PICK", - "failed": "DETECT_OBJECTS", + "failed": "INSTRUCT_PICK", # announce anyway }, ) @@ -126,18 +125,18 @@ def __init__(self): "INSTRUCT_PICK", InstructPick(), transitions={ - "succeeded": "GO_TO_CABINET", + "succeeded": "GO_TO_DESTINATION", "failed": "INSTRUCT_PICK", # retry instruction }, ) - # ── Navigate to cabinet ─────────────────────────────────────────────── + # ── Navigate to the chosen destination (pose set by DecideDestination)─ self.add_state( - "GO_TO_CABINET", - GoToLocation(location_param="pick_and_place.cabinet.pose"), + "GO_TO_DESTINATION", + GoToLocation(), # reads blackboard["location"] transitions={ "succeeded": "INSTRUCT_PLACE", - "failed": "GO_TO_CABINET", # retry navigation + "failed": "INSTRUCT_PLACE", # announce even if nav failed }, ) @@ -156,8 +155,22 @@ def __init__(self): "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 + "succeeded": "SELECT_OBJECT", # loop back for next object + "failed": "GO_TO_TABLE", # retry navigation + }, + ) + + # ── Done ────────────────────────────────────────────────────────────── + self.add_state( + "FINISH", + Say( + text="I have sorted all the objects I could see on the table. " + "Pick and place complete." + ), + transitions={ + "succeeded": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", }, ) @@ -166,7 +179,7 @@ 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) @@ -196,6 +209,9 @@ def main(): 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"] = [] 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..a2f1eed48 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,7 @@ 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 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..a30486bad 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 @@ -51,7 +51,7 @@ def execute(self, blackboard) -> str: 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_str"] = "" yasmin.YASMIN_LOG_WARN( f"No shelf data (scan skipped) — defaulting to " f"the {object_category} shelf." 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/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..c36d09b9f 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,32 @@ 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}. " + f"I will give you 5 seconds. 5.. 4.. 3.. 2.. 1.." + ) say = Say(text=text) outcome = say.execute(blackboard) 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..f43814365 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 @@ -10,26 +10,25 @@ 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"]) + super().__init__(outcomes=["succeeded", "finished"]) self.add_input_key("detected_objects") self.add_output_key("selected_object") self.add_output_key("selected_object_name") @@ -61,27 +60,27 @@ def execute(self, blackboard) -> str: 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] + # 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" + + # Pop the next object so the loop advances on each iteration. + 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) From aa0d4192f8b33334ddc8c486ee805a73e9579794 Mon Sep 17 00:00:00 2001 From: Illia Putintsev Date: Tue, 16 Jun 2026 16:06:04 +0100 Subject: [PATCH 05/21] config for sim --- tasks/pick_and_place/config/config.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index ccf0d06b1..5581737e8 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -28,14 +28,14 @@ # ── DESTINATION 2: trash bin ── trash_bin: pose: - position: {x: 0.0, y: 0.0, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + position: {x: 6.0, y: -2.23, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.91, w: 0.41} # ── DESTINATION 3: cabinet (fake: boxes with objects) ── cabinet: pose: - position: {x: 0.0, y: 0.0, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + position: {x: -4.9, y: 2.7, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.97, w: 0.23} # Shelves — used by ScanShelves in the NEXT deliverable. # shelf_order is the iteration list; shelves. holds per-shelf config. From 04cc72c3769454be138e077c807b4f155aa7ae26 Mon Sep 17 00:00:00 2001 From: Yara Alkhelaiwi Date: Thu, 18 Jun 2026 16:52:11 +0100 Subject: [PATCH 06/21] Breakfast SM --- skills/src/lasr_skills/say.py | 2 +- .../states/detect_bowl_or_spoon.py | 103 +++++++++ .../states/detect_cereal_or_milk.py | 104 +++++++++ .../states/select_object_by_name.py | 113 +++++++++ .../pick_and_place/states/serve_breakfast.py | 214 ++++++++++++++++++ 5 files changed, 535 insertions(+), 1 deletion(-) create mode 100644 tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py create mode 100644 tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py create mode 100644 tasks/pick_and_place/pick_and_place/states/select_object_by_name.py create mode 100644 tasks/pick_and_place/pick_and_place/states/serve_breakfast.py 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/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py b/tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py new file mode 100644 index 000000000..0faf4578c --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py @@ -0,0 +1,103 @@ +import yasmin +import yasmin_ros + +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon + +from lasr_skills import DetectAllInPolygon + + +class DetectBowlSpoon(yasmin.State): + """ + Looks at the breakfast surface and detects the bowl and spoon. + + Standalone state, hardcoded to the breakfast_surface location and + a fixed object filter, kept separate from the table-specific + DetectObjects so neither needs to change while the breakfast flow + is being built out. + + Reads from ROS 2 params: + breakfast_surface.look_point — x, y, z + breakfast_surface.polygon — flat [x0,y0, x1,y1, ...] + + Blackboard outputs: + detected_objects : List[Detection3D] + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("detected_objects") + + self.node = yasmin_ros.logger_node + + try: + self._look_point = PointStamped( + point=Point( + x=self.node.get_parameter("breakfast_surface.look_point.x").value, + y=self.node.get_parameter("breakfast_surface.look_point.y").value, + z=self.node.get_parameter("breakfast_surface.look_point.z").value, + ), + header=Header(frame_id="map"), + ) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load breakfast_surface.look_point from params: {e}. " + "Using default (0, 0, 0.8)." + ) + self._look_point = PointStamped( + point=Point(x=0.0, y=0.0, z=0.8), + header=Header(frame_id="map"), + ) + + try: + polygon_flat = self.node.get_parameter("breakfast_surface.polygon").value + coords = list(zip(polygon_flat[::2], polygon_flat[1::2])) + self._polygon = ShapelyPolygon(coords) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load breakfast_surface.polygon from params: {e}. " + "Using empty polygon." + ) + self._polygon = ShapelyPolygon() + + self._object_filter = ["bowl", "spoon"] + + def execute(self, blackboard) -> str: + # TODO: call LookToPoint with self._look_point once ported to YASMIN + yasmin.YASMIN_LOG_INFO( + f"[TODO] Looking at breakfast surface at point " + f"({self._look_point.point.x:.2f}, " + f"{self._look_point.point.y:.2f}, " + f"{self._look_point.point.z:.2f})." + ) + + try: + detector = DetectAllInPolygon( + polygon=self._polygon, + object_filter=self._object_filter, + min_confidence=0.1, + # TODO: switch to robocup.pt or your competition model + model="yolo11n-seg.pt", + ) + + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] + + outcome = detector.execute(blackboard) + + if outcome == "failed": + yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed for breakfast surface.") + return "failed" + + detected = blackboard["detected_objects"] + labels = [obj.name for obj in detected] + yasmin.YASMIN_LOG_INFO( + f"Detected {len(detected)} object(s) on breakfast surface: {labels}." + ) + + return "succeeded" + + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Bowl/spoon detection failed: {e}") + return "failed" \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py b/tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py new file mode 100644 index 000000000..1debcaed3 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py @@ -0,0 +1,104 @@ +import yasmin +import yasmin_ros + +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon + +from lasr_skills import DetectAllInPolygon + + +class DetectCerealMilk(yasmin.State): + """ + Looks at the cabinet and detects the cereal and milk, which sit next + to their respective categories per the rulebook setup. + + Standalone state, hardcoded to the cabinet location and a fixed + object filter, kept separate from ScanShelves which builds the + general shelf category map rather than searching for two specific + named items. + + Reads from ROS 2 params: + cabinet.look_point — x, y, z + cabinet.polygon — flat [x0,y0, x1,y1, ...] + + Blackboard outputs: + detected_objects : List[Detection3D] + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("detected_objects") + + self.node = yasmin_ros.logger_node + + try: + self._look_point = PointStamped( + point=Point( + x=self.node.get_parameter("cabinet.look_point.x").value, + y=self.node.get_parameter("cabinet.look_point.y").value, + z=self.node.get_parameter("cabinet.look_point.z").value, + ), + header=Header(frame_id="map"), + ) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load cabinet.look_point from params: {e}. " + "Using default (0, 0, 0.8)." + ) + self._look_point = PointStamped( + point=Point(x=0.0, y=0.0, z=0.8), + header=Header(frame_id="map"), + ) + + try: + polygon_flat = self.node.get_parameter("cabinet.polygon").value + coords = list(zip(polygon_flat[::2], polygon_flat[1::2])) + self._polygon = ShapelyPolygon(coords) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load cabinet.polygon from params: {e}. " + "Using empty polygon." + ) + self._polygon = ShapelyPolygon() + + self._object_filter = ["cereal", "milk"] + + def execute(self, blackboard) -> str: + # TODO: call LookToPoint with self._look_point once ported to YASMIN + yasmin.YASMIN_LOG_INFO( + f"[TODO] Looking at cabinet at point " + f"({self._look_point.point.x:.2f}, " + f"{self._look_point.point.y:.2f}, " + f"{self._look_point.point.z:.2f})." + ) + + try: + detector = DetectAllInPolygon( + polygon=self._polygon, + object_filter=self._object_filter, + min_confidence=0.1, + # TODO: switch to robocup.pt or your competition model + model="yolo11n-seg.pt", + ) + + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] + + outcome = detector.execute(blackboard) + + if outcome == "failed": + yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed for cabinet.") + return "failed" + + detected = blackboard["detected_objects"] + labels = [obj.name for obj in detected] + yasmin.YASMIN_LOG_INFO( + f"Detected {len(detected)} object(s) in cabinet: {labels}." + ) + + return "succeeded" + + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Cereal/milk detection failed: {e}") + return "failed" \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/select_object_by_name.py b/tasks/pick_and_place/pick_and_place/states/select_object_by_name.py new file mode 100644 index 000000000..79e5b4bf4 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/select_object_by_name.py @@ -0,0 +1,113 @@ +import cv2 +import yasmin +import yasmin_ros + +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +from rclpy.qos import QoSProfile, DurabilityPolicy + + +class SelectObjectByName(yasmin.State): + """ + Selects a specific named object from the detected_objects list and + publishes a visualisation for the referee, same as + SelectAndVisualiseObject but matching by name instead of always + taking the first detection. + + Used for breakfast items, where a detection step returns more than + one known item at once (e.g. bowl and spoon detected together) and + a specific one needs to be picked out. + + Constructor args: + target_name : str — the object name to search for, e.g. "bowl" + + Blackboard inputs: + detected_objects : List[Detection3D] + + Blackboard outputs: + selected_object : Detection3D + selected_object_name : str + """ + + def __init__(self, target_name: str): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("detected_objects") + self.add_output_key("selected_object") + self.add_output_key("selected_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) + + def execute(self, blackboard) -> str: + detected = blackboard["detected_objects"] + + selected = None + for obj in detected: + if obj.name == self._target_name: + selected = obj + break + + if selected is None: + yasmin.YASMIN_LOG_WARN( + f"'{self._target_name}' not found in detected_objects." + ) + return "failed" + + blackboard["selected_object"] = selected + blackboard["selected_object_name"] = selected.name + + yasmin.YASMIN_LOG_INFO(f"Selected object: {selected.name}") + + self._publish_visualisation(selected) + + return "succeeded" + + def _publish_visualisation(self, detection) -> None: + """ + Grabs the latest camera image and draws a bounding box for the + referee. Same pattern as SelectAndVisualiseObject — no image is + attached to Detection3D, so it must be fetched separately. + """ + try: + import rclpy + 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: + yasmin.YASMIN_LOG_WARN("Could not get camera image for visualisation.") + return + + cv_im = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="rgb8") + xywh = detection.xywh + + cv2.rectangle( + cv_im, + (int(xywh[0]), int(xywh[1])), + (int(xywh[0] + xywh[2]), int(xywh[1] + xywh[3])), + (0, 255, 0), + 2, + ) + cv2.putText( + cv_im, + f"{detection.name} {detection.confidence:.2f}", + (int(xywh[0]), int(xywh[1] - 10)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 255, 0), + 2, + ) + + self._referee_pub.publish( + self._bridge.cv2_to_imgmsg(cv_im, encoding="rgb8") + ) + 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 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..b77b920e6 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -0,0 +1,214 @@ +import yasmin +import yasmin_ros + +from lasr_skills import GoToLocation, Say + +from pick_and_place.states.detect_bowl_spoon import DetectBowlSpoon +from pick_and_place.states.detect_cereal_milk import DetectCerealMilk +from pick_and_place.states.select_object_by_name import SelectObjectByName +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. + + Bowl and spoon are detected on a designated surface, cereal and milk + are detected in the cabinet next to their respective categories. + Every pick and place is delegated to the human operator -- detection + is used purely for recognition scoring and referee visualisation, + not for any manipulation. Placement instructions use fixed text via + InstructPlaceText since breakfast rules are rule-based, not + shelf-derived like the cleanup-phase InstructPlace. + + Sequence: + GO_TO_BREAKFAST_SURFACE + -> DETECT_BOWL_SPOON + -> SELECT_BOWL -> INSTRUCT_PICK_BOWL + -> SELECT_SPOON -> INSTRUCT_PICK_SPOON + -> GO_TO_TABLE + -> INSTRUCT_PLACE_BOWL (centre of table) + -> INSTRUCT_PLACE_SPOON (next to bowl) + -> GO_TO_CABINET + -> DETECT_CEREAL_MILK + -> SELECT_CEREAL -> INSTRUCT_PICK_CEREAL + -> SELECT_MILK -> INSTRUCT_PICK_MILK + -> GO_TO_TABLE + -> INSTRUCT_PLACE_CEREAL (next to bowl, with clearance) + -> INSTRUCT_PLACE_MILK (next to cereal, with clearance) + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + + self.add_state( + "GO_TO_BREAKFAST_SURFACE", + GoToLocation(location_param="breakfast_surface.pose"), + transitions={ + "succeeded": "DETECT_BOWL_SPOON", + "failed": "GO_TO_BREAKFAST_SURFACE", + }, + ) + + self.add_state( + "DETECT_BOWL_SPOON", + DetectBowlSpoon(), + transitions={ + "succeeded": "SELECT_BOWL", + "failed": "DETECT_BOWL_SPOON", + }, + ) + + self.add_state( + "SELECT_BOWL", + SelectObjectByName(target_name="bowl"), + transitions={ + "succeeded": "INSTRUCT_PICK_BOWL", + "failed": "DETECT_BOWL_SPOON", + }, + ) + + self.add_state( + "INSTRUCT_PICK_BOWL", + InstructPick(), + transitions={ + "succeeded": "SELECT_SPOON", + "failed": "INSTRUCT_PICK_BOWL", + }, + ) + + self.add_state( + "SELECT_SPOON", + SelectObjectByName(target_name="spoon"), + transitions={ + "succeeded": "INSTRUCT_PICK_SPOON", + "failed": "DETECT_BOWL_SPOON", + }, + ) + + self.add_state( + "INSTRUCT_PICK_SPOON", + InstructPick(), + transitions={ + "succeeded": "GO_TO_TABLE_1", + "failed": "INSTRUCT_PICK_SPOON", + }, + ) + + self.add_state( + "GO_TO_TABLE_1", + GoToLocation(location_param="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", + "failed": "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", + "failed": "GO_TO_CABINET", + "canceled": "GO_TO_CABINET", + }, + ) + + self.add_state( + "GO_TO_CABINET", + GoToLocation(location_param="cabinet.pose"), + transitions={ + "succeeded": "DETECT_CEREAL_MILK", + "failed": "GO_TO_CABINET", + }, + ) + + self.add_state( + "DETECT_CEREAL_MILK", + DetectCerealMilk(), + transitions={ + "succeeded": "SELECT_CEREAL", + "failed": "DETECT_CEREAL_MILK", + }, + ) + + self.add_state( + "SELECT_CEREAL", + SelectObjectByName(target_name="cereal"), + transitions={ + "succeeded": "INSTRUCT_PICK_CEREAL", + "failed": "DETECT_CEREAL_MILK", + }, + ) + + self.add_state( + "INSTRUCT_PICK_CEREAL", + InstructPick(), + transitions={ + "succeeded": "SELECT_MILK", + "failed": "INSTRUCT_PICK_CEREAL", + }, + ) + + self.add_state( + "SELECT_MILK", + SelectObjectByName(target_name="milk"), + transitions={ + "succeeded": "INSTRUCT_PICK_MILK", + "failed": "DETECT_CEREAL_MILK", + }, + ) + + self.add_state( + "INSTRUCT_PICK_MILK", + InstructPick(), + transitions={ + "succeeded": "GO_TO_TABLE_2", + "failed": "INSTRUCT_PICK_MILK", + }, + ) + + self.add_state( + "GO_TO_TABLE_2", + GoToLocation(location_param="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, " + "leaving at least five centimetres of clear space." + ), + transitions={ + "succeeded": "INSTRUCT_PLACE_MILK", + "failed": "INSTRUCT_PLACE_MILK", + "canceled": "INSTRUCT_PLACE_MILK", + }, + ) + + self.add_state( + "INSTRUCT_PLACE_MILK", + Say( + text="Please place the milk next to the cereal, " + "leaving at least five centimetres of clear space." + ), + transitions={ + "succeeded": "succeeded", + "failed": "succeeded", + "canceled": "succeeded", + }, + ) \ No newline at end of file From 4c716a93ccd5831f2c40e262a2ca4c251ce34c26 Mon Sep 17 00:00:00 2001 From: yarakmk Date: Thu, 18 Jun 2026 20:21:10 +0100 Subject: [PATCH 07/21] Serve Breakfast State Machine --- tasks/pick_and_place/config/config.yaml | 62 ++++++++- .../launch/serve_breakfast.launch.py | 27 ++++ .../pick_and_place/states/__init__.py | 3 +- .../states/detect_bowl_or_spoon.py | 103 -------------- .../states/detect_cereal_or_milk.py | 104 -------------- .../pick_and_place/states/detect_objects.py | 56 +++++--- .../states/select_and_visualize_object.py | 130 +++++++----------- .../states/select_object_by_name.py | 113 --------------- .../pick_and_place/states/serve_breakfast.py | 110 ++++++--------- .../pick_and_place/test_serve_breakfast.py | 29 ++++ tasks/pick_and_place/setup.py | 33 ++--- 11 files changed, 266 insertions(+), 504 deletions(-) create mode 100644 tasks/pick_and_place/launch/serve_breakfast.launch.py delete mode 100644 tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py delete mode 100644 tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py delete mode 100644 tasks/pick_and_place/pick_and_place/states/select_object_by_name.py create mode 100644 tasks/pick_and_place/pick_and_place/test_serve_breakfast.py diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index c7acf2288..b22962efe 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -1,17 +1,67 @@ /**: ros__parameters: pick_and_place: + + # Category the referee designates as trash (announced on Setup Days). + # Objects of this category are routed to the trash bin. "" disables it. + trash_category: "snack" + + # Open-vocab query words for table detection (common nouns). + objects: ["cup", "can", "bottle", "bowl", "box"] + table: 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] + position: {x: 3.0, y: -2.7, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.23} + look_point: [5.25, 2.27, 0.78] 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] z_min: 0.7 z_max: 1.5 + + # ── DESTINATION 1: dishwasher (tableware + cutlery) ── + dishwasher: + pose: + position: {x: 1.1, y: 3.0, z: 0.0} # <-- ВСТАВ pose (ros2 topic echo --once /amcl_pose) + orientation: {x: 0.0, y: 0.0, z: -0.62, w: 0.78} + + # ── DESTINATION 2: trash bin ── + trash_bin: + pose: + position: {x: 6.0, y: -2.23, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.91, w: 0.41} + + # ── DESTINATION 3: cabinet (fake: boxes with objects) ── 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: -4.9, y: 2.7, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.97, w: 0.23} + + # Shelves — used by ScanShelves in the NEXT deliverable. + # shelf_order is the iteration list; shelves. holds per-shelf config. + # (Tune look_point / polygon / z when the fake cabinet is built.) + shelf_order: ["top", "middle", "bottom"] + shelves: + top: + torso_lift_joint: 0.30 + look_point: [0.0, 0.0, 1.0] + polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + z_min: 0.9 + z_max: 1.3 + middle: + torso_lift_joint: 0.15 + look_point: [0.0, 0.0, 0.7] + polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + z_min: 0.6 + z_max: 0.9 + bottom: + torso_lift_joint: 0.0 + look_point: [0.0, 0.0, 0.4] + polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + z_min: 0.3 + z_max: 0.6 + # ── BREAKFAST SURFACE (bowl and spoon pickup location) ── + breakfast_surface: + pose: + position: {x: 1.62, y: -3.68, z: 0.0063} # TODO: get from Gazebo + orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.23} \ No newline at end of file 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/states/__init__.py b/tasks/pick_and_place/pick_and_place/states/__init__.py index 004fc900b..78ba69a19 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -6,4 +6,5 @@ from .classify_category import ClassifyCategory 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 diff --git a/tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py b/tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py deleted file mode 100644 index 0faf4578c..000000000 --- a/tasks/pick_and_place/pick_and_place/states/detect_bowl_or_spoon.py +++ /dev/null @@ -1,103 +0,0 @@ -import yasmin -import yasmin_ros - -from geometry_msgs.msg import Point, PointStamped -from std_msgs.msg import Header -from shapely import Polygon as ShapelyPolygon - -from lasr_skills import DetectAllInPolygon - - -class DetectBowlSpoon(yasmin.State): - """ - Looks at the breakfast surface and detects the bowl and spoon. - - Standalone state, hardcoded to the breakfast_surface location and - a fixed object filter, kept separate from the table-specific - DetectObjects so neither needs to change while the breakfast flow - is being built out. - - Reads from ROS 2 params: - breakfast_surface.look_point — x, y, z - breakfast_surface.polygon — flat [x0,y0, x1,y1, ...] - - Blackboard outputs: - detected_objects : List[Detection3D] - """ - - def __init__(self): - super().__init__(outcomes=["succeeded", "failed"]) - self.add_output_key("detected_objects") - - self.node = yasmin_ros.logger_node - - try: - self._look_point = PointStamped( - point=Point( - x=self.node.get_parameter("breakfast_surface.look_point.x").value, - y=self.node.get_parameter("breakfast_surface.look_point.y").value, - z=self.node.get_parameter("breakfast_surface.look_point.z").value, - ), - header=Header(frame_id="map"), - ) - except Exception as e: - yasmin.YASMIN_LOG_WARN( - f"Could not load breakfast_surface.look_point from params: {e}. " - "Using default (0, 0, 0.8)." - ) - self._look_point = PointStamped( - point=Point(x=0.0, y=0.0, z=0.8), - header=Header(frame_id="map"), - ) - - try: - polygon_flat = self.node.get_parameter("breakfast_surface.polygon").value - coords = list(zip(polygon_flat[::2], polygon_flat[1::2])) - self._polygon = ShapelyPolygon(coords) - except Exception as e: - yasmin.YASMIN_LOG_WARN( - f"Could not load breakfast_surface.polygon from params: {e}. " - "Using empty polygon." - ) - self._polygon = ShapelyPolygon() - - self._object_filter = ["bowl", "spoon"] - - def execute(self, blackboard) -> str: - # TODO: call LookToPoint with self._look_point once ported to YASMIN - yasmin.YASMIN_LOG_INFO( - f"[TODO] Looking at breakfast surface at point " - f"({self._look_point.point.x:.2f}, " - f"{self._look_point.point.y:.2f}, " - f"{self._look_point.point.z:.2f})." - ) - - try: - detector = DetectAllInPolygon( - polygon=self._polygon, - object_filter=self._object_filter, - min_confidence=0.1, - # TODO: switch to robocup.pt or your competition model - model="yolo11n-seg.pt", - ) - - blackboard["detected_objects"] = [] - blackboard["debug_images"] = [] - - outcome = detector.execute(blackboard) - - if outcome == "failed": - yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed for breakfast surface.") - return "failed" - - detected = blackboard["detected_objects"] - labels = [obj.name for obj in detected] - yasmin.YASMIN_LOG_INFO( - f"Detected {len(detected)} object(s) on breakfast surface: {labels}." - ) - - return "succeeded" - - except Exception as e: - yasmin.YASMIN_LOG_ERROR(f"Bowl/spoon detection failed: {e}") - return "failed" \ No newline at end of file diff --git a/tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py b/tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py deleted file mode 100644 index 1debcaed3..000000000 --- a/tasks/pick_and_place/pick_and_place/states/detect_cereal_or_milk.py +++ /dev/null @@ -1,104 +0,0 @@ -import yasmin -import yasmin_ros - -from geometry_msgs.msg import Point, PointStamped -from std_msgs.msg import Header -from shapely import Polygon as ShapelyPolygon - -from lasr_skills import DetectAllInPolygon - - -class DetectCerealMilk(yasmin.State): - """ - Looks at the cabinet and detects the cereal and milk, which sit next - to their respective categories per the rulebook setup. - - Standalone state, hardcoded to the cabinet location and a fixed - object filter, kept separate from ScanShelves which builds the - general shelf category map rather than searching for two specific - named items. - - Reads from ROS 2 params: - cabinet.look_point — x, y, z - cabinet.polygon — flat [x0,y0, x1,y1, ...] - - Blackboard outputs: - detected_objects : List[Detection3D] - """ - - def __init__(self): - super().__init__(outcomes=["succeeded", "failed"]) - self.add_output_key("detected_objects") - - self.node = yasmin_ros.logger_node - - try: - self._look_point = PointStamped( - point=Point( - x=self.node.get_parameter("cabinet.look_point.x").value, - y=self.node.get_parameter("cabinet.look_point.y").value, - z=self.node.get_parameter("cabinet.look_point.z").value, - ), - header=Header(frame_id="map"), - ) - except Exception as e: - yasmin.YASMIN_LOG_WARN( - f"Could not load cabinet.look_point from params: {e}. " - "Using default (0, 0, 0.8)." - ) - self._look_point = PointStamped( - point=Point(x=0.0, y=0.0, z=0.8), - header=Header(frame_id="map"), - ) - - try: - polygon_flat = self.node.get_parameter("cabinet.polygon").value - coords = list(zip(polygon_flat[::2], polygon_flat[1::2])) - self._polygon = ShapelyPolygon(coords) - except Exception as e: - yasmin.YASMIN_LOG_WARN( - f"Could not load cabinet.polygon from params: {e}. " - "Using empty polygon." - ) - self._polygon = ShapelyPolygon() - - self._object_filter = ["cereal", "milk"] - - def execute(self, blackboard) -> str: - # TODO: call LookToPoint with self._look_point once ported to YASMIN - yasmin.YASMIN_LOG_INFO( - f"[TODO] Looking at cabinet at point " - f"({self._look_point.point.x:.2f}, " - f"{self._look_point.point.y:.2f}, " - f"{self._look_point.point.z:.2f})." - ) - - try: - detector = DetectAllInPolygon( - polygon=self._polygon, - object_filter=self._object_filter, - min_confidence=0.1, - # TODO: switch to robocup.pt or your competition model - model="yolo11n-seg.pt", - ) - - blackboard["detected_objects"] = [] - blackboard["debug_images"] = [] - - outcome = detector.execute(blackboard) - - if outcome == "failed": - yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed for cabinet.") - return "failed" - - detected = blackboard["detected_objects"] - labels = [obj.name for obj in detected] - yasmin.YASMIN_LOG_INFO( - f"Detected {len(detected)} object(s) in cabinet: {labels}." - ) - - return "succeeded" - - except Exception as e: - yasmin.YASMIN_LOG_ERROR(f"Cereal/milk detection failed: {e}") - return "failed" \ 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..931797c63 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 @@ -51,26 +51,28 @@ class DetectObjects(yasmin.State): 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 + 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, queries: list = None): super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detected_objects") self.node = yasmin_ros.logger_node self.bridge = CvBridge() - - try: - q = list( - self.node.get_parameter("pick_and_place.objects") - .get_parameter_value() - .string_array_value - ) - self._queries = q or list(self.DEFAULT_QUERIES) - except Exception: - self._queries = list(self.DEFAULT_QUERIES) + if queries is not None: + self._queries = queries + else: + try: + q = list( + self.node.get_parameter("pick_and_place.objects") + .get_parameter_value() + .string_array_value + ) + self._queries = q or list(self.DEFAULT_QUERIES) + except Exception: + self._queries = list(self.DEFAULT_QUERIES) self._rgb = None self._depth = None @@ -91,7 +93,8 @@ def __init__(self): self._ovd = self.node.create_client(OpenVocabDetect, "open_vocab/detect") self._head = ActionClient( - self.node, FollowJointTrajectory, + self.node, + FollowJointTrajectory, "/head_controller/follow_joint_trajectory", ) @@ -144,12 +147,22 @@ def _clean_label(self, 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 + 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 + 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 @@ -183,13 +196,17 @@ def _project_3d(self, cx, cy): ps.point.z = d try: tr = self._tf.lookup_transform( - "map", cam_frame, self._rgb.header.stamp, + "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), + "map", + cam_frame, + ROS2Time(seconds=0), timeout=ROS2Duration(seconds=0.5), ) except Exception: @@ -250,9 +267,10 @@ def execute(self, blackboard): d3.point = pt detected.append(d3) + blackboard["last_rgb_image"] = self._rgb 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 + return "succeeded" if detected else "failed" 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..7df007a39 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,113 +1,89 @@ import cv2 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 +from rclpy.qos import QoSProfile, DurabilityPolicy 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. - - 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. - + Selects an object from the detected_objects list and publishes a + debug image with a bounding box to /referee_view so the referee can + confirm the robot's selection. + + Without a target_name, selects the first detected object — used by + the table/extra-surface cleanup loops where order doesn't matter. + With a target_name, selects the specific named object from the list + — used by breakfast setup, where DetectObjects(queries=["bowl","spoon"]) + can return either order and a specific one needs to be picked out. + Reuses the cached image set on the blackboard by DetectObjects + ("last_rgb_image") rather than re-fetching a fresh camera frame, + so the visualisation matches exactly what was detected. + + Constructor args: + target_name : str | None — object name to search for; None + + selects the first detection Blackboard inputs: detected_objects : List[Detection3D] - Output of DetectAllInPolygon — each item has .name, .xywh, - .confidence, and the raw image stored at index [2]. + last_rgb_image : Image — set by DetectObjects 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 """ - def __init__(self): + def __init__(self, target_name: str = None): super().__init__(outcomes=["succeeded", "failed"]) + 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() - - # Latched publisher so the referee view stays visible after publish - qos = QoSProfile( - depth=1, - durability=DurabilityPolicy.TRANSIENT_LOCAL, - ) + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) 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) - - - 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 + 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 "failed" + else: + # Default behaviour for cleanup loops — always take the first + selected = detected[0] + + blackboard["selected_object"] = selected blackboard["selected_object_name"] = selected.name - blackboard["object_name"] = selected.name yasmin.YASMIN_LOG_INFO(f"Selected object: {selected.name}") - - # ── 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) - + self._publish_visualisation(selected, blackboard) return "succeeded" - def _publish_visualisation(self, detection) -> None: + def _publish_visualisation(self, detection, blackboard) -> None: + """ + Draws a bounding box and label on the cached detection-time image + and publishes it to /referee_view, satisfying rule 16's perception + communication requirement. + """ 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: - yasmin.YASMIN_LOG_WARN("Could not get camera image for visualisation.") + image_msg = blackboard.get("last_rgb_image") + if image_msg is None: + yasmin.YASMIN_LOG_WARN("No cached image available for visualisation.") return - - label = detection.name - xywh = detection.xywh - confidence = detection.confidence - cv_im = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="rgb8") - + xywh = detection.xywh # top-left format from DetectObjects cv2.rectangle( cv_im, (int(xywh[0]), int(xywh[1])), @@ -115,20 +91,20 @@ def _publish_visualisation(self, detection) -> None: (0, 255, 0), 2, ) + cv2.putText( cv_im, - f"{label} {confidence:.2f}", + f"{detection.name} {detection.confidence:.2f}", (int(xywh[0]), int(xywh[1] - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2, ) - self._referee_pub.publish( self._bridge.cv2_to_imgmsg(cv_im, encoding="rgb8") ) 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/select_object_by_name.py b/tasks/pick_and_place/pick_and_place/states/select_object_by_name.py deleted file mode 100644 index 79e5b4bf4..000000000 --- a/tasks/pick_and_place/pick_and_place/states/select_object_by_name.py +++ /dev/null @@ -1,113 +0,0 @@ -import cv2 -import yasmin -import yasmin_ros - -from cv_bridge import CvBridge -from sensor_msgs.msg import Image -from rclpy.qos import QoSProfile, DurabilityPolicy - - -class SelectObjectByName(yasmin.State): - """ - Selects a specific named object from the detected_objects list and - publishes a visualisation for the referee, same as - SelectAndVisualiseObject but matching by name instead of always - taking the first detection. - - Used for breakfast items, where a detection step returns more than - one known item at once (e.g. bowl and spoon detected together) and - a specific one needs to be picked out. - - Constructor args: - target_name : str — the object name to search for, e.g. "bowl" - - Blackboard inputs: - detected_objects : List[Detection3D] - - Blackboard outputs: - selected_object : Detection3D - selected_object_name : str - """ - - def __init__(self, target_name: str): - super().__init__(outcomes=["succeeded", "failed"]) - self.add_input_key("detected_objects") - self.add_output_key("selected_object") - self.add_output_key("selected_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) - - def execute(self, blackboard) -> str: - detected = blackboard["detected_objects"] - - selected = None - for obj in detected: - if obj.name == self._target_name: - selected = obj - break - - if selected is None: - yasmin.YASMIN_LOG_WARN( - f"'{self._target_name}' not found in detected_objects." - ) - return "failed" - - blackboard["selected_object"] = selected - blackboard["selected_object_name"] = selected.name - - yasmin.YASMIN_LOG_INFO(f"Selected object: {selected.name}") - - self._publish_visualisation(selected) - - return "succeeded" - - def _publish_visualisation(self, detection) -> None: - """ - Grabs the latest camera image and draws a bounding box for the - referee. Same pattern as SelectAndVisualiseObject — no image is - attached to Detection3D, so it must be fetched separately. - """ - try: - import rclpy - 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: - yasmin.YASMIN_LOG_WARN("Could not get camera image for visualisation.") - return - - cv_im = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="rgb8") - xywh = detection.xywh - - cv2.rectangle( - cv_im, - (int(xywh[0]), int(xywh[1])), - (int(xywh[0] + xywh[2]), int(xywh[1] + xywh[3])), - (0, 255, 0), - 2, - ) - cv2.putText( - cv_im, - f"{detection.name} {detection.confidence:.2f}", - (int(xywh[0]), int(xywh[1] - 10)), - cv2.FONT_HERSHEY_SIMPLEX, - 0.5, - (0, 255, 0), - 2, - ) - - self._referee_pub.publish( - self._bridge.cv2_to_imgmsg(cv_im, encoding="rgb8") - ) - 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 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 index b77b920e6..0afbd9ad7 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -1,11 +1,8 @@ import yasmin import yasmin_ros - from lasr_skills import GoToLocation, Say - -from pick_and_place.states.detect_bowl_spoon import DetectBowlSpoon -from pick_and_place.states.detect_cereal_milk import DetectCerealMilk -from pick_and_place.states.select_object_by_name import SelectObjectByName +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 @@ -15,22 +12,23 @@ class ServeBreakfast(yasmin.StateMachine): Bowl and spoon are detected on a designated surface, cereal and milk are detected in the cabinet next to their respective categories. - Every pick and place is delegated to the human operator -- detection - is used purely for recognition scoring and referee visualisation, - not for any manipulation. Placement instructions use fixed text via - InstructPlaceText since breakfast rules are rule-based, not - shelf-derived like the cleanup-phase InstructPlace. + + Detection uses open-vocabulary DetectObjects with a custom query list + per stop, and SelectAndVisualiseObject picks each named item out of + the detected pair via target_name. Every pick and place is delegated + to the human operator -- detection is used purely for recognition + scoring and referee visualisation, not for any manipulation. Sequence: GO_TO_BREAKFAST_SURFACE - -> DETECT_BOWL_SPOON + -> DETECT_BOWL_SPOON (queries=["bowl", "spoon"]) -> SELECT_BOWL -> INSTRUCT_PICK_BOWL -> SELECT_SPOON -> INSTRUCT_PICK_SPOON -> GO_TO_TABLE -> INSTRUCT_PLACE_BOWL (centre of table) -> INSTRUCT_PLACE_SPOON (next to bowl) -> GO_TO_CABINET - -> DETECT_CEREAL_MILK + -> DETECT_CEREAL_MILK (queries=["cereal", "milk"]) -> SELECT_CEREAL -> INSTRUCT_PICK_CEREAL -> SELECT_MILK -> INSTRUCT_PICK_MILK -> GO_TO_TABLE @@ -40,175 +38,157 @@ class ServeBreakfast(yasmin.StateMachine): def __init__(self): super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) - self.add_state( "GO_TO_BREAKFAST_SURFACE", - GoToLocation(location_param="breakfast_surface.pose"), + GoToLocation(location_param="pick_and_place.breakfast_surface.pose"), transitions={ "succeeded": "DETECT_BOWL_SPOON", - "failed": "GO_TO_BREAKFAST_SURFACE", + "failed": "GO_TO_BREAKFAST_SURFACE", }, ) - self.add_state( "DETECT_BOWL_SPOON", - DetectBowlSpoon(), + DetectObjects(queries=["bowl", "spoon"]), transitions={ "succeeded": "SELECT_BOWL", - "failed": "DETECT_BOWL_SPOON", + "failed": "DETECT_BOWL_SPOON", }, ) - self.add_state( "SELECT_BOWL", - SelectObjectByName(target_name="bowl"), + SelectAndVisualiseObject(target_name="bowl"), transitions={ "succeeded": "INSTRUCT_PICK_BOWL", - "failed": "DETECT_BOWL_SPOON", + "failed": "DETECT_BOWL_SPOON", }, ) - self.add_state( "INSTRUCT_PICK_BOWL", InstructPick(), transitions={ "succeeded": "SELECT_SPOON", - "failed": "INSTRUCT_PICK_BOWL", + "failed": "INSTRUCT_PICK_BOWL", }, ) - self.add_state( "SELECT_SPOON", - SelectObjectByName(target_name="spoon"), + SelectAndVisualiseObject(target_name="spoon"), transitions={ "succeeded": "INSTRUCT_PICK_SPOON", - "failed": "DETECT_BOWL_SPOON", + "failed": "DETECT_BOWL_SPOON", }, ) - self.add_state( "INSTRUCT_PICK_SPOON", InstructPick(), transitions={ "succeeded": "GO_TO_TABLE_1", - "failed": "INSTRUCT_PICK_SPOON", + "failed": "INSTRUCT_PICK_SPOON", }, ) - self.add_state( "GO_TO_TABLE_1", - GoToLocation(location_param="table.pose"), + GoToLocation(location_param="pick_and_place.table.pose"), transitions={ "succeeded": "INSTRUCT_PLACE_BOWL", - "failed": "GO_TO_TABLE_1", + "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", - "failed": "INSTRUCT_PLACE_SPOON", - "canceled": "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", - "failed": "GO_TO_CABINET", - "canceled": "GO_TO_CABINET", + "aborted": "GO_TO_CABINET", + "canceled": "GO_TO_CABINET", }, ) - self.add_state( "GO_TO_CABINET", - GoToLocation(location_param="cabinet.pose"), + GoToLocation(location_param="pick_and_place.cabinet.pose"), transitions={ "succeeded": "DETECT_CEREAL_MILK", - "failed": "GO_TO_CABINET", + "failed": "GO_TO_CABINET", }, ) - self.add_state( "DETECT_CEREAL_MILK", - DetectCerealMilk(), + DetectObjects(queries=["cereal", "milk"]), transitions={ "succeeded": "SELECT_CEREAL", - "failed": "DETECT_CEREAL_MILK", + "failed": "DETECT_CEREAL_MILK", }, ) - self.add_state( "SELECT_CEREAL", - SelectObjectByName(target_name="cereal"), + SelectAndVisualiseObject(target_name="cereal"), transitions={ "succeeded": "INSTRUCT_PICK_CEREAL", - "failed": "DETECT_CEREAL_MILK", + "failed": "DETECT_CEREAL_MILK", }, ) - self.add_state( "INSTRUCT_PICK_CEREAL", InstructPick(), transitions={ "succeeded": "SELECT_MILK", - "failed": "INSTRUCT_PICK_CEREAL", + "failed": "INSTRUCT_PICK_CEREAL", }, ) - self.add_state( "SELECT_MILK", - SelectObjectByName(target_name="milk"), + SelectAndVisualiseObject(target_name="milk"), transitions={ "succeeded": "INSTRUCT_PICK_MILK", - "failed": "DETECT_CEREAL_MILK", + "failed": "DETECT_CEREAL_MILK", }, ) - self.add_state( "INSTRUCT_PICK_MILK", InstructPick(), transitions={ "succeeded": "GO_TO_TABLE_2", - "failed": "INSTRUCT_PICK_MILK", + "failed": "INSTRUCT_PICK_MILK", }, ) - self.add_state( "GO_TO_TABLE_2", - GoToLocation(location_param="table.pose"), + GoToLocation(location_param="pick_and_place.table.pose"), transitions={ "succeeded": "INSTRUCT_PLACE_CEREAL", - "failed": "GO_TO_TABLE_2", + "failed": "GO_TO_TABLE_2", }, ) - self.add_state( "INSTRUCT_PLACE_CEREAL", Say( text="Please place the cereal next to the bowl, " - "leaving at least five centimetres of clear space." + "leaving at least five centimetres of clear space." ), transitions={ "succeeded": "INSTRUCT_PLACE_MILK", - "failed": "INSTRUCT_PLACE_MILK", - "canceled": "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, " - "leaving at least five centimetres of clear space." + "leaving at least five centimetres of clear space." ), transitions={ "succeeded": "succeeded", - "failed": "succeeded", - "canceled": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", }, - ) \ 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..308d7ced1 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 +import rclpy +import yasmin +import yasmin_ros +from pick_and_place.states.serve_breakfast import ServeBreakfast + + +def main(): + rclpy.init() + yasmin_ros.set_ros_loggers() + + bb = yasmin.Blackboard() + + # Initialise all keys ServeBreakfast needs + bb["detected_objects"] = [] + bb["debug_images"] = [] + bb["selected_object"] = None + bb["selected_object_name"] = "" + bb["last_rgb_image"] = None + + sm = ServeBreakfast() + outcome = sm(bb) + + yasmin.YASMIN_LOG_INFO(f"ServeBreakfast finished with outcome: {outcome}") + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/tasks/pick_and_place/setup.py b/tasks/pick_and_place/setup.py index e82bef3c2..1f3bb966d 100644 --- a/tasks/pick_and_place/setup.py +++ b/tasks/pick_and_place/setup.py @@ -1,32 +1,33 @@ 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", ], }, From 4ebe401ef7cc8fb0669d9dd0a35a3453b459affd Mon Sep 17 00:00:00 2001 From: Fadi <130671609+Fadi-Mostefai@users.noreply.github.com> Date: Thu, 18 Jun 2026 20:41:08 +0100 Subject: [PATCH 08/21] HRI task (now SM1-5) (#447) * Ros2 Port Language Package * Introduction testing * Modifications. fix init before final push * Message filter * Launch file * State machine and setup.py modifications * Fix register face and recognise * Port HRI introduce states to YASMIN * Fix YOLO detections in introduce. * Initial code for pick and place * Bug fixes * Fixes after testing on the robot * Black formatting * HRI Task - SM1-3 fully tested and working on the robot (#443) * Testing HRI SM2 * LLM package with venv * Fixed SM3 * RVIZ debug for HRI testing * Fixed lasr_llm by removing broken llama_cpp_python * Successfull SM2 of HRI * WIP - SM2 to SM3 * WIP: CHECKS * WIP - SM1 -> SM3 Final checks to see if it works consistently needs to be carried out * Ported recieve Object * Temporary detect3d fix * Working SM1-3 of HRI --------- Co-authored-by: Aldrich-Fernandes * New VLM service to be used in HRI (#444) * vlm service * service update * Added back license and readme * Formatted files with black --------- Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi * VLM service for HRI task (#445) * Working VLM Replaced old vqa for describing people with new vlm service * Formatted files with black * Fixed accidental merge errors * WIP - Setting up and Testing HRI task * Fix introduce and remove testing code * WIP - HRI task semi-working SM1-4 works correctly, however during SM4 it doesn't look at the seated guests * Change LLM to Ollama got HRI get anem and drink * Updated Seat Guest (SM3) * WIP - HRI SM4 being refactored * Verbal introduction is correct * Debugging SM4 face person errors * Formatted files with black --------- Co-authored-by: Yara Alkhelaiwi Co-authored-by: Yara Alkhelaiwi Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi Co-authored-by: Aldrich-Fernandes Co-authored-by: Ma'ayan Armony --- FETCH_HEAD | 0 .../lasr_llm/lasr_llm/llm_inference.py | 296 +++++++---------- .../lasr_llm/nodes/hri_task_service.py | 4 +- common/language/lasr_llm/lasr_llm/utils.py | 89 +++-- common/language/lasr_llm/requirements.in | 4 +- common/language/lasr_llm/requirements.txt | 113 ++++++- common/language/lasr_llm/test/test_utils.py | 62 ++++ .../lasr_llm_interfaces/CMakeLists.txt | 2 +- .../lasr_vision_reid/add_face.py | 11 +- .../lasr_vision_reid/service.py | 51 +-- skills/src/lasr_skills/ask_and_listen.py | 2 +- skills/src/lasr_skills/detect_3d.py | 36 +- skills/src/lasr_skills/detect_3d_in_area.py | 7 +- skills/src/lasr_skills/receive_object.py | 2 +- skills/src/lasr_skills/vision/get_image.py | 26 +- .../lasr_skills/wait_for_person_in_area.py | 2 +- tasks/HRI/HRI/state_machine.py | 69 +++- tasks/HRI/HRI/states/__init__.py | 8 + .../HRI/HRI/states/clearSeatingDetections.py | 19 ++ tasks/HRI/HRI/states/getGuestData.py | 64 ++++ tasks/HRI/HRI/states/getIntroductionStr.py | 22 ++ tasks/HRI/HRI/states/get_name_and_drink.py | 7 +- tasks/HRI/HRI/states/greet.py | 45 ++- tasks/HRI/HRI/states/hri_learn_faces.py | 2 +- tasks/HRI/HRI/states/introduce.py | 233 +++++++++++++ tasks/HRI/HRI/states/recognise.py | 150 +++++++++ tasks/HRI/HRI/states/seat_guest.py | 313 ++++-------------- tasks/HRI/HRI/states/start_door_sm.py | 17 +- tasks/HRI/config/debug.rviz | 61 ++-- tasks/HRI/config/lab.yaml | 52 +-- tasks/HRI/setup.py | 1 + tasks/pick_and_place/config/config.yaml | 72 ++++ tasks/pick_and_place/package.xml | 24 ++ .../pick_and_place/pick_and_place/__init__.py | 0 .../pick_and_place/state_machine.py | 220 ++++++++++++ .../pick_and_place/states/__init__.py | 0 .../pick_and_place/states/choose_shelf.py | 136 ++++++++ .../states/classify_category.py | 266 +++++++++++++++ .../pick_and_place/states/compute_approach.py | 184 ++++++++++ .../pick_and_place/states/detect_objects.py | 124 +++++++ .../states/find_and_go_to_table.py | 217 ++++++++++++ .../pick_and_place/states/instruct_pick.py | 35 ++ .../pick_and_place/states/instruct_place.py | 54 +++ .../pick_and_place/states/scan_shelves.py | 217 ++++++++++++ .../states/select_and_visualize_object.py | 121 +++++++ .../pick_and_place/states/start.py | 116 +++++++ .../pick_and_place/test_detect.py | 27 ++ tasks/pick_and_place/resource/pick_and_place | 0 tasks/pick_and_place/setup.cfg | 4 + tasks/pick_and_place/setup.py | 30 ++ tasks/pick_and_place/test/test_copyright.py | 27 ++ tasks/pick_and_place/test/test_flake8.py | 25 ++ tasks/pick_and_place/test/test_pep257.py | 23 ++ 53 files changed, 3094 insertions(+), 598 deletions(-) create mode 100644 FETCH_HEAD create mode 100644 common/language/lasr_llm/test/test_utils.py create mode 100644 tasks/HRI/HRI/states/clearSeatingDetections.py create mode 100644 tasks/HRI/HRI/states/getGuestData.py create mode 100644 tasks/HRI/HRI/states/getIntroductionStr.py create mode 100644 tasks/HRI/HRI/states/introduce.py create mode 100644 tasks/HRI/HRI/states/recognise.py create mode 100644 tasks/pick_and_place/config/config.yaml create mode 100644 tasks/pick_and_place/package.xml create mode 100644 tasks/pick_and_place/pick_and_place/__init__.py create mode 100644 tasks/pick_and_place/pick_and_place/state_machine.py create mode 100644 tasks/pick_and_place/pick_and_place/states/__init__.py create mode 100644 tasks/pick_and_place/pick_and_place/states/choose_shelf.py create mode 100644 tasks/pick_and_place/pick_and_place/states/classify_category.py create mode 100644 tasks/pick_and_place/pick_and_place/states/compute_approach.py create mode 100644 tasks/pick_and_place/pick_and_place/states/detect_objects.py create mode 100644 tasks/pick_and_place/pick_and_place/states/find_and_go_to_table.py create mode 100644 tasks/pick_and_place/pick_and_place/states/instruct_pick.py create mode 100644 tasks/pick_and_place/pick_and_place/states/instruct_place.py create mode 100644 tasks/pick_and_place/pick_and_place/states/scan_shelves.py create mode 100644 tasks/pick_and_place/pick_and_place/states/select_and_visualize_object.py create mode 100644 tasks/pick_and_place/pick_and_place/states/start.py create mode 100644 tasks/pick_and_place/pick_and_place/test_detect.py create mode 100644 tasks/pick_and_place/resource/pick_and_place create mode 100644 tasks/pick_and_place/setup.cfg create mode 100644 tasks/pick_and_place/setup.py create mode 100644 tasks/pick_and_place/test/test_copyright.py create mode 100644 tasks/pick_and_place/test/test_flake8.py create mode 100644 tasks/pick_and_place/test/test_pep257.py diff --git a/FETCH_HEAD b/FETCH_HEAD new file mode 100644 index 000000000..e69de29bb diff --git a/common/language/lasr_llm/lasr_llm/llm_inference.py b/common/language/lasr_llm/lasr_llm/llm_inference.py index e5ddd4a8b..5d1e5ed34 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,74 +39,113 @@ 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}) - if self.config.model_type == "pipeline": - if self.config.task: - self.task = self.config.task + messages.append({"role": "user", "content": query}) + + 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.client.pull(self.model_name) + self.logger.info( + f"[LLMInference] '{self.model_name}' saved locally — offline use enabled." + ) + def infer_task(self) -> str: name = self.model_name.lower() if "ner" in name or "token" in name: @@ -124,83 +157,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. @@ -291,26 +247,26 @@ def classify_category(objects: List[str]) -> str: 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) + 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 @@ -327,11 +283,11 @@ def extract_fields_llm(text: str, fields: List[str]) -> Dict: 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} @@ -354,31 +310,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 87fc8ccf1..af897cdfd 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 @@ -31,7 +31,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/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/vision/lasr_vision_reid/lasr_vision_reid/add_face.py b/common/vision/lasr_vision_reid/lasr_vision_reid/add_face.py index 4a3205acb..9a7437e0e 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,12 @@ 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" 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/service.py b/common/vision/lasr_vision_reid/lasr_vision_reid/service.py index 58ee8ff8c..e24e21df2 100644 --- a/common/vision/lasr_vision_reid/lasr_vision_reid/service.py +++ b/common/vision/lasr_vision_reid/lasr_vision_reid/service.py @@ -75,7 +75,7 @@ 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 @@ -100,7 +100,7 @@ def _recognise_2d( 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, @@ -193,7 +193,7 @@ def _recognise_3d( 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,9 +252,9 @@ 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 @@ -296,7 +296,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, @@ -374,24 +374,25 @@ 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(): diff --git a/skills/src/lasr_skills/ask_and_listen.py b/skills/src/lasr_skills/ask_and_listen.py index 4ab9cb0d7..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( diff --git a/skills/src/lasr_skills/detect_3d.py b/skills/src/lasr_skills/detect_3d.py index cbcce7b5b..58a68e666 100644 --- a/skills/src/lasr_skills/detect_3d.py +++ b/skills/src/lasr_skills/detect_3d.py @@ -53,31 +53,27 @@ 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.data = None self.image_msg = None - self.node.create_subscription( - CameraInfo, - self.depth_camera_info_topic, - self._cache_camera_info, - qos_profile=camera_qos, + 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( @@ -90,28 +86,14 @@ def callback(self, image_msg, depth_msg): if self.data is None: self.data = (image_msg, depth_msg) - def _cache_camera_info(self, msg: CameraInfo) -> None: - if self.cam_info is None: - self.cam_info = msg - def _create_req(self, blackboard): self.data = None self.image_msg = None - if self.cam_info is None: - deadline = time.time() + 5.0 - while self.cam_info is None and time.time() < deadline: - time.sleep(0.25) - 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" - deadline = time.time() + 30.0 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." ) @@ -123,7 +105,7 @@ def _create_req(self, blackboard): req = YoloDetection3D.Request( image_raw=image_msg, depth_image=depth_msg, - depth_camera_info=self.cam_info, + depth_camera_info=self.cache.getLast(), model=self.model, confidence=self.confidence, filter=self.filter, @@ -136,7 +118,7 @@ def _create_req(self, blackboard): 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 6d7ccd540..db873abe0 100644 --- a/skills/src/lasr_skills/detect_3d_in_area.py +++ b/skills/src/lasr_skills/detect_3d_in_area.py @@ -61,6 +61,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 @@ -131,7 +136,7 @@ def __init__( z_max: Optional[float] = None, ): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + 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: diff --git a/skills/src/lasr_skills/receive_object.py b/skills/src/lasr_skills/receive_object.py index c2d656746..280f9d7a9 100755 --- a/skills/src/lasr_skills/receive_object.py +++ b/skills/src/lasr_skills/receive_object.py @@ -29,7 +29,7 @@ def _create_request(self, blackboard): class ReceiveObject(StateMachine): def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) if object_name is None: self.add_input_key("object_name") diff --git a/skills/src/lasr_skills/vision/get_image.py b/skills/src/lasr_skills/vision/get_image.py index b03339570..35e61e982 100755 --- a/skills/src/lasr_skills/vision/get_image.py +++ b/skills/src/lasr_skills/vision/get_image.py @@ -5,7 +5,7 @@ import rclpy from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy - +import message_filters from typing import Optional from sensor_msgs.msg import Image, PointCloud2 @@ -23,33 +23,23 @@ def __init__(self, topic="head_front_camera/rgb/image_raw"): 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, ) - self.node = yasmin_ros.logger_node - - self.msg = None - - self.node.create_subscription( - Image, topic, self.image_cb, qos_profile=self.camera_qos + self.image_sub = message_filters.Subscriber( + self.node, Image, "head_front_camera/rgb/image_raw", camera_qos ) - def image_cb(self, msg): - if self.msg is None: - self.msg = msg + self.cache = message_filters.Cache(self.image_sub) def execute(self, blackboard): - self.msg = None - - while self.msg is None: - yasmin.YASMIN_LOG_INFO("Waiting for rgb frame") - time.sleep(1) - try: - blackboard["img_msg"] = self.msg + blackboard["img_msg"] = self.cache.getLast() return "succeeded" except Exception as e: yasmin.YASMIN_LOG_ERROR(str(e)) 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 bf69620de..47797b28a 100644 --- a/skills/src/lasr_skills/wait_for_person_in_area.py +++ b/skills/src/lasr_skills/wait_for_person_in_area.py @@ -22,7 +22,7 @@ def execute(self, blackboard): class WaitForPersonInArea(StateMachine): def __init__(self): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detections_3d") node = yasmin_ros.logger_node diff --git a/tasks/HRI/HRI/state_machine.py b/tasks/HRI/HRI/state_machine.py index 60bfa7581..287051866 100644 --- a/tasks/HRI/HRI/state_machine.py +++ b/tasks/HRI/HRI/state_machine.py @@ -63,7 +63,17 @@ def wait_cb(blackboard, msg): self.add_state( "GO_TO_DOOR", GoToLocation(location_param="door_pose"), - transitions={"succeeded": "GREET", "failed": "failed"}, + transitions={"succeeded": "POST_NAV", "failed": "failed"}, + ) + + self.add_state( + "POST_NAV", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "GREET", + "aborted": "failed", + "canceled": "failed", + }, ) self.add_state( @@ -95,7 +105,17 @@ def wait_cb(blackboard, msg): self.add_state( "SAY_FOLLOW", - Say(text="Welcome. Follow me to the seating area."), + Say(format_str="Welcome {}. Follow me to the seating area."), + transitions={ + "succeeded": "PRE_NAV_2", + "aborted": "failed", + "canceled": "failed", + }, + ) + + self.add_state( + "PRE_NAV_2", + PlayMotion("pre_navigation"), transitions={ "succeeded": "GUIDE_TO_SEAT", "aborted": "failed", @@ -106,7 +126,17 @@ def wait_cb(blackboard, msg): self.add_state( "GUIDE_TO_SEAT", # GUIDES GUEST TO SEATING AREA GoToLocation(location_param="seat_pose"), - transitions={"succeeded": "SEAT_GUEST", "failed": "failed"}, + transitions={"succeeded": "POST_NAV_2", "failed": "failed"}, + ) + + self.add_state( + "POST_NAV_2", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "SEAT_GUEST", + "aborted": "failed", + "canceled": "failed", + }, ) self.add_state( @@ -118,13 +148,33 @@ def wait_cb(blackboard, msg): self.add_state( "CHECK", yasmin.CbState(outcomes=["succeeded", "GO_TO_DOOR_2"], callback=self.check), - transitions={"succeeded": "succeeded", "GO_TO_DOOR_2": "GO_TO_DOOR_2"}, + transitions={"succeeded": "INTRODUCE", "GO_TO_DOOR_2": "PRE_NAV_3"}, + ) + + self.add_state( + "PRE_NAV_3", + PlayMotion("pre_navigation"), + transitions={ + "succeeded": "GO_TO_DOOR_2", + "aborted": "failed", + "canceled": "failed", + }, ) self.add_state( "GO_TO_DOOR_2", GoToLocation(location_param="door_pose"), - transitions={"succeeded": "GREET_2", "failed": "failed"}, + transitions={"succeeded": "POST_NAV_3", "failed": "failed"}, + ) + + self.add_state( + "POST_NAV_3", + PlayMotion("post_navigation"), + transitions={ + "succeeded": "GREET_2", + "aborted": "failed", + "canceled": "failed", + }, ) self.add_state( @@ -133,6 +183,12 @@ def wait_cb(blackboard, msg): transitions={"succeeded": "STOP_EYE_TRACKER", "failed": "failed"}, ) + self.add_state( + "INTRODUCE", + Introduce(), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) + def check(self, blackboard): guest = blackboard["guest_data"][f"guest{self.guest_id}"] yasmin.YASMIN_LOG_INFO(f"{self.guest_id}") @@ -199,6 +255,7 @@ def main(): "detection": False, "seating_detection": False, "attributes": {}, + "seated_point": None, }, "guest2": { "name": "", @@ -206,6 +263,7 @@ def main(): "detection": False, "seating_detection": False, "attributes": {}, + "seated_point": None, }, } @@ -215,6 +273,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..299ea33eb 100644 --- a/tasks/HRI/HRI/states/__init__.py +++ b/tasks/HRI/HRI/states/__init__.py @@ -1,4 +1,5 @@ from .speech_recovery import SpeechRecovery + from .learn_host_face import LearnHostFace from .hri_learn_faces import HRILearnFaces from .seat_guest import SeatGuest @@ -8,3 +9,10 @@ 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 diff --git a/tasks/HRI/HRI/states/clearSeatingDetections.py b/tasks/HRI/HRI/states/clearSeatingDetections.py new file mode 100644 index 000000000..4261014fb --- /dev/null +++ b/tasks/HRI/HRI/states/clearSeatingDetections.py @@ -0,0 +1,19 @@ +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: + 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_name_and_drink.py b/tasks/HRI/HRI/states/get_name_and_drink.py index 3a81a9683..407f08949 100755 --- a/tasks/HRI/HRI/states/get_name_and_drink.py +++ b/tasks/HRI/HRI/states/get_name_and_drink.py @@ -26,6 +26,7 @@ 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 @@ -43,6 +44,9 @@ def _handle_resp(self, blackboard, result): result.name if self.task == "name" else result.favourite_drink ) + if self.task == "name": + blackboard["placeholders"] = result.name + return "succeeded" class PostRecoveryDecision(yasmin.State): @@ -87,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 0a1fad174..6ae0f686f 100644 --- a/tasks/HRI/HRI/states/greet.py +++ b/tasks/HRI/HRI/states/greet.py @@ -1,6 +1,13 @@ import yasmin -from lasr_skills import Say, StartEyeTracker, WaitForPersonInArea, AskAndListen +from lasr_skills import ( + Say, + StartEyeTracker, + WaitForPersonInArea, + AskAndListen, + ReceiveObject, + StopEyeTracker, +) from HRI.states import ( GetNameAndDrink, GetGuestAttributes, @@ -19,7 +26,7 @@ class LookAndGreetGuest(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") @@ -129,18 +136,48 @@ def __init__(self, last_resort, guest_id): ), transitions={ "succeeded": "GET_NAME_DRINK_FACE", - "failed": "GREET_AND_ASK_GUEST", + "failed": "failed", }, remappings={"transcribed_speech": "guest_transcription"}, ) + + transition = "SAY_BAG" if guest_id == "guest2" else "succeeded" + self.add_state( "GET_NAME_DRINK_FACE", conc_name_drink_face, transitions={ - "succeeded": "succeeded", + "succeeded": transition, "failed": "failed", "failed_vision": "failed", "failed_face": "failed", "failed_attributes": "failed", }, ) + + self.add_state( + "SAY_BAG", + Say(text="I see you have a bag for me."), + transitions={ + "succeeded": "STOP_EYE_TRACKING", + "aborted": "STOP_EYE_TRACKING", + "canceled": "STOP_EYE_TRACKING", + }, + ) + + self.add_state( + "STOP_EYE_TRACKING", + StopEyeTracker(), + transitions={ + "succeeded": "GRAB_BAG", + "aborted": "failed", + "canceled": "failed", + "timeout": "failed", + }, + ) + + self.add_state( + "GRAB_BAG", + ReceiveObject(object_name="bag"), + transitions={"succeeded": "succeeded", "failed": "failed"}, + ) diff --git a/tasks/HRI/HRI/states/hri_learn_faces.py b/tasks/HRI/HRI/states/hri_learn_faces.py index 4104316c3..5183ddfaf 100644 --- a/tasks/HRI/HRI/states/hri_learn_faces.py +++ b/tasks/HRI/HRI/states/hri_learn_faces.py @@ -119,7 +119,7 @@ def execute(self, blackboard): return "failed" 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..75f3580c7 --- /dev/null +++ b/tasks/HRI/HRI/states/introduce.py @@ -0,0 +1,233 @@ +""" +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_detected") + 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") + + self.add_state( + "RESET_SEATING_DETECTIONS", + ClearSeatingDetections(), + transitions={"succeeded": "FIND_PEOPLE", "failed": "failed"}, + ) + + self.add_state( + "FIND_PEOPLE", + DetectAllInPolygon( + polygon=self.seating_area, + object_filter=["person"], + min_coverage=1.0, + 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": "RECOGNISE", + "aborted": "RECOGNISE", + "canceled": "failed", + "timeout": "RECOGNISE", + }, + remappings={"pointstamped": "person_point_stamped"}, + ) + + 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": "succeeded", "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", + }, + ) + + def _loop_person_index(self, blackboard): + guest1point = blackboard["guest_data"]["guest1"]["seated_point"] + guest2point = blackboard["guest_data"]["guest2"]["seated_point"] + people_detected = len(blackboard["people_detected"]) + index = blackboard["person_index"] + yasmin.YASMIN_LOG_INFO(str(index)) + yasmin.YASMIN_LOG_INFO(str(guest1point)) + yasmin.YASMIN_LOG_INFO(str(guest2point)) + yasmin.YASMIN_LOG_INFO(str(people_detected)) + + if guest1point is not None and guest2point is not None: + return "succeeded" + elif index < people_detected: + point = blackboard["people_detected"][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" + + 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/recognise.py b/tasks/HRI/HRI/states/recognise.py new file mode 100644 index 000000000..43c048d5a --- /dev/null +++ b/tasks/HRI/HRI/states/recognise.py @@ -0,0 +1,150 @@ +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 + + +class Recognise(yasmin_ros.ServiceState): + def __init__(self): + super().__init__( + srv_type=Recognise3D, + srv_name="/lasr_vision_reid/recognise/threed", + create_request_handler=self._create_request, + response_handler=self._handle_resp, + outcomes=["no_detections"], + ) + + self.add_output_key("guest_data") + + 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 + + request.image_raw = image + request.depth_image = depth + request.depth_camera_info = self.cache.getLast() + request.threshold = 0.5 + request.target_frame = "map" + + return request + + def _handle_resp(self, blackboard, response): + 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 + return "succeeded" + + return "aborted" + + +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( + "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 286a7542d..98f313954 100644 --- a/tasks/HRI/HRI/states/seat_guest.py +++ b/tasks/HRI/HRI/states/seat_guest.py @@ -66,36 +66,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: @@ -104,129 +74,66 @@ 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 - yasmin.YASMIN_LOG_INFO( - f"Detected {len(seated_guest_locs)} seated guests in the seating area." - ) - 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." - ) - 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}." + left_sofa_occupied = False + right_sofa_occupied = False + unseated_sofa_persons = [] + non_sofa_chairs = {} + + for detection in blackboard["seat_detections"]: + detection_point = ShapelyPoint( + detection.point.x, detection.point.y, detection.point.z ) - 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 + if detection.name == "person": + 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}) + + 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 take 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: @@ -242,100 +149,48 @@ 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, ): - 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) 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_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" + self.add_state( "PROCESS_DETECTIONS", ProcessDetections( @@ -344,40 +199,8 @@ def __init__( left_sofa_area=self.left_sofa_area, right_sofa_area=self.right_sofa_area, ), - transitions={"succeeded": detection_transition, "failed": "failed"}, + transitions={"succeeded": "LOOK_TO_SEAT", "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( - "SAY_AND_LEARN_HOST_FACE", - sm_con, - transitions={"succeeded": "LOOK_TO_SEAT", "failed": "LOOK_TO_SEAT"}, - ) self.add_state( "LOOK_TO_SEAT", @@ -417,22 +240,6 @@ def __init__( ) 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( @@ -462,8 +269,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 diff --git a/tasks/HRI/HRI/states/start_door_sm.py b/tasks/HRI/HRI/states/start_door_sm.py index 7a2e3369c..316705de1 100644 --- a/tasks/HRI/HRI/states/start_door_sm.py +++ b/tasks/HRI/HRI/states/start_door_sm.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, GoToLocation, PlayMotion class StartDoorSM(StateMachine): # TODO: Rename to start_task and move to Skills @@ -20,13 +20,24 @@ def __init__( location: Union[Pose, None] = None, location_param: Union[str, None] = "start_pose", ): - super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + super().__init__(outcomes=["succeeded", "failed"]) self.add_state( "DETECT_DOOR_OPENING", DetectDoorOpening(), - transitions={"door_opened": "GO_TO_START", "failed": "failed"}, + transitions={"door_opened": "PRE_NAV", "failed": "failed"}, ) + + self.add_state( + "PRE_NAV", + PlayMotion("pre_navigation"), + transitions={ + "succeeded": "GO_TO_START", + "aborted": "failed", + "canceled": "failed", + }, + ) + self.add_state( "GO_TO_START", GoToLocation( 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 3aa21f7bb..ba01852cc 100644 --- a/tasks/HRI/config/lab.yaml +++ b/tasks/HRI/config/lab.yaml @@ -3,65 +3,65 @@ hri: # the `hri` Node's Parameters # Start location after door start_pose: position: - x: 3.045619723531395 - y: 0.4649833631759533 + x: 2.620011965794007 + y: 0.4284228083916832 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.9927799557187154 - w: 0.11994982085499546 + z: -0.9676446468413096 + w: 0.25231693847095826 # Where to wait for guests door_pose: position: - x: 2.133728259050112 - y: 0.9460632470337332 + x: 0.9737232865224763 + y: 0.6644210227864706 z: 0.0 orientation: x: 0.0 y: 0.0 - z: 0.9378581866285763 - w: 0.34701876285549516 + z: 0.9883189417995438 + w: 0.15239970236266812 door_polygon: - top_left: [1.4293599128723145, 1.1347585916519165] - top_right: [1.8499776124954224, 1.6542631387710571] - bottom_right: [1.4631035327911377, 2.074192762374878] - bottom_left: [0.940778374671936, 1.5865740776062012] + 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: 1.6410613033142212 - y: 0.029216336819556613 + x: 0.8439305474571136 + y: -0.7982808179074764 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.9565993334920534 - w: 0.29140644324132453 + z: -0.7507265515887908 + w: 0.6606130824768782 # Where the robot looks at the general sofa sofa_point: - x: 0.4285953640937805 - y: -1.1452689170837402 + x: 0.40349432826042175 + y: -2.7426514625549316 z: 0.5 # From robot POV: [top left, top right,bottom right, bottom left ] # General area to perform detections in seat_area: - top_left: [0.7586971521377563, -1.8606892824172974] - top_right: [-0.9268304109573364, -0.5480098724365234] - bottom_right: [0.03138554096221924, 0.5941693782806396] - bottom_left: [1.653577566146850, -0.5811513662338257] + top_left: [0.941597044467926, -3.303898811340332] + top_right: [-0.664279580116272, -2.5707762241363525] + bottom_right: [-0.34006959199905396, -1.5264309644699097] + bottom_left: [1.3503732681274414, -1.9663374423980713] # Max number of seats max_people_on_sofa: 2 # Seatable area sofa_area: - top_left: [0.728126049041748, -1.872155785560608] - top_right: [-0.356697678565979, -1.0435458421707153] - bottom_right: [0.16386330127716064, -0.47996777296066284] - bottom_left: [1.1935354471206665, -1.1827179193496704] + top_left: [0.8116632699966431, -3.0669937133789062] + top_right: [-0.058406829833984375, -2.6968655586242676] + bottom_right: [0.03105384111404419, -2.327255964279175] + bottom_left: [0.9847490191459656, -2.6538729667663574] diff --git a/tasks/HRI/setup.py b/tasks/HRI/setup.py index cc12e953e..449a54e32 100644 --- a/tasks/HRI/setup.py +++ b/tasks/HRI/setup.py @@ -51,6 +51,7 @@ 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", ], }, ) diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml new file mode 100644 index 000000000..14702e0a1 --- /dev/null +++ b/tasks/pick_and_place/config/config.yaml @@ -0,0 +1,72 @@ +pick_and_place: + ros__parameters: + table: + pose: + position: + x: 6.33 + y: 5.9 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.707 + w: 0.707 + look_point: + x: 6.55 + y: 4.82 + z: 0.75 + polygon: + top_left: [5.80, 5.27] + top_right: [7.30, 5.27] + bottom_right: [7.30, 4.37] + bottom_left: [5.80, 4.37] + search_polygon: + top_left: [5.3, 5.9] + top_right: [7.8, 5.9] + bottom_right: [7.8, 3.8] + bottom_left: [5.3, 3.8] + z_min: 0.7 + z_max: 1.5 + 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 + shelves: + top: + torso_lift_joint: 0.35 + look_point: + x: 0.0 + y: 0.0 + z: 1.2 + polygon: + top_left: [0.0, 1.0] + top_right: [1.0, 1.0] + bottom_right: [1.0, 0.0] + bottom_left: [0.0, 0.0] + z_min: 1.0 + z_max: 1.4 + bottom: + torso_lift_joint: 0.1 + look_point: + x: 0.0 + y: 0.0 + z: 0.6 + polygon: + top_left: [0.0, 1.0] + top_right: [1.0, 1.0] + bottom_right: [1.0, 0.0] + bottom_left: [0.0, 0.0] + z_min: 0.4 + z_max: 0.8 + objects: + cocacola: + category: "drinks" + sprite: + category: "drinks" \ No newline at end of file diff --git a/tasks/pick_and_place/package.xml b/tasks/pick_and_place/package.xml new file mode 100644 index 000000000..04af79a7d --- /dev/null +++ b/tasks/pick_and_place/package.xml @@ -0,0 +1,24 @@ + + + + pick_and_place + 0.0.0 + TODO: Package description + yara + TODO: License declaration + + rclpy + yasmin + yasmin_ros + lasr_vision_yolo + lasr_speech + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/tasks/pick_and_place/pick_and_place/__init__.py b/tasks/pick_and_place/pick_and_place/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/tasks/pick_and_place/pick_and_place/state_machine.py b/tasks/pick_and_place/pick_and_place/state_machine.py new file mode 100644 index 000000000..1e2540403 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -0,0 +1,220 @@ +from threading import Thread + +import rclpy +from rclpy.node import Node + +import yasmin +import yasmin_ros +from yasmin_viewer import YasminViewerPub + +from lasr_skills import Say, GoToLocation + +from pick_and_place.states import ( + Start, + ScanShelves, + FindAndGoToTable, + DetectObjects, + SelectAndVisualiseObject, + ClassifyCategory, + ChooseShelf, + InstructPick, + InstructPlace, +) + +try: + from rclpy.executors import EventsExecutor as Executor +except ImportError: + from rclpy.executors import MultiThreadedExecutor as Executor + + +class PickAndPlace(yasmin.StateMachine): + """ + Main 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. + + 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 + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + + # ── Entry ───────────────────────────────────────────────────────────── + self.add_state( + "START", + Start(), + transitions={ + "succeeded": "SCAN_SHELVES", + "failed": "failed", + }, + ) + + # ── Scan cabinet shelves (done once) ────────────────────────────────── + self.add_state( + "SCAN_SHELVES", + ScanShelves(), + transitions={ + "succeeded": "FIND_AND_GO_TO_TABLE", + "failed": "failed", + }, + ) + + # ── Find and navigate to table ──────────────────────────────────────── + self.add_state( + "FIND_AND_GO_TO_TABLE", + FindAndGoToTable(), + transitions={ + "succeeded": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", # proceed even if table not found + }, + ) + + # ── Detect all objects on table ─────────────────────────────────────── + # Re-entered at the top of every loop iteration + self.add_state( + "DETECT_OBJECTS", + DetectObjects(), + transitions={ + "succeeded": "SELECT_OBJECT", + "failed": "DETECT_OBJECTS", # retry until objects found + }, + ) + + # ── Select object and visualise for referee ─────────────────────────── + self.add_state( + "SELECT_OBJECT", + SelectAndVisualiseObject(), + transitions={ + "succeeded": "CLASSIFY_CATEGORY", + "failed": "DETECT_OBJECTS", # re-scan if nothing to select + }, + ) + + # ── Classify selected object into a category ────────────────────────── + self.add_state( + "CLASSIFY_CATEGORY", + ClassifyCategory(task="object"), + transitions={ + "succeeded": "CHOOSE_SHELF", + "failed": "CHOOSE_SHELF", # proceed with unknown category + "empty": "DETECT_OBJECTS", # nothing to classify, re-scan + }, + ) + + # ── Choose which shelf to place object on ───────────────────────────── + self.add_state( + "CHOOSE_SHELF", + ChooseShelf(), + transitions={ + "succeeded": "INSTRUCT_PICK", + "failed": "DETECT_OBJECTS", + }, + ) + + # ── Instruct operator to pick up object ─────────────────────────────── + self.add_state( + "INSTRUCT_PICK", + InstructPick(), + transitions={ + "succeeded": "GO_TO_CABINET", + "failed": "INSTRUCT_PICK", # retry instruction + }, + ) + + # ── 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, + ) + + self._executor = Executor() + self._executor.add_node(self) + self._spin_thread = Thread(target=self._executor.spin) + self._spin_thread.start() + + +def main(): + rclpy.init() + + node = PickAndPlaceNode() + yasmin_ros.set_ros_loggers(node) + + sm = PickAndPlace() + + # Uncomment to visualise the state machine in RViz/browser + # YasminViewerPub(sm) + + bb = yasmin.Blackboard() + + # Initialise all blackboard keys used across the machine + 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"] = [] + + 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() + + +if __name__ == "__main__": + 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 new file mode 100644 index 000000000..e69de29bb 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 new file mode 100644 index 000000000..986d0c9b3 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/choose_shelf.py @@ -0,0 +1,136 @@ +import yasmin +import yasmin_ros + + +class ChooseShelf(yasmin.State): + """ + Chooses the most appropriate shelf for the selected object based on + the shelf_data built by ScanShelves. + + Expects object_category to already be in the blackboard — set by + ClassifyCategory(task="object") which runs before this state. + + Selection priority: + 1. Shelf whose dominant category matches the object's category + 2. Shelf with the most items of the same category + 3. An empty shelf (assigns the object's category to it) + 4. Final fallback: the least full shelf + + Updates shelf_data in the blackboard after each placement so subsequent + objects are placed correctly relative to what is already there. + + Blackboard inputs: + object_category : str — set by ClassifyCategory + selected_object_name : str + shelf_data : dict — built by ScanShelves + + Blackboard outputs: + chosen_shelf : str — shelf ID e.g. "shelf_1" + chosen_shelf_str : str — placement hint e.g. "near the cereal" + shelf_data : dict — updated with new placement + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("object_category") + self.add_input_key("selected_object_name") + self.add_input_key("shelf_data") + self.add_output_key("chosen_shelf") + self.add_output_key("chosen_shelf_str") + self.add_output_key("shelf_data") + + def execute(self, blackboard) -> str: + object_name = blackboard["selected_object_name"] + object_category = blackboard["object_category"] + shelf_data = blackboard["shelf_data"] + + yasmin.YASMIN_LOG_INFO( + f"Choosing shelf for '{object_name}' (category: '{object_category}')." + ) + yasmin.YASMIN_LOG_INFO(f"Current shelf data: {shelf_data}") + + chosen_shelf = None + chosen_shelf_str = "" + max_count = -1 + fallback_shelf = None + min_total_objects = float("inf") + + # ── Pass 1: find best matching shelf ───────────────────────────────── + for shelf_name, shelf_info in shelf_data.items(): + + # Priority 1: dominant category is an exact match + if shelf_info["category"] == object_category: + chosen_shelf = shelf_name + yasmin.YASMIN_LOG_INFO( + f"Exact dominant category match on '{shelf_name}'." + ) + break + + # 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 + chosen_shelf = shelf_name + yasmin.YASMIN_LOG_INFO( + f"Best category count so far ({count}) on '{shelf_name}'." + ) + + # Track fallback: least full shelf + total_objects = len(shelf_info.get("objects", [])) + if total_objects < min_total_objects: + min_total_objects = total_objects + fallback_shelf = shelf_name + + # ── Pass 2: try an empty shelf ──────────────────────────────────────── + if chosen_shelf is None or max_count == 0: + for shelf_name, shelf_info in shelf_data.items(): + if shelf_info["category"] == "empty": + chosen_shelf = shelf_name + shelf_data[shelf_name]["category"] = object_category + yasmin.YASMIN_LOG_INFO(f"Using empty shelf: '{shelf_name}'.") + break + + # ── Pass 3: final fallback ──────────────────────────────────────────── + if not chosen_shelf and fallback_shelf: + chosen_shelf = fallback_shelf + yasmin.YASMIN_LOG_WARN( + f"No category match or empty shelf. " + f"Falling back to least full shelf: '{chosen_shelf}'." + ) + + # ── Update shelf_data and set outputs ───────────────────────────────── + if chosen_shelf: + shelf_info = shelf_data[chosen_shelf] + + 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 + ) + + new_dominant = max( + shelf_info["category_counts"].items(), key=lambda x: x[1] + )[0] + shelf_info["category"] = new_dominant + + if not was_empty and category_previously_present: + chosen_shelf_str = f"near the {object_category}" + else: + chosen_shelf_str = "" + + blackboard["chosen_shelf"] = chosen_shelf + blackboard["chosen_shelf_str"] = chosen_shelf_str + blackboard["shelf_data"] = shelf_data + + yasmin.YASMIN_LOG_INFO( + f"Chose shelf '{chosen_shelf}'. " + f"Placement hint: '{chosen_shelf_str}'." + ) + return "succeeded" + + yasmin.YASMIN_LOG_ERROR("No suitable shelf found.") + 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 new file mode 100644 index 000000000..53fb8a5f6 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/classify_category.py @@ -0,0 +1,266 @@ +import yasmin +import yasmin_ros +import rclpy + +from lasr_llm_msgs.srv import Llm + +# Hardcoded category map as fallback when params are unavailable. +# Mirrors the category_map from the ROS 1 ClassifyCategory. +# Ideally these live in your config yaml under pick_and_place.objects..category +CATEGORY_MAP = { + "fruit": { + "apple", + "banana", + "orange", + "grape", + "pineapple", + "lemon", + "lime", + "peach", + "plum", + "pear", + "mango", + "watermelon", + "strawberry", + "blueberry", + }, + "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", + }, + "snack": { + "chips", + "crackers", + "candy", + "chocolate bar", + "cookie", + "snack bag", + "biscuit", + "granola bar", + "popcorn", + }, + "cleaning": { + "soap", + "sponge", + "brush", + "cleaner", + "detergent", + "tissue box", + "toilet paper", + "broom", + "mop", + "spray bottle", + "bucket", + }, + "cereal": { + "cereal", + "cereal box", + "oats", + "muesli", + }, + "dish": { + "fork", + "knife", + "spoon", + "plate", + "bowl", + "cup", + "wine glass", + "mug", + "chopsticks", + }, +} + + +class ClassifyCategory(yasmin.State): + """ + Classifies an object or a list of objects into a category. + + Used in two places in the pipeline: + 1. Inside ScanShelves — to determine the dominant category of each shelf + from the list of objects detected on it. + 2. After SelectAndVisualiseObject — to classify the selected table object + before ChooseShelf runs. + + Classification priority: + 1. ROS 2 param lookup (pick_and_place.objects..category) + 2. Hardcoded CATEGORY_MAP + 3. LLM fallback via /lasr_llm/llm + + Blackboard inputs: + object_name : str — single object name (used when task="object") + object_names : List[str] — list of names (used when task="shelf") + + Blackboard outputs: + object_category : str — category for a single object + shelf_category : str — dominant category for a shelf + """ + + def __init__(self, task: str = "object"): + """ + Args: + task: "object" — classify a single object name from blackboard["object_name"] + "shelf" — classify a shelf from blackboard["object_names"] (list) + """ + super().__init__(outcomes=["succeeded", "failed", "empty"]) + + assert task in ( + "object", + "shelf", + ), f"ClassifyCategory task must be 'object' or 'shelf', got '{task}'" + + self._task = task + + if task == "object": + self.add_input_key("object_name") + self.add_output_key("object_category") + else: + self.add_input_key("object_names") + self.add_output_key("shelf_category") + + self.node = yasmin_ros.logger_node + self._llm_client = self.node.create_client(Llm, "/lasr_llm/llm") + + def execute(self, blackboard) -> str: + if self._task == "object": + return self._classify_object(blackboard) + else: + return self._classify_shelf(blackboard) + + # ── Task handlers ───────────────────────────────────────────────────────── + + def _classify_object(self, blackboard) -> str: + """Classifies a single object name into a category.""" + name = blackboard["object_name"] + if not name: + yasmin.YASMIN_LOG_WARN("object_name is empty.") + return "empty" + + category = self._get_category(name.lower()) + if category: + blackboard["object_category"] = category + yasmin.YASMIN_LOG_INFO(f"Classified '{name}' as '{category}'.") + return "succeeded" + + yasmin.YASMIN_LOG_WARN(f"Could not classify '{name}'.") + return "failed" + + def _classify_shelf(self, blackboard) -> str: + """ + Classifies a shelf by finding the dominant category across all + object names detected on it. + """ + names = blackboard["object_names"] + if not names: + blackboard["shelf_category"] = "empty" + return "succeeded" + + from collections import Counter + + category_counts = Counter() + + for name in names: + category = self._get_category(name.lower()) + if category: + category_counts[category] += 1 + + if category_counts: + dominant = category_counts.most_common(1)[0][0] + blackboard["shelf_category"] = dominant + yasmin.YASMIN_LOG_INFO( + f"Shelf dominant category: '{dominant}' " + f"from counts {dict(category_counts)}." + ) + else: + blackboard["shelf_category"] = "unknown" + yasmin.YASMIN_LOG_WARN("Could not classify any objects on shelf.") + + return "succeeded" + + # ── Classification helpers ──────────────────────────────────────────────── + + def _get_category(self, name: str) -> str | None: + """ + Returns the category for an object name using three fallback levels: + 1. ROS 2 param + 2. Hardcoded CATEGORY_MAP + 3. LLM + """ + # 1. Param lookup + try: + category = ( + self.node.get_parameter(f"pick_and_place.objects.{name}.category") + .get_parameter_value() + .string_value + ) + if category: + return category + except Exception: + pass + + # 2. Hardcoded map + for category, items in CATEGORY_MAP.items(): + if name in items: + return category + + # 3. LLM fallback + return self._classify_with_llm(name) + + def _classify_with_llm(self, name: str) -> str | None: + """Calls the LLM service to determine the category of an unknown object.""" + if not self._llm_client.wait_for_service(timeout_sec=5.0): + yasmin.YASMIN_LOG_WARN("LLM service not available.") + return None + + category_list = ", ".join(sorted(CATEGORY_MAP.keys())) + + req = Llm.Request() + req.system_prompt = ( + "You are a robot classifying household objects into categories. " + "Respond with only one word from the list provided." + ) + req.prompt = ( + f"Which category does '{name}' belong to most? " + f"Choose from: {category_list}." + ) + req.max_tokens = 10 + + future = self._llm_client.call_async(req) + rclpy.spin_until_future_complete(self.node, future) + response = future.result() + + if response is None: + yasmin.YASMIN_LOG_WARN("LLM call failed.") + return None + + words = response.output.strip().lower().replace(",", "").split() + for word in words: + if word in CATEGORY_MAP: + return word + + yasmin.YASMIN_LOG_WARN( + f"LLM response '{response.output}' didn't match any known category." + ) + return None diff --git a/tasks/pick_and_place/pick_and_place/states/compute_approach.py b/tasks/pick_and_place/pick_and_place/states/compute_approach.py new file mode 100644 index 000000000..1f293064e --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/compute_approach.py @@ -0,0 +1,184 @@ +import numpy as np +import yasmin +import yasmin_ros +import rclpy + +from typing import List +from geometry_msgs.msg import Point, Pose, PoseStamped, PoseWithCovarianceStamped +from std_msgs.msg import Header + +from nav2_msgs.action import ComputePathToPose +from rclpy.action import ActionClient + + +class ComputeApproach(yasmin.State): + """ + Computes reachable approach poses around each detected table candidate. + + Ported from ROS 1 SMACH ComputeApproach. The move_base make_plan service + is replaced with the Nav2 ComputePathToPose action to check reachability. + + Blackboard inputs: + table_candidate_poses : List[Point] + 3D positions of detected table candidates. + + Blackboard outputs: + table_approach_poses : List[Pose] + Reachable approach poses sorted closest-first, one per table candidate. + """ + + def __init__(self, map_frame_min_distance: float = 1.0, n_samples: int = 25): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("table_candidate_poses") + self.add_output_key("table_approach_poses") + + self.node = yasmin_ros.logger_node + self._map_frame_min_distance = map_frame_min_distance + self.n_samples = n_samples + + # Nav2 action client for checking path reachability + self._path_client = ActionClient( + self.node, ComputePathToPose, "/compute_path_to_pose" + ) + + def execute(self, blackboard) -> str: + table_candidates: List[Point] = blackboard["table_candidate_poses"] + + if not table_candidates: + yasmin.YASMIN_LOG_WARN("No table candidate poses in blackboard.") + return "failed" + + # Get current robot pose + current_pose = self._get_current_pose() + if current_pose is None: + yasmin.YASMIN_LOG_ERROR("Could not get current robot pose.") + return "failed" + + approach_poses: List[Pose] = [] + + for table_point in table_candidates: + point_samples = self._sample_points(table_point) + candidate_poses = self._calculate_poses(table_point, point_samples) + + closest_distance = float("inf") + closest_pose = None + + for pose in candidate_poses: + if self._can_reach_pose(pose): + distance = np.linalg.norm( + np.array([pose.position.x, pose.position.y]) + - np.array([current_pose.position.x, current_pose.position.y]) + ) + if distance < closest_distance: + closest_distance = distance + closest_pose = pose + + if closest_pose is not None: + approach_poses.append(closest_pose) + else: + yasmin.YASMIN_LOG_WARN( + f"No reachable approach pose found for table candidate " + f"at ({table_point.x:.2f}, {table_point.y:.2f})." + ) + + if approach_poses: + blackboard["table_approach_poses"] = approach_poses + yasmin.YASMIN_LOG_INFO(f"Computed {len(approach_poses)} approach poses.") + return "succeeded" + + yasmin.YASMIN_LOG_WARN( + "No reachable approach poses found for any table candidate." + ) + return "failed" + + # ── Private helpers ─────────────────────────────────────────────────────── + + def _get_current_pose(self) -> Pose | None: + """ + Gets the current robot pose from /amcl_pose. + Falls back to origin if the topic is unavailable. + """ + try: + success, msg = rclpy.wait_for_message.wait_for_message( + msg_type=PoseWithCovarianceStamped, + node=self.node, + topic="/amcl_pose", + time_to_wait=5.0, + ) + if success: + return msg.pose.pose + yasmin.YASMIN_LOG_WARN("No pose received from /amcl_pose, using origin.") + return Pose() + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Failed to get current pose: {e}") + return None + + def _sample_points(self, point: Point) -> List[Point]: + """ + Samples points uniformly around a circle of radius + map_frame_min_distance centred at the given table point. + """ + angles = np.linspace(0, 2 * np.pi, self.n_samples, endpoint=False) + return [ + Point( + x=point.x + self._map_frame_min_distance * np.cos(angle), + y=point.y + self._map_frame_min_distance * np.sin(angle), + z=0.0, + ) + for angle in angles + ] + + def _calculate_poses( + self, target_point: Point, point_samples: List[Point] + ) -> List[Pose]: + """ + Calculates poses facing the target point from each sampled position. + Orientation is a yaw-only quaternion pointing toward target_point. + """ + poses = [] + for point in point_samples: + dx = target_point.x - point.x + dy = target_point.y - point.y + angle = np.arctan2(dy, dx) + + pose = Pose() + pose.position = point + pose.orientation.z = np.sin(angle / 2) + pose.orientation.w = np.cos(angle / 2) + poses.append(pose) + return poses + + def _can_reach_pose(self, target_pose: Pose) -> bool: + """ + Checks reachability by sending a ComputePathToPose goal to Nav2. + Returns True if Nav2 returns a non-empty path. + + This replaces the ROS 1 move_base/make_plan service call. + """ + if not self._path_client.wait_for_server(timeout_sec=3.0): + yasmin.YASMIN_LOG_WARN("ComputePathToPose action server not available.") + return False + + goal = ComputePathToPose.Goal() + goal.goal = PoseStamped( + header=Header(frame_id="map"), + pose=target_pose, + ) + goal.planner_id = "" + + try: + future = self._path_client.send_goal_async(goal) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=3.0) + goal_handle = future.result() + + if not goal_handle or not goal_handle.accepted: + return False + + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(self.node, result_future, timeout_sec=5.0) + result = result_future.result() + + return result is not None and len(result.result.path.poses) > 0 + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"Path planning check failed: {e}") + return False 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 new file mode 100644 index 000000000..3947a450e --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/detect_objects.py @@ -0,0 +1,124 @@ +import yasmin +import yasmin_ros + +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon + +from lasr_skills.detect_all_in_polygon import DetectAllInPolygon + + +class DetectObjects(yasmin.State): + """ + Looks at the dining table and detects all objects on it within + a defined polygon. + + Ported from ROS 1 SMACH DetectObjects. The two-state machine + (LOOK_AT_TABLE → DETECT_OBJECTS) collapses into a single yasmin.State + since there is no branching between them. + + Uses DetectAllInPolygon (ROS 2 YASMIN version) instead of + DetectAllInPolygonSensorData — no image is attached to detections. + + Reads from ROS 2 params: + pick_and_place.table.look_point — [x, y, z] + pick_and_place.table.polygon — flat [x0,y0, x1,y1, ...] + pick_and_place.objects — list of object names to filter for + + Blackboard outputs: + detected_objects : List[Detection3D] + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("detected_objects") + + self.node = yasmin_ros.logger_node + + # Load params + try: + self._look_point = PointStamped( + point=Point( + x=self.node.get_parameter("table.look_point.x").value, + y=self.node.get_parameter("table.look_point.y").value, + z=self.node.get_parameter("table.look_point.z").value, + ), + header=Header(frame_id="map"), + ) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load table look_point from params: {e}. " + "Using default (0, 0, 0.8)." + ) + self._look_point = PointStamped( + point=Point(x=0.0, y=0.0, z=0.8), + header=Header(frame_id="map"), + ) + + try: + self._polygon = ShapelyPolygon( + [ + self.node.get_parameter("table.polygon.top_left").value, + self.node.get_parameter("table.polygon.top_right").value, + self.node.get_parameter("table.polygon.bottom_right").value, + self.node.get_parameter("table.polygon.bottom_left").value, + ] + ) + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load table polygon from params: {e}. " + "Using empty polygon." + ) + self._polygon = ShapelyPolygon() + + try: + objects_param = self.node.get_parameter("objects").value + self._object_filter = list(objects_param) if objects_param else None + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load object filter from params: {e}. " + "Detecting all objects." + ) + self._object_filter = None + + def execute(self, blackboard) -> str: + # ── 1. Look at table ───────────────────────────────────────────────── + # TODO: call LookToPoint with self._look_point once ported to YASMIN + yasmin.YASMIN_LOG_INFO( + f"[TODO] Looking at table at point " + f"({self._look_point.point.x:.2f}, " + f"{self._look_point.point.y:.2f}, " + f"{self._look_point.point.z:.2f})." + ) + + # ── 2. Detect objects within table polygon ─────────────────────────── + try: + detector = DetectAllInPolygon( + polygon=self._polygon, + object_filter=self._object_filter, + min_confidence=0.1, + # TODO: switch to robocup.pt or your competition model + model="yolo11n-seg.pt", + ) + + # DetectAllInPolygon needs these keys initialised + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] + + outcome = detector.execute(blackboard) + + if outcome == "failed": + yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed.") + return "failed" + + detected = blackboard["detected_objects"] + labels = [obj.name for obj in detected] + yasmin.YASMIN_LOG_INFO( + f"Detected {len(detected)} object(s) on table: {labels}." + ) + + return "succeeded" + + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Object detection failed: {e}") + return "failed" 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 new file mode 100644 index 000000000..75b3607da --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/find_and_go_to_table.py @@ -0,0 +1,217 @@ +import yasmin +import yasmin_ros +import rclpy + +from typing import List +from geometry_msgs.msg import Point, Pose, PoseStamped, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon + +from lasr_skills import Say, GoToLocation +from pick_and_place.states.compute_approach import ComputeApproach + +from lasr_skills import DetectAllInPolygon + + +class FindAndGoToTable(yasmin.StateMachine): + """ + Finds the dining table by detecting it within a search polygon, + computes reachable approach poses, and navigates to the closest one. + + Ported from ROS 1 SMACH FindAndGoToTable + GoToTable. + The two nested state machines are kept as one YASMIN StateMachine — + GoToTable's pose-popping loop is handled inside the GO_TO_TABLE state + via a CbState callback. + + Reads from ROS 2 params: + pick_and_place.table.search_polygon — list of [x, y] pairs + + Blackboard outputs: + table_pose : Pose — the approach pose the robot reached + table_approach_poses : List[Pose] + table_candidate_poses : List[Point] + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + self.add_output_key("table_pose") + + node = yasmin_ros.get_node() + + # Load search polygon from ROS 2 params + # TODO: confirm param name matches your yaml + try: + raw = ( + node.get_parameter("pick_and_place.table.search_polygon") + .get_parameter_value() + .double_array_value + ) + coords = list(zip(raw[::2], raw[1::2])) + search_polygon = ShapelyPolygon(coords) + except Exception: + yasmin.YASMIN_LOG_WARN( + "Could not load table search polygon from params. " + "Using empty polygon — detection will find nothing." + ) + search_polygon = ShapelyPolygon() + + # ── States ──────────────────────────────────────────────────────────── + + self.add_state( + "SAY_LOOKING", + Say(text="I am looking for the table."), + transitions={ + "succeeded": "DETECT_TABLE", + "failed": "DETECT_TABLE", + "aborted": "DETECT_TABLE", + }, + ) + + self.add_state( + "DETECT_TABLE", + DetectAllInPolygon( + polygon=search_polygon, + object_filter=["dining table", "tv"], + min_confidence=0.05, + ), + transitions={ + "succeeded": "GET_TABLE_POSE", + "failed": "DETECT_TABLE", # retry on failure + }, + ) + + # Extract Point positions from detected objects + get_table_pose_cb = yasmin.CbState( + outcomes=["succeeded", "failed"], + callback=self._get_table_pose, + ) + get_table_pose_cb.add_input_key("detected_objects") + get_table_pose_cb.add_output_key("table_candidate_poses") + + self.add_state( + "GET_TABLE_POSE", + get_table_pose_cb, + transitions={ + "succeeded": "COMPUTE_APPROACH", + "failed": "DETECT_TABLE", + }, + ) + + self.add_state( + "COMPUTE_APPROACH", + ComputeApproach(), + transitions={ + "succeeded": "GO_TO_TABLE", + "failed": "DETECT_TABLE", + }, + ) + + # Navigate to each approach pose, popping from the list until one succeeds + go_to_table_cb = yasmin.CbState( + outcomes=["succeeded", "failed"], + callback=self._go_to_table, + ) + go_to_table_cb.add_input_key("table_approach_poses") + go_to_table_cb.add_output_key("table_approach_poses") + go_to_table_cb.add_output_key("table_pose") + + self.add_state( + "GO_TO_TABLE", + go_to_table_cb, + transitions={ + "succeeded": "succeeded", + "failed": "DETECT_TABLE", + }, + ) + + # ── Callbacks ───────────────────────────────────────────────────────────── + + def _get_table_pose(self, blackboard) -> str: + """ + Extracts Point positions from detected objects, filtering for + dining table or tv detections. Mirrors the ROS 1 _get_table_pose CBState. + """ + table_points = [ + obj.point + for obj in blackboard["detected_objects"] + if obj.name in ["dining table", "tv"] + ] + + if table_points: + blackboard["table_candidate_poses"] = table_points + yasmin.YASMIN_LOG_INFO(f"Found {len(table_points)} table candidate(s).") + return "succeeded" + + yasmin.YASMIN_LOG_WARN("No dining table or tv detected in polygon.") + return "failed" + + def _go_to_table(self, blackboard) -> str: + """ + Pops approach poses one at a time and attempts navigation to each. + Stores the successful pose as table_pose. + + Mirrors the GoToTable nested state machine from the ROS 1 version — + the pose-popping loop is collapsed into a single callback here since + YASMIN CbState + GoToLocation handle this more cleanly than a + nested StateMachine. + """ + approach_poses: List[Pose] = blackboard["table_approach_poses"] + + if not approach_poses: + yasmin.YASMIN_LOG_WARN("No approach poses left to try.") + return "failed" + + node = yasmin_ros.get_node() + + while approach_poses: + pose = approach_poses.pop(0) + blackboard["table_approach_poses"] = approach_poses + + yasmin.YASMIN_LOG_INFO( + f"Trying approach pose at " + f"({pose.position.x:.2f}, {pose.position.y:.2f})." + ) + + # Use GoToLocation skill — expects blackboard["location"] as PoseStamped + # We set it temporarily and call navigate directly via Nav2 + # TODO: confirm GoToLocation's blackboard key in your ROS 2 port + try: + from nav2_msgs.action import NavigateToPose + from rclpy.action import ActionClient + + client = ActionClient(node, NavigateToPose, "navigate_to_pose") + if not client.wait_for_server(timeout_sec=5.0): + yasmin.YASMIN_LOG_ERROR("Nav2 action server not available.") + continue + + goal = NavigateToPose.Goal() + goal.pose = PoseStamped( + header=Header(frame_id="map"), + pose=pose, + ) + + future = client.send_goal_async(goal) + rclpy.spin_until_future_complete(node, future) + goal_handle = future.result() + + if not goal_handle or not goal_handle.accepted: + yasmin.YASMIN_LOG_WARN( + "Navigation goal rejected, trying next pose." + ) + continue + + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(node, result_future) + + blackboard["table_pose"] = pose + yasmin.YASMIN_LOG_INFO("Successfully navigated to table.") + return "succeeded" + + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"Navigation attempt failed: {e}. Trying next pose." + ) + continue + + yasmin.YASMIN_LOG_WARN("All approach poses exhausted.") + return "failed" 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 new file mode 100644 index 000000000..85a4ee3c6 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/instruct_pick.py @@ -0,0 +1,35 @@ +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. + + Blackboard inputs: + selected_object_name : str + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("selected_object_name") + + def execute(self, blackboard) -> str: + name = blackboard["selected_object_name"] + + yasmin.YASMIN_LOG_INFO(f"Instructing pick: {name}") + + text = f"I have selected the {name}. " f"Please pick it up and hold it ready." + + say = Say(text=text) + outcome = say.execute(blackboard) + + if outcome in ("succeeded", "aborted"): + return "succeeded" + + return "failed" 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 new file mode 100644 index 000000000..9a38e10db --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/instruct_place.py @@ -0,0 +1,54 @@ +import yasmin +import yasmin_ros + +from lasr_skills import Say + + +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. + + Blackboard inputs: + selected_object_name : str + chosen_shelf : str + chosen_shelf_str : str + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("selected_object_name") + self.add_input_key("chosen_shelf") + self.add_input_key("chosen_shelf_str") + + def execute(self, blackboard) -> str: + name = blackboard["selected_object_name"] + chosen_shelf = blackboard["chosen_shelf"] + chosen_shelf_str = blackboard["chosen_shelf_str"] + + yasmin.YASMIN_LOG_INFO( + f"Instructing place: {name} on {chosen_shelf} {chosen_shelf_str}" + ) + + 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.." + ) + + say = Say(text=text) + outcome = say.execute(blackboard) + + if outcome in ("succeeded", "aborted"): + return "succeeded" + + return "failed" 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 new file mode 100644 index 000000000..f9ac2ace0 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/scan_shelves.py @@ -0,0 +1,217 @@ +import rclpy +import yasmin +import yasmin_ros + +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon + +from lasr_skills import DetectAllInPolygon +from pick_and_place.states.classify_category import ClassifyCategory + + +class ScanShelves(yasmin.State): + """ + Iterates over each shelf in the cabinet, detects objects on each shelf, + and delegates category classification to ClassifyCategory. + + Responsibility of this state: perception only. + - Read shelf config from params + - Adjust torso height + - Look at shelf + - Detect objects within shelf polygon + - Store raw object names per shelf + + Category classification is handled by ClassifyCategory (task="shelf"). + + Reads from ROS 2 params: + pick_and_place.cabinet.shelves — list of shelf IDs + pick_and_place.cabinet.shelves..torso_lift_joint + pick_and_place.cabinet.shelves..look_point + pick_and_place.cabinet.shelves..polygon + pick_and_place.cabinet.shelves..z_min + pick_and_place.cabinet.shelves..z_max + + Blackboard output: + shelf_data : dict + { + "shelf_1": { + "objects": ["cereal", "oats"], + "category": "cereal", + }, + ... + } + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_output_key("shelf_data") + + self.node = yasmin_ros.logger_node + + # ClassifyCategory instance reused for each shelf + self._classifier = ClassifyCategory(task="shelf") + + # TODO: initialise torso action client + # self._torso_client = ActionClient( + # self.node, FollowJointTrajectoryAction, + # "/torso_controller/follow_joint_trajectory" + # ) + + 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") + .get_parameter_value() + .string_array_value + ) + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Could not load shelf IDs from params: {e}") + return "failed" + + if not shelf_ids: + yasmin.YASMIN_LOG_ERROR("No shelf IDs found in parameters.") + return "failed" + + for shelf_id in shelf_ids: + yasmin.YASMIN_LOG_INFO(f"Scanning shelf: {shelf_id}") + + # ── 1. Get shelf config ─────────────────────────────────────── + if not self._get_shelf_config(shelf_id): + yasmin.YASMIN_LOG_ERROR(f"Failed to get config for shelf {shelf_id}.") + return "failed" + + # ── 2. Adjust torso ─────────────────────────────────────────── + self._adjust_torso(shelf_id) + + # ── 3. Look at shelf ────────────────────────────────────────── + # TODO: call LookToPoint with self._current_look_point + yasmin.YASMIN_LOG_INFO( + f"[TODO] Looking at shelf {shelf_id} " + f"at point {self._current_look_point.point}." + ) + + # ── 4. Detect objects on shelf ──────────────────────────────── + object_names = self._detect_objects(shelf_id, blackboard) + + # ── 5. Classify shelf via ClassifyCategory ──────────────────── + blackboard["object_names"] = object_names + outcome = self._classifier.execute(blackboard) + + if outcome == "failed": + yasmin.YASMIN_LOG_WARN( + f"ClassifyCategory failed for shelf {shelf_id}. " + "Marking as unknown." + ) + shelf_category = "unknown" + else: + shelf_category = blackboard["shelf_category"] + + shelf_data[shelf_id] = { + "objects": object_names, + "category": shelf_category, + } + + yasmin.YASMIN_LOG_INFO( + f"Shelf {shelf_id}: category='{shelf_category}', " + f"objects={object_names}." + ) + + blackboard["shelf_data"] = shelf_data + return "succeeded" + + # ── Private helpers ─────────────────────────────────────────────────────── + + def _get_shelf_config(self, shelf_id: str) -> bool: + """Reads shelf-specific params and caches them on self.""" + try: + prefix = f"pick_and_place.cabinet.shelves.{shelf_id}" + + self._current_torso_height = ( + self.node.get_parameter(f"{prefix}.torso_lift_joint") + .get_parameter_value() + .double_value + ) + + look_pt = ( + self.node.get_parameter(f"{prefix}.look_point") + .get_parameter_value() + .double_array_value + ) + self._current_look_point = PointStamped( + point=Point(x=look_pt[0], y=look_pt[1], z=look_pt[2]), + 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) + + self._current_z_min = ( + self.node.get_parameter(f"{prefix}.z_min") + .get_parameter_value() + .double_value + ) + self._current_z_max = ( + self.node.get_parameter(f"{prefix}.z_max") + .get_parameter_value() + .double_value + ) + return True + + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Error reading params for shelf {shelf_id}: {e}") + return False + + def _adjust_torso(self, shelf_id: str) -> None: + """ + Moves the torso to the correct height for viewing this shelf. + TODO: replace log with FollowJointTrajectory action call. + """ + yasmin.YASMIN_LOG_INFO( + f"[TODO] Adjusting torso to {self._current_torso_height:.3f}m " + f"for shelf {shelf_id}." + ) + + def _detect_objects(self, shelf_id: str, blackboard) -> list: + """ + Runs DetectAllInPolygon within the shelf polygon and returns + a list of detected object name strings. + + DetectAllInPolygon outputs to blackboard["detected_objects"] as + List[Detection3D]. We extract just the names here since that is + all ClassifyCategory and shelf_data need. + """ + try: + detector = DetectAllInPolygon( + polygon=self._current_polygon, + min_confidence=0.1, + # TODO: switch to robocup.pt or your competition model + model="yolo11n-seg.pt", + ) + + # DetectAllInPolygon needs these keys initialised + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] + + outcome = detector.execute(blackboard) + + if outcome == "failed": + yasmin.YASMIN_LOG_WARN( + f"DetectAllInPolygon failed for shelf {shelf_id}." + ) + return [] + + return [obj.name for obj in blackboard["detected_objects"]] + + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"Detection failed for shelf {shelf_id}: {e}") + return [] 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 new file mode 100644 index 000000000..a55360110 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/select_and_visualize_object.py @@ -0,0 +1,121 @@ +import cv2 +import yasmin +import yasmin_ros +import rclpy +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +from rclpy.qos import QoSProfile, DurabilityPolicy + + +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. + + 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. + + 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 + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"]) + self.add_input_key("detected_objects") + self.add_output_key("selected_object") + self.add_output_key("selected_object_name") + + self.node = yasmin_ros.logger_node + self._bridge = CvBridge() + + # Latched publisher so the referee view stays visible after publish + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self._referee_pub = self.node.create_publisher(Image, "/referee_view", qos) + + 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 + blackboard["selected_object_name"] = selected.name + + yasmin.YASMIN_LOG_INFO(f"Selected object: {selected.name}") + + # ── 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) + + return "succeeded" + + 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: + yasmin.YASMIN_LOG_WARN("Could not get camera image for visualisation.") + return + + label = detection.name + xywh = detection.xywh + confidence = detection.confidence + + cv_im = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="rgb8") + + cv2.rectangle( + cv_im, + (int(xywh[0]), int(xywh[1])), + (int(xywh[0] + xywh[2]), int(xywh[1] + xywh[3])), + (0, 255, 0), + 2, + ) + cv2.putText( + cv_im, + f"{label} {confidence:.2f}", + (int(xywh[0]), int(xywh[1] - 10)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 255, 0), + 2, + ) + + self._referee_pub.publish( + self._bridge.cv2_to_imgmsg(cv_im, encoding="rgb8") + ) + yasmin.YASMIN_LOG_INFO("Published visualisation to /referee_view.") + + except Exception as e: + yasmin.YASMIN_LOG_WARN(f"Could not publish visualisation: {e}") diff --git a/tasks/pick_and_place/pick_and_place/states/start.py b/tasks/pick_and_place/pick_and_place/states/start.py new file mode 100644 index 000000000..b748c6c25 --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/states/start.py @@ -0,0 +1,116 @@ +import yasmin +import yasmin_ros + +from std_msgs.msg import Empty +from lasr_skills import Say, GoToLocation, DetectDoorOpening + + +class Start(yasmin.StateMachine): + """ + Entry sequence for the Pick and Place task. + + Ported from ROS 1 SMACH Start. The five-state sequence collapses + into a YASMIN StateMachine using lasr_skills states directly. + + Sequence: + 1. Wait for start signal on /pick_and_place/start + 2. Say "Start of Pick and Place task" + 3. Say "Waiting for the door to open" + 4. Detect door opening + 5. Navigate to the table + 6. Ask referee to open cabinet doors + + Blackboard outputs: + (none — all navigation targets loaded from params) + """ + + def __init__(self): + super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) + + # 1. Wait for start signal + def wait_cb(blackboard, msg): + yasmin.YASMIN_LOG_INFO("Received start signal.") + return "succeeded" + + self.add_state( + "WAIT_START", + yasmin_ros.MonitorState( + topic_name="/pick_and_place/start", + msg_type=Empty, + monitor_handler=wait_cb, + outcomes=["succeeded", "failed"], + ), + transitions={ + "succeeded": "SAY_START", + "failed": "WAIT_START", + "canceled": "failed", + }, + ) + + # 2. Announce start + self.add_state( + "SAY_START", + Say(text="Start of Pick and Place task."), + transitions={ + "succeeded": "SAY_WAITING", + "failed": "SAY_WAITING", + "aborted": "SAY_WAITING", + }, + ) + + # 3. Say waiting for door + self.add_state( + "SAY_WAITING", + Say(text="Waiting for the door to open."), + transitions={ + "succeeded": "WAIT_FOR_DOOR", + "failed": "WAIT_FOR_DOOR", + "aborted": "WAIT_FOR_DOOR", + }, + ) + + # 4. Detect door opening + self.add_state( + "WAIT_FOR_DOOR", + DetectDoorOpening(timeout=1.0), + transitions={ + "door_opened": "SAY_GOING_TO_TABLE", + "failed": "WAIT_FOR_DOOR", + }, + ) + + # 5. Announce navigation + self.add_state( + "SAY_GOING_TO_TABLE", + Say(text="I am going to the table."), + transitions={ + "succeeded": "GO_TO_TABLE", + "failed": "GO_TO_TABLE", + "aborted": "GO_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", + }, + ) + + # 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.." + ), + transitions={ + "succeeded": "succeeded", + "failed": "succeeded", + "aborted": "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 new file mode 100644 index 000000000..632874aff --- /dev/null +++ b/tasks/pick_and_place/pick_and_place/test_detect.py @@ -0,0 +1,27 @@ +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() diff --git a/tasks/pick_and_place/resource/pick_and_place b/tasks/pick_and_place/resource/pick_and_place new file mode 100644 index 000000000..e69de29bb diff --git a/tasks/pick_and_place/setup.cfg b/tasks/pick_and_place/setup.cfg new file mode 100644 index 000000000..b6635a285 --- /dev/null +++ b/tasks/pick_and_place/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pick_and_place +[install] +install_scripts=$base/lib/pick_and_place diff --git a/tasks/pick_and_place/setup.py b/tasks/pick_and_place/setup.py new file mode 100644 index 000000000..88e45e069 --- /dev/null +++ b/tasks/pick_and_place/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + +package_name = "pick_and_place" + +setup( + name=package_name, + 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"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="yara", + maintainer_email="yaralkhelaiwi@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + extras_require={ + "test": [ + "pytest", + ], + }, + entry_points={ + "console_scripts": [ + "state_machine = pick_and_place.state_machine:main", + "test_detect = pick_and_place.test_detect:main", + ], + }, +) diff --git a/tasks/pick_and_place/test/test_copyright.py b/tasks/pick_and_place/test/test_copyright.py new file mode 100644 index 000000000..ceffe896d --- /dev/null +++ b/tasks/pick_and_place/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/tasks/pick_and_place/test/test_flake8.py b/tasks/pick_and_place/test/test_flake8.py new file mode 100644 index 000000000..ee79f31ac --- /dev/null +++ b/tasks/pick_and_place/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/tasks/pick_and_place/test/test_pep257.py b/tasks/pick_and_place/test/test_pep257.py new file mode 100644 index 000000000..a2c3deb8e --- /dev/null +++ b/tasks/pick_and_place/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" From a0512731f7f2af08399c6a855e5d86b9ba1db1af Mon Sep 17 00:00:00 2001 From: Illia Putintsev Date: Fri, 19 Jun 2026 13:25:42 +0100 Subject: [PATCH 09/21] pick and place clip --- .../lasr_vision_open_vocabulary/node.py | 94 ++++ skills/src/lasr_skills/go_to_location.py | 4 +- tasks/pick_and_place/config/config.yaml | 34 +- .../launch/pick_and_place.launch.py | 60 +-- .../pick_and_place/detect_tuner.py | 220 +++++++++ .../pick_and_place/state_machine.py | 31 +- .../pick_and_place/states/__init__.py | 5 +- .../states/add_table_collision.py | 418 +++++++++++++++++ .../pick_and_place/states/approach_table.py | 149 ++++++ .../states/classify_category.py | 42 +- .../pick_and_place/states/detect_objects.py | 7 +- .../states/find_and_go_to_table.py | 3 +- .../pick_and_place/states/grasp_object.py | 439 ++++++++++++++++++ .../pick_and_place/states/instruct_place.py | 1 - .../pick_and_place/states/start.py | 6 +- tasks/pick_and_place/setup.py | 1 + 16 files changed, 1449 insertions(+), 65 deletions(-) create mode 100644 tasks/pick_and_place/pick_and_place/detect_tuner.py create mode 100644 tasks/pick_and_place/pick_and_place/states/add_table_collision.py create mode 100644 tasks/pick_and_place/pick_and_place/states/approach_table.py create mode 100644 tasks/pick_and_place/pick_and_place/states/grasp_object.py 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..ae1e38560 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 @@ -36,6 +36,26 @@ def __init__(self): encoder_path = self.get_parameter("sam_encoder_path").value decoder_path = self.get_parameter("sam_decoder_path").value + # CLIP recognition rerank: detector localises (boxes), CLIP re-labels each + # crop against a candidate list. OFF by default - enabled per-task via + # params (e.g. pick_and_place passes clip_rerank:=true + clip_candidates). + self.declare_parameter("clip_rerank", False) + 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 +116,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 +140,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/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index 4ac04c95c..0a2b77c99 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -1,6 +1,8 @@ from typing import Union import rclpy +import time + import yasmin from yasmin import StateMachine, State, Blackboard import yasmin_ros @@ -81,7 +83,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" diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index 5581737e8..3c59af0d7 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -7,35 +7,43 @@ trash_category: "snack" # Open-vocab query words for table detection (common nouns). - objects: ["cup", "can", "bottle", "bowl", "box"] + objects: ["cup", "can", "bottle", "bowl", "box", "apple"] table: pose: - position: {x: 3.0, y: -2.7, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.23} + position: {x: 0.2685513414272136, y: -1.0408096690305066, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.9975020239235763, w: 0.07063789541293575} + observe_pose: + position: {x: 0.5876252953308133, y: -1.0687492206833655, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.996990359390238, w: 0.07752562984538826} look_point: [5.25, 2.27, 0.78] 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] z_min: 0.7 z_max: 1.5 + collision: + detect: true + frame_id: map + size: [1.02, 1.2, 0.9] + position: [1.2, -3.9, 0.37] # ── DESTINATION 1: dishwasher (tableware + cutlery) ── dishwasher: pose: - position: {x: 1.1, y: 3.0, z: 0.0} # <-- ВСТАВ pose (ros2 topic echo --once /amcl_pose) - orientation: {x: 0.0, y: 0.0, z: -0.62, w: 0.78} + position: {x: -0.369876712341527, y: 1.1809765765046163, z: 0.0} # <-- ВСТАВ pose (ros2 topic echo --once /amcl_pose) + orientation: {x: 0.0, y: 0.0, z: 0.9920314752902727, w: 0.1259902854723536} # ── DESTINATION 2: trash bin ── trash_bin: pose: - position: {x: 6.0, y: -2.23, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: -0.91, w: 0.41} + position: {x: 2.1755595062984496, y: 0.9536759374323881, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: 0.6918191579917108, w: 0.7220708086023423} # ── DESTINATION 3: cabinet (fake: boxes with objects) ── cabinet: pose: - position: {x: -4.9, y: 2.7, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: -0.97, w: 0.23} + position: {x: -0.6363920463255814, y: -2.107459083686513, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.8431441386528592, w: 0.5376876058226823} # Shelves — used by ScanShelves in the NEXT deliverable. # shelf_order is the iteration list; shelves. holds per-shelf config. @@ -59,4 +67,10 @@ look_point: [0.0, 0.0, 0.4] polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] z_min: 0.3 - z_max: 0.6 \ No newline at end of file + z_max: 0.6 + + grasp: + enable: false + reach: 0.80 + use_moveit: true + publish_box: true \ 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 608e559a7..49503e4ca 100644 --- a/tasks/pick_and_place/launch/pick_and_place.launch.py +++ b/tasks/pick_and_place/launch/pick_and_place.launch.py @@ -2,40 +2,39 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +# CLIP recognition candidates: the open-vocab detector LOCALISES objects (boxes), +# then CLIP re-labels each crop against THIS list (fixes "Pringles -> cup"). Names +# must be CATEGORY_MAP-friendly so routing works (see classify_category.py). +CLIP_CANDIDATES = [ + "pringles", "iced tea", "apple", "bottle", "can", "coke", "cup", "sprite", "water bottle", "banana", +] + def generate_launch_description(): """ One-shot launch for the Pick and Place task. - Brings up everything that used to be started in separate terminals EXCEPT - the robot platform itself: - - open-vocabulary detection service (open_vocab/detect) ← required - - detection visualiser (comes with the above) - - LLM category-fallback service (optional, use_llm:=true) - - the task state machine (state_machine) - - point-head stub (/head_controller/point_head_action) + Perception = open-vocab detection (localisation) + CLIP rerank (recognition), + both inside the lasr_vision_open_vocabulary node (its venv already has + transformers/torch — CLIP comes for free, no new deps). - Still launch SEPARATELY (the platform, unchanged between runs): - - the simulator / robot bringup (camera, TF, controllers) - - nav2 + localisation (map, /amcl_pose) — GoToLocation needs this + Still launch SEPARATELY: simulator / robot bringup + nav2 + localisation. - Start the task after open_vocab has finished loading its model: + Start after the model has loaded: 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") - open_vocab_launch = os.path.join( + ov_params = os.path.join( get_package_share_directory("lasr_vision_open_vocabulary"), - "launch", - "open_vocab.launch.py", + "config", + "params.yaml", ) use_llm = LaunchConfiguration("use_llm") @@ -45,16 +44,25 @@ def generate_launch_description(): "use_llm", default_value="false", description="Also start the storing_groceries LLM service " - "(category fallback). Forced onto CPU to avoid GPU OOM. " - "Most groceries resolve via CATEGORY_MAP, so default off.", + "(category fallback). Forced onto CPU. Default off.", ), - # ── Perception: open-vocabulary detection (open_vocab/detect) ───────── - # Reuses lasr_vision_open_vocabulary's own params.yaml (model / device / - # weights). Keep your local fix there: grounding_dino_weights: '' and - # model_device set for your GPU. - IncludeLaunchDescription( - PythonLaunchDescriptionSource(open_vocab_launch), + # ── Perception: open-vocab detection + CLIP recognition rerank ──────── + Node( + package="lasr_vision_open_vocabulary", + executable="open_vocabulary_node", + name="lasr_vision_open_vocabulary", + output="screen", + parameters=[ + ov_params, + {"clip_rerank": True, "clip_candidates": CLIP_CANDIDATES}, + ], + ), + Node( + package="lasr_vision_open_vocabulary", + executable="detection_visualizer", + name="detection_visualizer", + output="screen", ), # ── Optional: LLM category-fallback service (CPU-forced) ───────────── @@ -76,7 +84,7 @@ def generate_launch_description(): parameters=[config], ), - # ── Head stub: serves /head_controller/point_head_action ───────────── + # ── Head stub ──────────────────────────────────────────────────────── Node( package="pick_and_place", executable="point_head_stub", 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/state_machine.py b/tasks/pick_and_place/pick_and_place/state_machine.py index 94afbd4ec..771fe2323 100644 --- a/tasks/pick_and_place/pick_and_place/state_machine.py +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -18,6 +18,9 @@ ChooseShelf, InstructPick, InstructPlace, + AddTableCollision, + GraspObject, + ApproachTable ) from rclpy.executors import MultiThreadedExecutor as Executor @@ -64,11 +67,28 @@ def __init__(self): "START", Start(), transitions={ - "succeeded": "DETECT_OBJECTS", + "succeeded": "ADD_TABLE_COLLISION", "failed": "failed", }, ) + self.add_state( + "ADD_TABLE_COLLISION", + AddTableCollision(head_tilt=-0.6), # детект усього столу здалеку + transitions={ + "succeeded": "GO_TO_TABLE_FOR_PICK", # було "DETECT_OBJECTS" + }, + ) + + 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", @@ -125,11 +145,14 @@ def __init__(self): "INSTRUCT_PICK", InstructPick(), transitions={ - "succeeded": "GO_TO_DESTINATION", + "succeeded": "GRASP", "failed": "INSTRUCT_PICK", # retry instruction }, ) - + self.add_state( + "GRASP", GraspObject(), + transitions={"succeeded": "GO_TO_DESTINATION", "failed": "GO_TO_DESTINATION"}, + ) # ── Navigate to the chosen destination (pose set by DecideDestination)─ self.add_state( "GO_TO_DESTINATION", @@ -194,7 +217,7 @@ def main(): yasmin_ros.set_ros_loggers(node) sm = PickAndPlace() - + # Uncomment to visualise the state machine in RViz/browser # YasminViewerPub(sm) 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 a2f1eed48..be49845f6 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -7,4 +7,7 @@ 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 .add_table_collision import AddTableCollision +from .grasp_object import GraspObject +from .approach_table import ApproachTable \ No newline at end of file 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/classify_category.py b/tasks/pick_and_place/pick_and_place/states/classify_category.py index 4ab71481d..639bf0f92 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,37 +3,37 @@ 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", + "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", + "fork", "knife", "spoon", "plate", "bowl", "wine glass", "mug", "chopsticks", }, } @@ -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/detect_objects.py b/tasks/pick_and_place/pick_and_place/states/detect_objects.py index 6989d1186..07de474c9 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 @@ -44,14 +44,14 @@ class DetectObjects(yasmin.State): HEAD_PAN_JOINT = "head_1_joint" HEAD_TILT_JOINT = "head_2_joint" - HEAD_TILT_DOWN = -0.4 + HEAD_TILT_DOWN = -0.65 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 + DEFAULT_QUERIES = ["cup", "can", "bottle", "bowl", "box", "iced tea", "apple"] + BOX_THRESHOLD = 0.25 TEXT_THRESHOLD = 0.10 NMS_IOU = 0.5 @@ -243,7 +243,6 @@ def execute(self, blackboard): 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: 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_place.py b/tasks/pick_and_place/pick_and_place/states/instruct_place.py index c36d09b9f..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 @@ -46,7 +46,6 @@ def execute(self, blackboard) -> str: text = ( f"Please place the {name} in {destination_str}{hint}. " - f"I will give you 5 seconds. 5.. 4.. 3.. 2.. 1.." ) say = Say(text=text) 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..407bad11d 100644 --- a/tasks/pick_and_place/pick_and_place/states/start.py +++ b/tasks/pick_and_place/pick_and_place/states/start.py @@ -93,7 +93,7 @@ def wait_cb(blackboard, msg): # 6. Navigate to table self.add_state( "GO_TO_TABLE", - GoToLocation(location_param="pick_and_place.table.pose"), + GoToLocation(location_param="pick_and_place.table.observe_pose"), transitions={ "succeeded": "ASK_OPEN_CABINET", "failed": "ASK_OPEN_CABINET", @@ -104,9 +104,7 @@ def wait_cb(blackboard, msg): 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.." + text="" ), transitions={ "succeeded": "succeeded", diff --git a/tasks/pick_and_place/setup.py b/tasks/pick_and_place/setup.py index e82bef3c2..9b651bbcc 100644 --- a/tasks/pick_and_place/setup.py +++ b/tasks/pick_and_place/setup.py @@ -28,6 +28,7 @@ "state_machine = pick_and_place.state_machine:main", "test_detect = pick_and_place.test_detect:main", "point_head_stub = pick_and_place.point_head_stub:main", + "detect_tuner = pick_and_place.detect_tuner:main", ], }, ) From 4288195cedf1e8493b0bdc537ba591420c620987 Mon Sep 17 00:00:00 2001 From: yarakmk Date: Fri, 19 Jun 2026 13:30:31 +0100 Subject: [PATCH 10/21] Fixes for simulation --- tasks/pick_and_place/config/config.yaml | 4 +-- .../pick_and_place/states/serve_breakfast.py | 6 ++--- .../pick_and_place/test_detect.py | 25 ------------------- 3 files changed, 5 insertions(+), 30 deletions(-) delete mode 100644 tasks/pick_and_place/pick_and_place/test_detect.py diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index b22962efe..eb336dc96 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -7,7 +7,7 @@ trash_category: "snack" # Open-vocab query words for table detection (common nouns). - objects: ["cup", "can", "bottle", "bowl", "box"] + objects: ["cup", "can", "bottle", "bowl", "box", "spoon"] table: pose: @@ -63,5 +63,5 @@ # ── BREAKFAST SURFACE (bowl and spoon pickup location) ── breakfast_surface: pose: - position: {x: 1.62, y: -3.68, z: 0.0063} # TODO: get from Gazebo + position: {x: 2.8, y: -3.8, z: 0.0063} orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.23} \ No newline at end of file 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 index 0afbd9ad7..071383206 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -122,7 +122,7 @@ def __init__(self): ) self.add_state( "DETECT_CEREAL_MILK", - DetectObjects(queries=["cereal", "milk"]), + DetectObjects(queries=["box", "bottle"]), transitions={ "succeeded": "SELECT_CEREAL", "failed": "DETECT_CEREAL_MILK", @@ -130,7 +130,7 @@ def __init__(self): ) self.add_state( "SELECT_CEREAL", - SelectAndVisualiseObject(target_name="cereal"), + SelectAndVisualiseObject(target_name="box"), transitions={ "succeeded": "INSTRUCT_PICK_CEREAL", "failed": "DETECT_CEREAL_MILK", @@ -146,7 +146,7 @@ def __init__(self): ) self.add_state( "SELECT_MILK", - SelectAndVisualiseObject(target_name="milk"), + SelectAndVisualiseObject(target_name="bottle"), transitions={ "succeeded": "INSTRUCT_PICK_MILK", "failed": "DETECT_CEREAL_MILK", 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 From e404d9fb89580b94dd83ebdb1850d5efb6aa457f Mon Sep 17 00:00:00 2001 From: yarakmk Date: Fri, 19 Jun 2026 18:08:01 +0100 Subject: [PATCH 11/21] Disabled navigation --- .../config/params.yaml | 2 +- .../pick_and_place/states/__init__.py | 3 +- .../states/select_and_visualize_object.py | 40 +++-- .../pick_and_place/states/serve_breakfast.py | 163 ++++++++++-------- .../pick_and_place/test_serve_breakfast.py | 21 ++- 5 files changed, 142 insertions(+), 87 deletions(-) diff --git a/common/vision/lasr_vision_open_vocabulary/config/params.yaml b/common/vision/lasr_vision_open_vocabulary/config/params.yaml index 2279ea798..5327ad088 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: '/home/robocup/Yara_ws/src/Base/common/vision/lasr_vision_open_vocabulary/models/grounding-dino-base' yoloworld_weights: 'yolov8s-world.pt' use_sam: false sam_encoder_path: '' 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 bf8551a5a..c562c2994 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -10,5 +10,6 @@ from .instruct_place import InstructPlace from .serve_breakfast import ServeBreakfast from .add_table_collision import AddTableCollision -from .grasp_object import GraspObject + +# from .grasp_object import GraspObject from .approach_table import ApproachTable 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 0aba3c07e..4daba0d82 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,4 +1,5 @@ import cv2 +import rclpy import yasmin import yasmin_ros from cv_bridge import CvBridge @@ -27,10 +28,10 @@ class SelectAndVisualiseObject(yasmin.State): 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() @@ -54,11 +55,13 @@ def execute(self, blackboard) -> str: yasmin.YASMIN_LOG_WARN( f"'{self._target_name}' not found in detected_objects." ) - return "failed" + return "finished" + detected.remove(selected) else: # Default behaviour for cleanup loops — always take the first - selected = detected[0] + selected = detected.pop(0) + blackboard["detected_objects"] = detected blackboard["selected_object"] = selected blackboard["selected_object_name"] = selected.name blackboard["object_name"] = selected.name @@ -77,19 +80,26 @@ def execute(self, blackboard) -> str: return "succeeded" - def _publish_visualisation(self, detection, blackboard) -> None: - """ - Draws a bounding box and label on the cached detection-time image - and publishes it to /referee_view, satisfying rule 16's perception - communication requirement. - """ + def _publish_visualisation(self, detection) -> None: try: - image_msg = blackboard.get("last_rgb_image") - if image_msg is None: - yasmin.YASMIN_LOG_WARN("No cached image available for visualisation.") + # 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: + yasmin.YASMIN_LOG_WARN("Could not get camera image for visualisation.") return + + label = detection.name + xywh = detection.xywh + confidence = detection.confidence + cv_im = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="rgb8") - xywh = detection.xywh # top-left format from DetectObjects + cv2.rectangle( cv_im, (int(xywh[0]), int(xywh[1])), @@ -97,16 +107,16 @@ def _publish_visualisation(self, detection, blackboard) -> None: (0, 255, 0), 2, ) - cv2.putText( cv_im, - f"{detection.name} {detection.confidence:.2f}", + f"{label} {confidence:.2f}", (int(xywh[0]), int(xywh[1] - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2, ) + self._referee_pub.publish( self._bridge.cv2_to_imgmsg(cv_im, encoding="rgb8") ) 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 index 071383206..8dd9d4f0e 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -10,48 +10,45 @@ class ServeBreakfast(yasmin.StateMachine): """ Sets up breakfast on the dining table after table cleanup is complete. - Bowl and spoon are detected on a designated surface, cereal and milk - are detected in the cabinet next to their respective categories. - - Detection uses open-vocabulary DetectObjects with a custom query list - per stop, and SelectAndVisualiseObject picks each named item out of - the detected pair via target_name. Every pick and place is delegated - to the human operator -- detection is used purely for recognition - scoring and referee visualisation, not for any manipulation. + 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_SPOON (queries=["bowl", "spoon"]) - -> SELECT_BOWL -> INSTRUCT_PICK_BOWL - -> SELECT_SPOON -> INSTRUCT_PICK_SPOON + -> DETECT_BOWL -> SELECT_BOWL -> INSTRUCT_PICK_BOWL + -> DETECT_SPOON -> SELECT_SPOON -> INSTRUCT_PICK_SPOON -> GO_TO_TABLE - -> INSTRUCT_PLACE_BOWL (centre of table) - -> INSTRUCT_PLACE_SPOON (next to bowl) + -> INSTRUCT_PLACE_BOWL + -> INSTRUCT_PLACE_SPOON -> GO_TO_CABINET - -> DETECT_CEREAL_MILK (queries=["cereal", "milk"]) - -> SELECT_CEREAL -> INSTRUCT_PICK_CEREAL - -> SELECT_MILK -> INSTRUCT_PICK_MILK + -> DETECT_CEREAL -> SELECT_CEREAL -> INSTRUCT_PICK_CEREAL + -> DETECT_MILK -> SELECT_MILK -> INSTRUCT_PICK_MILK -> GO_TO_TABLE - -> INSTRUCT_PLACE_CEREAL (next to bowl, with clearance) - -> INSTRUCT_PLACE_MILK (next to cereal, with clearance) + -> 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( - "GO_TO_BREAKFAST_SURFACE", - GoToLocation(location_param="pick_and_place.breakfast_surface.pose"), - transitions={ - "succeeded": "DETECT_BOWL_SPOON", - "failed": "GO_TO_BREAKFAST_SURFACE", - }, - ) - self.add_state( - "DETECT_BOWL_SPOON", - DetectObjects(queries=["bowl", "spoon"]), + "DETECT_BOWL", + DetectObjects(queries=["bowl"]), transitions={ "succeeded": "SELECT_BOWL", - "failed": "DETECT_BOWL_SPOON", + "failed": "DETECT_BOWL", }, ) self.add_state( @@ -59,41 +56,53 @@ def __init__(self): SelectAndVisualiseObject(target_name="bowl"), transitions={ "succeeded": "INSTRUCT_PICK_BOWL", - "failed": "DETECT_BOWL_SPOON", + "finished": "DETECT_BOWL", # not found, retry detection }, ) self.add_state( "INSTRUCT_PICK_BOWL", InstructPick(), transitions={ - "succeeded": "SELECT_SPOON", + "succeeded": "DETECT_SPOON", "failed": "INSTRUCT_PICK_BOWL", }, ) + + # Spoon + self.add_state( + "DETECT_SPOON", + DetectObjects(queries=["spoon"]), + transitions={ + "succeeded": "SELECT_SPOON", + "failed": "DETECT_SPOON", + }, + ) self.add_state( "SELECT_SPOON", SelectAndVisualiseObject(target_name="spoon"), transitions={ "succeeded": "INSTRUCT_PICK_SPOON", - "failed": "DETECT_BOWL_SPOON", + "finished": "DETECT_SPOON", # not found, retry detection }, ) self.add_state( "INSTRUCT_PICK_SPOON", InstructPick(), - transitions={ - "succeeded": "GO_TO_TABLE_1", - "failed": "INSTRUCT_PICK_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", + "failed": "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."), @@ -107,25 +116,29 @@ def __init__(self): "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", - }, - ) - self.add_state( - "GO_TO_CABINET", - GoToLocation(location_param="pick_and_place.cabinet.pose"), - transitions={ - "succeeded": "DETECT_CEREAL_MILK", - "failed": "GO_TO_CABINET", + "succeeded": "DETECT_CEREAL", + "aborted": "DETECT_CEREAL", + "canceled": "DETECT_CEREAL", }, ) + + # # 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_MILK", - DetectObjects(queries=["box", "bottle"]), + "DETECT_CEREAL", + DetectObjects(queries=["box"]), transitions={ "succeeded": "SELECT_CEREAL", - "failed": "DETECT_CEREAL_MILK", + "failed": "DETECT_CEREAL", }, ) self.add_state( @@ -133,46 +146,58 @@ def __init__(self): SelectAndVisualiseObject(target_name="box"), transitions={ "succeeded": "INSTRUCT_PICK_CEREAL", - "failed": "DETECT_CEREAL_MILK", + "finished": "DETECT_CEREAL", # not found, retry detection }, ) self.add_state( "INSTRUCT_PICK_CEREAL", InstructPick(), transitions={ - "succeeded": "SELECT_MILK", + "succeeded": "DETECT_MILK", "failed": "INSTRUCT_PICK_CEREAL", }, ) + + # Milk + self.add_state( + "DETECT_MILK", + DetectObjects(queries=["bottle"]), + transitions={ + "succeeded": "SELECT_MILK", + "failed": "DETECT_MILK", + }, + ) self.add_state( "SELECT_MILK", SelectAndVisualiseObject(target_name="bottle"), transitions={ "succeeded": "INSTRUCT_PICK_MILK", - "failed": "DETECT_CEREAL_MILK", + "finished": "DETECT_MILK", # not found, retry detection }, ) self.add_state( "INSTRUCT_PICK_MILK", InstructPick(), - transitions={ - "succeeded": "GO_TO_TABLE_2", - "failed": "INSTRUCT_PICK_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", + "failed": "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, " - "leaving at least five centimetres of clear space." + "with sufficient space between them." ), transitions={ "succeeded": "INSTRUCT_PLACE_MILK", @@ -184,7 +209,7 @@ def __init__(self): "INSTRUCT_PLACE_MILK", Say( text="Please place the milk next to the cereal, " - "leaving at least five centimetres of clear space." + "with sufficient space between them." ), transitions={ "succeeded": "succeeded", 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 index 308d7ced1..15e2687d2 100644 --- a/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py @@ -6,22 +6,41 @@ def main(): + rclpy.init() + yasmin_ros.set_ros_loggers() bb = yasmin.Blackboard() - # Initialise all keys ServeBreakfast needs bb["detected_objects"] = [] + bb["debug_images"] = [] + bb["selected_object"] = None + bb["selected_object_name"] = "" + bb["last_rgb_image"] = None + bb["object_name"] = "" + + # Add these three for InstructPick/InstructPlace + + bb["object_category"] = "breakfast item" + + bb["destination_str"] = "the dining table" + + bb["chosen_shelf"] = "" + + bb["chosen_shelf_str"] = "" + sm = ServeBreakfast() + outcome = sm(bb) yasmin.YASMIN_LOG_INFO(f"ServeBreakfast finished with outcome: {outcome}") + rclpy.shutdown() From 411314161ca593fe5a7602e01b4ccbffd92d04a2 Mon Sep 17 00:00:00 2001 From: Illia Putintsev Date: Fri, 19 Jun 2026 19:22:47 +0100 Subject: [PATCH 12/21] merge ros2 --- .../launch/pick_and_place.launch.py | 12 ++- .../pick_and_place/states/detect_objects.py | 33 +++++++- .../pick_and_place/vlm_classifier.py | 79 +++++++++++++++++++ 3 files changed, 120 insertions(+), 4 deletions(-) create mode 100644 tasks/pick_and_place/pick_and_place/vlm_classifier.py 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 49503e4ca..d1cf83ffb 100644 --- a/tasks/pick_and_place/launch/pick_and_place.launch.py +++ b/tasks/pick_and_place/launch/pick_and_place.launch.py @@ -6,7 +6,7 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node - +from launch.actions import ExecuteProcess # CLIP recognition candidates: the open-vocab detector LOCALISES objects (boxes), # then CLIP re-labels each crop against THIS list (fixes "Pringles -> cup"). Names @@ -55,7 +55,7 @@ def generate_launch_description(): output="screen", parameters=[ ov_params, - {"clip_rerank": True, "clip_candidates": CLIP_CANDIDATES}, + {"clip_rerank": False, "clip_candidates": CLIP_CANDIDATES}, ], ), Node( @@ -91,4 +91,12 @@ def generate_launch_description(): name="point_head_stub", output="screen", ), + ExecuteProcess( + cmd=["bash", "-c", + "curl -sf localhost:11434/api/tags >/dev/null 2>&1 " + "&& echo 'ollama already running' " + "|| exec ollama serve"], + name="ollama_serve", + output="screen", + ), ]) \ 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 07de474c9..42fd588c0 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 @@ -23,6 +23,8 @@ from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from builtin_interfaces.msg import Duration as DurationMsg +from pick_and_place.vlm_classifier import classify_crop + class DetectObjects(yasmin.State): """ @@ -33,7 +35,8 @@ class DetectObjects(yasmin.State): 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). + 5. VLM naming: a local VLM (Ollama) names each kept crop (replaces CLIP). + 6. Project each kept box centre → 3D via depth + TF (for manipulation later). ROS 2 params: pick_and_place.objects — query words (COMMON NOUNS). Empty → default. @@ -50,11 +53,18 @@ class DetectObjects(yasmin.State): DEPTH_TOPIC = "/head_front_camera/depth/image_raw" INFO_TOPIC = "/head_front_camera/rgb/camera_info" - DEFAULT_QUERIES = ["cup", "can", "bottle", "bowl", "box", "iced tea", "apple"] + DEFAULT_QUERIES = ["cup", "can", "bottle", "bowl", "apple"] BOX_THRESHOLD = 0.25 TEXT_THRESHOLD = 0.10 NMS_IOU = 0.5 + # ── VLM naming (Ollama). DINO finds the boxes; the VLM says what each is. + # Run clip_rerank:=false in the launch — the VLM replaces CLIP here. + VLM_ENABLE = True + VLM_MODEL = "moondream" + VLM_HOST = "http://localhost:11434" + VLM_TIMEOUT = 60.0 + def __init__(self): super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detected_objects") @@ -238,8 +248,27 @@ def execute(self, blackboard): cleaned = [(self._clean_label(n), c, b) for n, c, b in raw] kept = self._nms(cleaned) + # VLM naming: convert the RGB once, then let the VLM name each kept crop. + rgb_cv = None + if self.VLM_ENABLE: + try: + rgb_cv = self.bridge.imgmsg_to_cv2(self._rgb, "bgr8") + except Exception as e: + yasmin.YASMIN_LOG_WARN( + f"VLM: cannot convert RGB ({e}); keeping DINO labels." + ) + detected = [] for name, conf, (cx, cy, w, h) in kept: + if rgb_cv is not None: + vlm_name = classify_crop( + rgb_cv, (cx, cy, w, h), + model=self.VLM_MODEL, host=self.VLM_HOST, timeout=self.VLM_TIMEOUT, + ) + if vlm_name and vlm_name != name: + yasmin.YASMIN_LOG_INFO(f"VLM: '{name}' -> '{vlm_name}'") + name = vlm_name + d3 = Detection3D() d3.name = name d3.confidence = float(conf) 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..bbfb1acad --- /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", + "red bull", "apple", "banana", "cup", "mug", "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 From bc3402ad527b15042c42c0657dc7742b843403b4 Mon Sep 17 00:00:00 2001 From: yarakmk Date: Sun, 21 Jun 2026 15:10:45 +0100 Subject: [PATCH 13/21] edit config file --- tasks/pick_and_place/config/config.yaml | 12 +-- .../states/select_and_visualize_object.py | 37 +++++--- .../pick_and_place/states/serve_breakfast.py | 84 ++++++++++--------- .../pick_and_place/test_serve_breakfast.py | 2 +- 4 files changed, 78 insertions(+), 57 deletions(-) diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index c22bb5222..2b0398ef6 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -11,8 +11,8 @@ table: pose: - position: {x: 4.65, y: 2.5, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.2919} + position: {x: 0.41411805152893066, y: -1.4740591049194336, z: 0.002471923828125} + orientation: {x: 0.0, y: 0.0, z: -0.788207, w: 0.615411} 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] @@ -20,8 +20,8 @@ z_max: 1.5 cabinet: pose: - position: {x: -4.9, y: 2.7, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: -0.97, w: 0.23} + position: {x: 1.2077007293701172, y: -2.136275053024292, z: 0.2945556640625} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.178426, w: 0.983953} # Shelves — used by ScanShelves in the NEXT deliverable. # shelf_order is the iteration list; shelves. holds per-shelf config. @@ -55,5 +55,5 @@ # ── BREAKFAST SURFACE (bowl and spoon pickup location) ── breakfast_surface: pose: - position: {x: 2.8, y: -3.8, z: 0.0063} - orientation: {x: 0.0, y: 0.0, z: -0.9565, w: 0.23} \ No newline at end of file + position: {x: 0.10039059072732925, y: -0.8091638684272766, z: -0.001434326171875} + orientation: {x: 0.0, y: 0.0, z: 0.999945, w: 0.0104897} \ No newline at end of file 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 4daba0d82..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 @@ -4,7 +4,7 @@ import yasmin_ros from cv_bridge import CvBridge from sensor_msgs.msg import Image -from rclpy.qos import QoSProfile, DurabilityPolicy +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy class SelectAndVisualiseObject(yasmin.State): @@ -38,6 +38,19 @@ def __init__(self, target_name: str = None): qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) 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 + ) + + def _on_image(self, msg): + self._last_image = msg + def execute(self, blackboard) -> str: detected = blackboard["detected_objects"] if not detected: @@ -82,15 +95,15 @@ 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 @@ -98,7 +111,9 @@ def _publish_visualisation(self, detection) -> None: 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, 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 index 8dd9d4f0e..2f8b2fb4c 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -33,14 +33,14 @@ 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", - # }, - # ) + 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( @@ -51,6 +51,7 @@ def __init__(self): "failed": "DETECT_BOWL", }, ) + self.add_state( "SELECT_BOWL", SelectAndVisualiseObject(target_name="bowl"), @@ -59,6 +60,7 @@ def __init__(self): "finished": "DETECT_BOWL", # not found, retry detection }, ) + self.add_state( "INSTRUCT_PICK_BOWL", InstructPick(), @@ -77,6 +79,7 @@ def __init__(self): "failed": "DETECT_SPOON", }, ) + self.add_state( "SELECT_SPOON", SelectAndVisualiseObject(target_name="spoon"), @@ -85,24 +88,26 @@ def __init__(self): "finished": "DETECT_SPOON", # not found, retry detection }, ) + self.add_state( "INSTRUCT_PICK_SPOON", InstructPick(), transitions={ - "succeeded": "INSTRUCT_PLACE_BOWL", + "succeeded": "INSTRUCT_PICK_SPOON", "failed": "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", - # }, - # ) + # 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."), @@ -116,21 +121,21 @@ def __init__(self): "INSTRUCT_PLACE_SPOON", Say(text="Please place the spoon next to the bowl."), transitions={ - "succeeded": "DETECT_CEREAL", - "aborted": "DETECT_CEREAL", - "canceled": "DETECT_CEREAL", + "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", - # }, - # ) + # 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( @@ -179,20 +184,21 @@ def __init__(self): "INSTRUCT_PICK_MILK", InstructPick(), transitions={ - "succeeded": "INSTRUCT_PLACE_CEREAL", + "succeeded": "GO_TO_TABLE_2", "failed": "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( + "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( 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 index 15e2687d2..fe84748c2 100644 --- a/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py @@ -27,7 +27,7 @@ def main(): # Add these three for InstructPick/InstructPlace - bb["object_category"] = "breakfast item" + bb["object_category"] = "breakfast" bb["destination_str"] = "the dining table" From 8003c7c4b06e21ba9980c2f7799e039cc3ee07b8 Mon Sep 17 00:00:00 2001 From: michelebri <83598165+michelebri@users.noreply.github.com> Date: Wed, 24 Jun 2026 10:14:56 +0200 Subject: [PATCH 14/21] GPSR (#455) * start of GPSR package * Refactor GPSR state machine to use YASMIN, add AnnouncePlan state, and update TTS handling * Refactor: Black reformat * Add configuration files and update parameters for GPSR task modified prompt * - Introduced world.py to manage skills, locations, objects, people, and general knowledge for the GPSR task. - Added general knowledge configuration in general_knowledge.yaml. * Add prompts for skill selection, refinement, and planning; update locations, objects, and people configurations * Refactor GPSR task to enhance LLM integration and streamline planning process - Refactored agent.py to support cloud and local LLM dispatching, including new planning and querying functionalities. - Introduced planner.py for skill selection and planning pipeline. - Enhanced prompts for skill selection and planning. - Updated state machine to remove unnecessary states and streamline command processing. - Improved world management with new functions for loading and formatting skills, locations, and objects. - Removed deprecated states and configurations to simplify the codebase. * implemented go to skill and readme * Refactor: Black format --------- Co-authored-by: Ma'ayan Armony --- .gitignore | 2 +- common/foundation_models/lasr_vlm/setup.py | 1 - common/simulation/maps/label_locations.py | 5 +- common/simulation/maps/locations.yaml | 24 +- tasks/GPSR/GPSR/agent.py | 223 ++++++++++++++++--- tasks/GPSR/GPSR/planner.py | 106 +++++++++ tasks/GPSR/GPSR/prompts.py | 241 +++++++++++++++++++++ tasks/GPSR/GPSR/state_machine.py | 48 ++-- tasks/GPSR/GPSR/states/__init__.py | 4 - tasks/GPSR/GPSR/states/announce_plan.py | 22 -- tasks/GPSR/GPSR/states/dispatch_skill.py | 25 +-- tasks/GPSR/GPSR/states/input_state.py | 15 -- tasks/GPSR/GPSR/states/keyboard_input.py | 6 +- tasks/GPSR/GPSR/states/query_llm.py | 127 ++++------- tasks/GPSR/GPSR/states/speech_recovery.py | 207 ------------------ tasks/GPSR/GPSR/tts.py | 5 +- tasks/GPSR/GPSR/world.py | 139 ++++++++++++ tasks/GPSR/README.md | 73 +++++++ tasks/GPSR/config/general_knowledge.yaml | 2 + tasks/GPSR/config/locations.yaml | 38 +++- tasks/GPSR/config/objects.yaml | 152 +++++++++++++ tasks/GPSR/config/params.yaml | 22 +- tasks/GPSR/config/people.yaml | 10 + tasks/GPSR/config/skills.yaml | 14 ++ tasks/GPSR/external/requirements.txt | 1 + tasks/GPSR/external/server.py | 88 ++++++++ tasks/GPSR/requirements.txt | 2 + 27 files changed, 1167 insertions(+), 435 deletions(-) create mode 100644 tasks/GPSR/GPSR/planner.py create mode 100644 tasks/GPSR/GPSR/prompts.py delete mode 100644 tasks/GPSR/GPSR/states/announce_plan.py delete mode 100644 tasks/GPSR/GPSR/states/input_state.py delete mode 100644 tasks/GPSR/GPSR/states/speech_recovery.py create mode 100644 tasks/GPSR/GPSR/world.py create mode 100644 tasks/GPSR/README.md create mode 100644 tasks/GPSR/config/general_knowledge.yaml create mode 100644 tasks/GPSR/config/objects.yaml create mode 100644 tasks/GPSR/config/people.yaml create mode 100644 tasks/GPSR/config/skills.yaml create mode 100644 tasks/GPSR/external/requirements.txt create mode 100755 tasks/GPSR/external/server.py 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/common/foundation_models/lasr_vlm/setup.py b/common/foundation_models/lasr_vlm/setup.py index 339dee702..bfc97c43b 100644 --- a/common/foundation_models/lasr_vlm/setup.py +++ b/common/foundation_models/lasr_vlm/setup.py @@ -22,7 +22,6 @@ def run(self): setup( - cmdclass={"install": InstallCommand}, name=package_name, version="0.0.0", packages=find_packages(exclude=["test"]), 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/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 From 5157d53d5e945de78904c8180b16f96b26249053 Mon Sep 17 00:00:00 2001 From: Fadi <130671609+Fadi-Mostefai@users.noreply.github.com> Date: Fri, 26 Jun 2026 14:01:00 +0100 Subject: [PATCH 15/21] Full HRI MVP (#456) * Testing HRI SM2 * LLM package with venv * Fixed SM3 * RVIZ debug for HRI testing * Fixed lasr_llm by removing broken llama_cpp_python * Successfull SM2 of HRI * WIP - SM2 to SM3 * WIP: CHECKS * WIP - SM1 -> SM3 Final checks to see if it works consistently needs to be carried out * Ported recieve Object * Temporary detect3d fix * WIP: Follow Person Experimental * Merge branch 'ros2-hri-testing' into ros2-hri-sm6 * WIP: Planning SM6 * update waitforperson to use blackboard data as well * WIP: debugging stuck in loop * ran black formatting * Merge branch 'ros2' into ros2-hri-sm6 * Fixed broken merge * WIP: Debugging tracking * testing tf_listener instead of amcl pose for robot pose * Fixed tracking * Fixed following navigation * Fixed inconsistant following logic * Fixed bugs related to handle_sigint and service * Seperated updateable navigation to prevent conflicts * Fixed merge error and removed duplicate file * Fixed blocking logic * Added debug config for rviz * Ported additional States * HRI task (now SM1-5) (#447) * Ros2 Port Language Package * Introduction testing * Modifications. fix init before final push * Message filter * Launch file * State machine and setup.py modifications * Fix register face and recognise * Port HRI introduce states to YASMIN * Fix YOLO detections in introduce. * Initial code for pick and place * Bug fixes * Fixes after testing on the robot * Black formatting * HRI Task - SM1-3 fully tested and working on the robot (#443) * Testing HRI SM2 * LLM package with venv * Fixed SM3 * RVIZ debug for HRI testing * Fixed lasr_llm by removing broken llama_cpp_python * Successfull SM2 of HRI * WIP - SM2 to SM3 * WIP: CHECKS * WIP - SM1 -> SM3 Final checks to see if it works consistently needs to be carried out * Ported recieve Object * Temporary detect3d fix * Working SM1-3 of HRI --------- Co-authored-by: Aldrich-Fernandes * New VLM service to be used in HRI (#444) * vlm service * service update * Added back license and readme * Formatted files with black --------- Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi * VLM service for HRI task (#445) * Working VLM Replaced old vqa for describing people with new vlm service * Formatted files with black * Fixed accidental merge errors * WIP - Setting up and Testing HRI task * Fix introduce and remove testing code * WIP - HRI task semi-working SM1-4 works correctly, however during SM4 it doesn't look at the seated guests * Change LLM to Ollama got HRI get anem and drink * Updated Seat Guest (SM3) * WIP - HRI SM4 being refactored * Verbal introduction is correct * Debugging SM4 face person errors * Formatted files with black --------- Co-authored-by: Yara Alkhelaiwi Co-authored-by: Yara Alkhelaiwi Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi Co-authored-by: Aldrich-Fernandes Co-authored-by: Ma'ayan Armony * Added Follow Host to HRI * Fixed recovery states not being reached * Fixed looking at person while tracking * Fixed Detect_keypoints and started implementing place_bag * Added drop point detection * WIP: Place bag MVP * Removed potentially breaking handle_sigints * Updated GetPersonPoint logic and added goal forgiving * Updated wait times and removed redundent check * Merging fixed gpu version of HRI (#454) * HRI task (now SM1-5) (#447) * Ros2 Port Language Package * Introduction testing * Modifications. fix init before final push * Message filter * Launch file * State machine and setup.py modifications * Fix register face and recognise * Port HRI introduce states to YASMIN * Fix YOLO detections in introduce. * Initial code for pick and place * Bug fixes * Fixes after testing on the robot * Black formatting * HRI Task - SM1-3 fully tested and working on the robot (#443) * Testing HRI SM2 * LLM package with venv * Fixed SM3 * RVIZ debug for HRI testing * Fixed lasr_llm by removing broken llama_cpp_python * Successfull SM2 of HRI * WIP - SM2 to SM3 * WIP: CHECKS * WIP - SM1 -> SM3 Final checks to see if it works consistently needs to be carried out * Ported recieve Object * Temporary detect3d fix * Working SM1-3 of HRI --------- Co-authored-by: Aldrich-Fernandes * New VLM service to be used in HRI (#444) * vlm service * service update * Added back license and readme * Formatted files with black --------- Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi * VLM service for HRI task (#445) * Working VLM Replaced old vqa for describing people with new vlm service * Formatted files with black * Fixed accidental merge errors * WIP - Setting up and Testing HRI task * Fix introduce and remove testing code * WIP - HRI task semi-working SM1-4 works correctly, however during SM4 it doesn't look at the seated guests * Change LLM to Ollama got HRI get anem and drink * Updated Seat Guest (SM3) * WIP - HRI SM4 being refactored * Verbal introduction is correct * Debugging SM4 face person errors * Formatted files with black --------- Co-authored-by: Yara Alkhelaiwi Co-authored-by: Yara Alkhelaiwi Co-authored-by: rajhirym Co-authored-by: Yara Alkhelaiwi Co-authored-by: Aldrich-Fernandes Co-authored-by: Ma'ayan Armony * Added REID fallback Also made minor changes to accomodate gpu * Test HRI on GPU * Updated HRI to say attributes Also added in a SafeGoToLocation and moved StartDoorSM to skills * Debugging HRI task * Added gpu requirements for lasr_vision_reid * Testing of HRI with GPU * Fixing bugs * Fixing of EyeTracker * Working eye tracker, yolo is bottleneck * Fully fixed eye tracker * Adding in host * Fixed introduce for host * Formatted files * Merge branch 'ros2-hri-sm6' into ros2-gpu-and-reid * Added in host logic to SM * Testing HRI * Working HRI * Bug fixes * HRI MVP * Formatted files * Merge branch 'ros2' into ros2-hri-sm6 --- .../lasr_llm/lasr_llm/llm_inference.py | 25 +- .../lasr_llm/nodes/hri_task_service.py | 4 +- .../__init__.py | 1 + .../lasr_speech_recognition_whisper/cache.py | 16 +- .../transcribe_microphone_server.py | 8 +- .../scripts/microphone_tuning_test.py | 6 +- .../eye_tracker_action_server.py | 350 ++- .../action/EyeTracker.action | 6 +- .../lasr_vision_reid/add_face.py | 1 + .../lasr_vision_reid/relay_2d.py | 66 - .../lasr_vision_reid/relay_3d.py | 2 +- .../lasr_vision_reid/service.py | 140 +- .../vision/lasr_vision_reid/requirements.in | 2 +- .../vision/lasr_vision_reid/requirements.txt | 99 +- common/vision/lasr_vision_reid/setup.py | 1 - .../lasr_vision_yolo/service.py | 33 +- log.txt | 2662 +++++++++++++++++ skills/config/follow_debug.rviz | 785 +++++ skills/config/motions.yaml | 29 +- skills/launch/follow_person.launch.py | 51 + skills/setup.py | 3 + skills/src/lasr_skills/__init__.py | 24 +- .../lasr_skills/continuous_go_to_location.py | 121 + skills/src/lasr_skills/describe_people.py | 1 + skills/src/lasr_skills/detect_3d_in_area.py | 9 +- .../src/lasr_skills/detect_all_in_polygon.py | 34 +- skills/src/lasr_skills/detect_keypoints_3d.py | 172 ++ skills/src/lasr_skills/eye_tracker.py | 30 +- skills/src/lasr_skills/follow_person.py | 712 +++++ .../go_to_location_with_play_motion.py | 41 + skills/src/lasr_skills/handover_object.py | 206 -- skills/src/lasr_skills/receive_object.py | 78 +- skills/src/lasr_skills/rotate.py | 200 ++ .../src/lasr_skills/start_task.py | 18 +- skills/src/lasr_skills/wait_for_person.py | 5 +- .../lasr_skills/wait_for_person_in_area.py | 60 +- tasks/HRI/HRI/state_machine.py | 151 +- tasks/HRI/HRI/states/__init__.py | 4 +- .../HRI/HRI/states/clearSeatingDetections.py | 1 + tasks/HRI/HRI/states/greet.py | 201 +- tasks/HRI/HRI/states/hri_learn_faces.py | 25 +- tasks/HRI/HRI/states/introduce.py | 138 +- tasks/HRI/HRI/states/learn_host_face.py | 68 - .../HRI/HRI/states/locate_and_follow_host.py | 65 + tasks/HRI/HRI/states/place_bag.py | 418 +++ tasks/HRI/HRI/states/recognise.py | 21 +- tasks/HRI/HRI/states/seat_guest.py | 53 +- tasks/HRI/config/lab.yaml | 30 +- tasks/HRI/config/place_bag_debug.rviz | 799 +++++ tasks/HRI/setup.py | 1 + 50 files changed, 6861 insertions(+), 1115 deletions(-) delete mode 100644 common/vision/lasr_vision_reid/lasr_vision_reid/relay_2d.py create mode 100644 log.txt create mode 100644 skills/config/follow_debug.rviz create mode 100644 skills/launch/follow_person.launch.py create mode 100644 skills/src/lasr_skills/continuous_go_to_location.py create mode 100644 skills/src/lasr_skills/detect_keypoints_3d.py create mode 100644 skills/src/lasr_skills/follow_person.py create mode 100644 skills/src/lasr_skills/go_to_location_with_play_motion.py delete mode 100755 skills/src/lasr_skills/handover_object.py create mode 100644 skills/src/lasr_skills/rotate.py rename tasks/HRI/HRI/states/start_door_sm.py => skills/src/lasr_skills/start_task.py (73%) delete mode 100644 tasks/HRI/HRI/states/learn_host_face.py create mode 100644 tasks/HRI/HRI/states/locate_and_follow_host.py create mode 100644 tasks/HRI/HRI/states/place_bag.py create mode 100644 tasks/HRI/config/place_bag_debug.rviz diff --git a/common/language/lasr_llm/lasr_llm/llm_inference.py b/common/language/lasr_llm/lasr_llm/llm_inference.py index 5d1e5ed34..35c616f7b 100644 --- a/common/language/lasr_llm/lasr_llm/llm_inference.py +++ b/common/language/lasr_llm/lasr_llm/llm_inference.py @@ -141,11 +141,22 @@ def _ensure_model_available(self): self.logger.info( f"[LLMInference] Pulling '{self.model_name}' (this only happens once)..." ) - self.client.pull(self.model_name) + 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: @@ -207,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, @@ -224,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}." @@ -242,7 +253,7 @@ 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." @@ -260,7 +271,7 @@ def link_category(object: str, categories: 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") categories_str = ", ".join(categories) query = create_query( f"Detect which category {object} belongs to the most from the following categories: {categories_str}. If not appropriate category to go return 'new'", @@ -277,7 +288,7 @@ 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"] @@ -297,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." 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 af897cdfd..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,9 +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") 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_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 ce9d0a870..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,16 +70,17 @@ 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, @@ -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,37 +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 + feedback_msg = EyeTrackerAction.Feedback() + feedback_msg.running = False - if goal.cancel: - self.get_logger().info("Cancelling eye tracker") - self._done = True - goal_handle.succeed() - # self.destroy_node() - return EyeTrackerAction.Result() - - if self._robot_point is None: - self.get_logger().warn( - "No /robot_pose received yet; continuing and waiting asynchronously." - ) + 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: @@ -239,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), @@ -251,95 +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: @@ -350,34 +368,36 @@ 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() return EyeTrackerAction.Result() diff --git a/common/vision/lasr_vision_interfaces/action/EyeTracker.action b/common/vision/lasr_vision_interfaces/action/EyeTracker.action index 3fa40fa2f..885fdfdca 100644 --- a/common/vision/lasr_vision_interfaces/action/EyeTracker.action +++ b/common/vision/lasr_vision_interfaces/action/EyeTracker.action @@ -1,8 +1,8 @@ # goal geometry_msgs/Point person_point -# cancel -bool cancel --- # 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_reid/lasr_vision_reid/add_face.py b/common/vision/lasr_vision_reid/lasr_vision_reid/add_face.py index 9a7437e0e..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 @@ -84,6 +84,7 @@ def main(): # camera = node.declare_parameter("~camera", "head_front_camera").value name = node.declare_parameter("~name", "guest1").value # originally jared num_images = node.declare_parameter("~num_images", 10).value + # image_topic = "/image_raw" node.get_logger().info(f"Image topic: {image_topic}") 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 55c647e38..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!") 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 e24e21df2..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 ) @@ -80,76 +82,7 @@ def _extract_embeddings(self, im: np.ndarray) -> List[np.ndarray]: 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=False, - 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,9 +116,10 @@ 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) @@ -259,22 +192,17 @@ def _recognise_3d( 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( @@ -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: @@ -398,6 +304,10 @@ def _publish_results_3d( 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 ab404c897..69b66150d 100644 --- a/common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py +++ b/common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py @@ -100,7 +100,9 @@ def __init__(self, node: Node): ) self._device = self.node.get_parameter("~device").value - self.node.declare_parameter("~preload", ["yolo11n-seg.pt"]) + self.node.declare_parameter( + "~preload", ["yolo11n-seg.pt", "yolo11n.pt", "yolo11n-pose.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() @@ -517,17 +515,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/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 2987048c0..cdb377ae2 100755 --- a/skills/setup.py +++ b/skills/setup.py @@ -62,6 +62,9 @@ def run(self): "go_to_location = lasr_skills.go_to_location: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/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 a99127198..96b96eeb4 100755 --- a/skills/src/lasr_skills/describe_people.py +++ b/skills/src/lasr_skills/describe_people.py @@ -50,6 +50,7 @@ def _handle_resp(self, blackboard, response): "hair_color": response.hair_color, "hair_length": response.hair_length, "glasses": response.glasses, + "hat": response.hat, "shirt_color": response.shirt_color, } diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py index db873abe0..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): @@ -79,12 +81,9 @@ def execute(self, blackboard): ) for detection in detected_objects: - if ( - detection.point.x == "nan" + if math.isnan( + detection.point.x ): # CHECK: Potential broken? float vs string? - yasmin.YASMIN_LOG_WARN( - "NAN detection check work" - ) # Remove line if works continue yasmin.YASMIN_LOG_INFO( f"Detected a {detection.name} at x:{detection.point.x}, y:{detection.point.y}, z:{detection.point.z}" diff --git a/skills/src/lasr_skills/detect_all_in_polygon.py b/skills/src/lasr_skills/detect_all_in_polygon.py index 575fd95c8..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") @@ -157,21 +161,18 @@ def __init__( depth=10, ) - self.node = yasmin_ros.logger_node - self.msg = None - self.node.create_subscription( + info_sub = message_filters.Subscriber( + self.node, CameraInfo, "/head_front_camera/depth/camera_info", - self.info_cb, 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) - def info_cb(self, msg): - self.msg = msg - def _get_camera_fov_polygon(self) -> ShapelyPolygon: """ Projects the camera's FOV to the ground plane using intrinsics and TF. @@ -180,14 +181,7 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon: ShapelyPolygon: Footprint of camera FOV in map frame. """ - attempt = 0 - while self.msg is None: - if attempt < 5: - sleep(0.5) - attempt += 0.5 - else: - yasmin.YASMIN_LOG_INFO("No camera info received, ending state") - self.cancel_state() + self.msg = self.cache.getLast() model = PinholeCameraModel() model.fromCameraInfo(self.msg) @@ -200,10 +194,6 @@ 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: @@ -215,8 +205,6 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon: 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( @@ -383,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 @@ -519,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") 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 45123f5c4..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,28 +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_type=EyeTrackerAction, - create_goal_handler=self._create_goal, - response_timeout=1.0, - maximum_retry=0, + outcomes=["succeeded", "failed"], + callback=self.cancel_goal, ) - def _create_goal(self, blackboard): - return EyeTrackerAction.Goal(cancel=True) + 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_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 280f9d7a9..ce0b7fd24 100755 --- a/skills/src/lasr_skills/receive_object.py +++ b/skills/src/lasr_skills/receive_object.py @@ -1,6 +1,12 @@ #!/usr/bin/env python3 import rclpy +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 @@ -25,7 +31,6 @@ def _create_request(self, blackboard): return Empty.Request() -# TODO: Do we need to detect object or just assume that the second guest is holding a bag. class ReceiveObject(StateMachine): def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): @@ -36,62 +41,12 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): self.add_state( "CLEAR_OCTOMAP", ClearOctomap(), - transitions={"succeeded": "LOOK_LEFT", "aborted": "failed"}, - ) - - self.add_state( - "LOOK_LEFT", - PlayMotion(motion_name="look_left"), - transitions={ - "succeeded": "LOOK_DOWN_LEFT", - "aborted": "failed", - "canceled": "failed", - }, - ) - - self.add_state( - "LOOK_DOWN_LEFT", - PlayMotion(motion_name="look_down_left"), - transitions={ - "succeeded": "LOOK_RIGHT", - "aborted": "failed", - "canceled": "failed", - }, - ) - - self.add_state( - "LOOK_RIGHT", - PlayMotion(motion_name="look_right"), - transitions={ - "succeeded": "LOOK_DOWN_RIGHT", - "aborted": "failed", - "canceled": "failed", - }, - ) - - self.add_state( - "LOOK_DOWN_RIGHT", - PlayMotion(motion_name="look_down_right"), - transitions={ - "succeeded": "LOOK_DOWN_CENTRE", - "aborted": "failed", - "canceled": "failed", - }, + transitions={"succeeded": "LOOK_AROUND", "aborted": "failed"}, ) self.add_state( - "LOOK_DOWN_CENTRE", - PlayMotion(motion_name="look_centre"), - transitions={ - "succeeded": "LOOK_CENTRE", - "aborted": "failed", - "canceled": "failed", - }, - ) - - self.add_state( - "LOOK_CENTRE", - PlayMotion(motion_name="look_centre"), + "LOOK_AROUND", + PlayMotion(motion_name="head_tour"), transitions={ "succeeded": "SAY_REACH_ARM", "aborted": "failed", @@ -101,7 +56,9 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): self.add_state( "SAY_REACH_ARM", - Say(text="Please step back, I am going to reach my arm out."), + 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", @@ -139,12 +96,11 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): "canceled": "failed", }, ) - if object_name is not None: self.add_state( "SAY_PLACE", Say( - text=f"Please place the {object_name} in my hand. I will wait for a few seconds.", + text=f"I am ready to recieve the {object_name} in my hand. I will wait for a few seconds.", ), transitions={ "succeeded": "WAIT_5", @@ -156,7 +112,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): self.add_state( "SAY_PLACE", Say( - format_str="Please place the {} in my hand. I will wait for a few seconds.", + format_str="I am ready to recieve the {} in my hand. I will wait for a few seconds.", ), transitions={ "succeeded": "WAIT_5", @@ -165,7 +121,6 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): }, remapping={"placeholders": "object_name"}, ) - self.add_state( "WAIT_5", Wait(5), @@ -179,7 +134,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): # 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 + # 3. /gripper_controller/ action server - NOT AVAILABLE | use lasr_manipulation # self.add_state( # "CLOSE_GRIPPER", @@ -191,7 +146,7 @@ def __init__(self, object_name: Union[str, None] = None, vertical: bool = True): # }, # ) self.add_state( - "CLOSE_HALF_GRIPPER", # TEMPORARY REPLACEMENT - using half to not jam an item between gripper + "CLOSE_HALF_GRIPPER", # TEMPORARY REPLACEMENT PlayMotion(motion_name="close_half"), transitions={ "succeeded": "FOLD_ARM", @@ -234,3 +189,4 @@ def main(): 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/tasks/HRI/HRI/states/start_door_sm.py b/skills/src/lasr_skills/start_task.py similarity index 73% rename from tasks/HRI/HRI/states/start_door_sm.py rename to skills/src/lasr_skills/start_task.py index 316705de1..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, PlayMotion +from lasr_skills import DetectDoorOpening, SafeGoToLocation, PlayMotion class StartDoorSM(StateMachine): # TODO: Rename to start_task and move to Skills @@ -25,23 +25,13 @@ def __init__( self.add_state( "DETECT_DOOR_OPENING", DetectDoorOpening(), - transitions={"door_opened": "PRE_NAV", "failed": "failed"}, - ) - - self.add_state( - "PRE_NAV", - PlayMotion("pre_navigation"), - transitions={ - "succeeded": "GO_TO_START", - "aborted": "failed", - "canceled": "failed", - }, + 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/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 47797b28a..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,9 +1,11 @@ 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 @@ -14,43 +16,61 @@ def __init__(self): 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): + 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/HRI/HRI/state_machine.py b/tasks/HRI/HRI/state_machine.py index 287051866..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, StopEyeTracker, PlayMotion +from lasr_skills import Say, SafeGoToLocation, StartDoorSM, Rotate, FollowPerson from HRI.states import * @@ -62,125 +60,99 @@ def wait_cb(blackboard, msg): self.add_state( "GO_TO_DOOR", - GoToLocation(location_param="door_pose"), - transitions={"succeeded": "POST_NAV", "failed": "failed"}, + SafeGoToLocation(location_param="door_pose"), + transitions={"succeeded": "GREET", "failed": "failed"}, ) self.add_state( - "POST_NAV", - PlayMotion("post_navigation"), - transitions={ - "succeeded": "GREET", - "aborted": "failed", - "canceled": "failed", - }, + "GREET", # SM2: Greets guest + LookAndGreetGuest(guest_id="guest1"), + transitions={"succeeded": "GUIDE_TO_SEAT", "failed": "failed"}, ) self.add_state( - "GREET", # SM2: Greets guest - LookAndGreetGuest(last_resort=False, guest_id="guest1"), - transitions={"succeeded": "STOP_EYE_TRACKER", "failed": "failed"}, + "GUIDE_TO_SEAT", # GUIDES GUEST TO SEATING AREA + SafeGoToLocation(location_param="seat_pose"), + transitions={"succeeded": "SEAT_GUEST", "failed": "failed"}, ) self.add_state( - "STOP_EYE_TRACKER", - StopEyeTracker(), - transitions={ - "succeeded": "LOOK_CENTRE", - "aborted": "failed", - "canceled": "failed", - "timeout": "failed", - }, + "SEAT_GUEST", # SM3: Locates and seats guest in free seat + SeatGuest(guest_id="guest1"), + transitions={"succeeded": "CHECK", "failed": "failed"}, ) self.add_state( - "LOOK_CENTRE", - PlayMotion("look_centre"), - transitions={ - "succeeded": "SAY_FOLLOW", - "aborted": "failed", - "canceled": "failed", - }, + "CHECK", + yasmin.CbState(outcomes=["succeeded", "continue"], callback=self.check), + transitions={"succeeded": "INTRODUCE", "continue": "GO_TO_DOOR_2"}, ) self.add_state( - "SAY_FOLLOW", - Say(format_str="Welcome {}. Follow me to the seating area."), - transitions={ - "succeeded": "PRE_NAV_2", - "aborted": "failed", - "canceled": "failed", - }, + "GO_TO_DOOR_2", + SafeGoToLocation(location_param="door_pose"), + transitions={"succeeded": "GREET_2", "failed": "failed"}, ) self.add_state( - "PRE_NAV_2", - PlayMotion("pre_navigation"), - transitions={ - "succeeded": "GUIDE_TO_SEAT", - "aborted": "failed", - "canceled": "failed", - }, + "GREET_2", # SM2: Greets guest + LookAndGreetGuest(guest_id="guest2"), + transitions={"succeeded": "GUIDE_TO_SEAT_2", "failed": "failed"}, ) self.add_state( - "GUIDE_TO_SEAT", # GUIDES GUEST TO SEATING AREA - GoToLocation(location_param="seat_pose"), - transitions={"succeeded": "POST_NAV_2", "failed": "failed"}, + "GUIDE_TO_SEAT_2", # GUIDES GUEST TO SEATING AREA + SafeGoToLocation(location_param="seat_pose"), + transitions={"succeeded": "SEAT_GUEST_2", "failed": "failed"}, ) self.add_state( - "POST_NAV_2", - PlayMotion("post_navigation"), - transitions={ - "succeeded": "SEAT_GUEST", - "aborted": "failed", - "canceled": "failed", - }, + "SEAT_GUEST_2", # SM3: Locates and seats guest in free seat + SeatGuest(guest_id="guest2"), + transitions={"succeeded": "CHECK", "failed": "failed"}, ) self.add_state( - "SEAT_GUEST", # SM3: Locates and seats guest in free seat - SeatGuest(learn_host=False), - transitions={"succeeded": "CHECK", "failed": "failed"}, + "INTRODUCE", + Introduce(), + transitions={"succeeded": "ROTATE", "failed": "ROTATE"}, ) self.add_state( - "CHECK", - yasmin.CbState(outcomes=["succeeded", "GO_TO_DOOR_2"], callback=self.check), - transitions={"succeeded": "INTRODUCE", "GO_TO_DOOR_2": "PRE_NAV_3"}, + "ROTATE", + Rotate(angle=180), + transitions={"succeeded": "FOLLOW_HOST", "failed": "failed"}, ) self.add_state( - "PRE_NAV_3", - PlayMotion("pre_navigation"), + "FOLLOW_HOST", + FollowPerson(), transitions={ - "succeeded": "GO_TO_DOOR_2", - "aborted": "failed", - "canceled": "failed", + "succeeded": "PLACE_BAG", + "failed": "failed", }, ) self.add_state( - "GO_TO_DOOR_2", - GoToLocation(location_param="door_pose"), - transitions={"succeeded": "POST_NAV_3", "failed": "failed"}, + "PLACE_BAG", + PlaceBag(), + transitions={ + "succeeded": "succeeded", + "failed": "failed", + }, ) + self.add_state("STOP_TIMER", StopTimer(), transitions={"succeeded": "SAY_STOP"}) + self.add_state( - "POST_NAV_3", - PlayMotion("post_navigation"), + "SAY_STOP", + Say(), transitions={ - "succeeded": "GREET_2", + "succeeded": "succeeded", "aborted": "failed", "canceled": "failed", }, - ) - - self.add_state( - "GREET_2", # SM2: Greets guest - LookAndGreetGuest(last_resort=False, guest_id="guest2"), - transitions={"succeeded": "STOP_EYE_TRACKER", "failed": "failed"}, + remappings={"text": "time_text"}, ) self.add_state( @@ -190,15 +162,27 @@ def wait_cb(blackboard, msg): ) def check(self, blackboard): - guest = blackboard["guest_data"][f"guest{self.guest_id}"] + guest1 = blackboard["guest_data"]["guest1"] yasmin.YASMIN_LOG_INFO(f"{self.guest_id}") - for key in guest.keys(): - value = guest[key] - yasmin.YASMIN_LOG_INFO(f"{key}: {value}") + 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 "GO_TO_DOOR_2" if self.guest_id == 2 else "succeeded" + return "continue" if self.guest_id == 2 else "succeeded" def setup(self): start_con_sm = yasmin.Concurrence( @@ -249,6 +233,7 @@ def main(): face_detection_confidence = 0.2 bb["guest_data"] = { + "host": {"seated_point": None, "seating_detection": False}, "guest1": { "name": "", "drink": "", diff --git a/tasks/HRI/HRI/states/__init__.py b/tasks/HRI/HRI/states/__init__.py index 299ea33eb..3cdc29b8b 100644 --- a/tasks/HRI/HRI/states/__init__.py +++ b/tasks/HRI/HRI/states/__init__.py @@ -1,11 +1,9 @@ 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 @@ -16,3 +14,5 @@ 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 index 4261014fb..eb466fa9c 100644 --- a/tasks/HRI/HRI/states/clearSeatingDetections.py +++ b/tasks/HRI/HRI/states/clearSeatingDetections.py @@ -14,6 +14,7 @@ def __init__(self): 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/greet.py b/tasks/HRI/HRI/states/greet.py index 6ae0f686f..eddbb082a 100644 --- a/tasks/HRI/HRI/states/greet.py +++ b/tasks/HRI/HRI/states/greet.py @@ -7,6 +7,7 @@ AskAndListen, ReceiveObject, StopEyeTracker, + Wait, ) from HRI.states import ( GetNameAndDrink, @@ -15,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"]) 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={ @@ -93,42 +148,6 @@ def __init__(self, last_resort, guest_id): conc_name_drink_face.add_input_key("guest_data") conc_name_drink_face.add_output_key("guest_data") - 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(), - transitions={ - "succeeded": "GET_PERSON_POINT", - "failed": "SAY_WAITING_FOR_GUEST", - }, - remappings={"detections_3d": "person_detections"}, - ) - self.add_state( - "GET_PERSON_POINT", - GetPersonPoint(), - transitions={ - "succeeded": "START_EYE_TRACKER", - "failed": "SAY_WAITING_FOR_GUEST", - }, - ) - self.add_state( - "START_EYE_TRACKER", - StartEyeTracker(), - transitions={ - "succeeded": "GREET_AND_ASK_GUEST", - "aborted": "SAY_WAITING_FOR_GUEST", - "canceled": "failed", - "timeout": "GREET_AND_ASK_GUEST", - }, - ) self.add_state( "GREET_AND_ASK_GUEST", AskAndListen( @@ -141,7 +160,7 @@ def __init__(self, last_resort, guest_id): remappings={"transcribed_speech": "guest_transcription"}, ) - transition = "SAY_BAG" if guest_id == "guest2" else "succeeded" + transition = "GET_ATTRIBUTE_STR" if guest_id == "guest2" else "SAY_WELCOME" self.add_state( "GET_NAME_DRINK_FACE", @@ -156,28 +175,100 @@ def __init__(self, last_resort, guest_id): ) self.add_state( - "SAY_BAG", - Say(text="I see you have a bag for me."), + "SAY_WELCOME", + Say(format_str="Welcome to the party {}. Please follow me to be seated."), transitions={ - "succeeded": "STOP_EYE_TRACKING", - "aborted": "STOP_EYE_TRACKING", - "canceled": "STOP_EYE_TRACKING", + "succeeded": "STOP_EYE_TRACKING_1", + "aborted": "failed", + "canceled": "failed", }, ) self.add_state( - "STOP_EYE_TRACKING", + "STOP_EYE_TRACKING_1", StopEyeTracker(), transitions={ - "succeeded": "GRAB_BAG", + "succeeded": "succeeded", + "failed": "failed", + }, + ) + + self.add_state( + "GET_ATTRIBUTE_STR", + attribute, + transitions={"succeeded": "SAY_ATTRIBUTE", "failed": "failed"}, + ) + + self.add_state( + "SAY_ATTRIBUTE", + Say(), + transitions={ + "succeeded": "STOP_EYE_TRACKING_2", "aborted": "failed", "canceled": "failed", - "timeout": "failed", }, ) + self.add_state( + "STOP_EYE_TRACKING_2", + StopEyeTracker(), + transitions={ + "succeeded": "WAIT", + "failed": "failed", + }, + ) + + self.add_state( + "WAIT", Wait(2), transitions={"succeeded": "GRAB_BAG", "failed": "failed"} + ) + self.add_state( "GRAB_BAG", ReceiveObject(object_name="bag"), - transitions={"succeeded": "succeeded", "failed": "failed"}, + transitions={"succeeded": "SAY_WELCOME_2", "failed": "failed"}, + ) + + self.add_state( + "SAY_WELCOME_2", + Say(text="Please follow me to be seated."), + transitions={ + "succeeded": "succeeded", + "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 5183ddfaf..87d70db01 100644 --- a/tasks/HRI/HRI/states/hri_learn_faces.py +++ b/tasks/HRI/HRI/states/hri_learn_faces.py @@ -104,19 +104,26 @@ 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"]) diff --git a/tasks/HRI/HRI/states/introduce.py b/tasks/HRI/HRI/states/introduce.py index 75f3580c7..b0bf52663 100644 --- a/tasks/HRI/HRI/states/introduce.py +++ b/tasks/HRI/HRI/states/introduce.py @@ -66,7 +66,7 @@ def __init__(self): callback=self._loop_person_index, ) loop_state.add_input_key("person_index") - loop_state.add_input_key("people_detected") + 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") @@ -77,25 +77,33 @@ def __init__(self): guest_loop.add_input_key("guest_data") guest_loop.add_output_key("guest_data") - self.add_state( - "RESET_SEATING_DETECTIONS", - ClearSeatingDetections(), - transitions={"succeeded": "FIND_PEOPLE", "failed": "failed"}, + 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( - "FIND_PEOPLE", - DetectAllInPolygon( - polygon=self.seating_area, - object_filter=["person"], - min_coverage=1.0, - min_new_object_dist=0.50, - min_confidence=0.5, - ), + "RESET_SEATING_DETECTIONS", + ClearSeatingDetections(), transitions={"succeeded": "LOOP_PERSON_STATE", "failed": "failed"}, - remappings={"detected_objects": "people_detected"}, ) + # 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, @@ -110,14 +118,18 @@ def __init__(self): "LOOK_AT_PERSON", LookToPoint(), transitions={ - "succeeded": "RECOGNISE", - "aborted": "RECOGNISE", + "succeeded": "WAIT", + "aborted": "WAIT", "canceled": "failed", - "timeout": "RECOGNISE", + "timeout": "WAIT", }, remappings={"pointstamped": "person_point_stamped"}, ) + self.add_state( + "WAIT", Wait(2), transitions={"succeeded": "RECOGNISE", "failed": "failed"} + ) + self.add_state( "RECOGNISE", Recognise(), @@ -141,7 +153,7 @@ def __init__(self): self.add_state( "GRAB_GUEST_POINT", guest_loop, - transitions={"succeeded": "succeeded", "continue": "GET_INTRODUCTION_STR"}, + transitions={"succeeded": "GET_HOST", "continue": "GET_INTRODUCTION_STR"}, ) self.add_state( @@ -182,28 +194,102 @@ def __init__(self): }, ) + 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"] - people_detected = len(blackboard["people_detected"]) + 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(str(guest1point)) - yasmin.YASMIN_LOG_INFO(str(guest2point)) - yasmin.YASMIN_LOG_INFO(str(people_detected)) + 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: + if guest1point is not None and guest2point is not None and host is not None: return "succeeded" - elif index < people_detected: - point = blackboard["people_detected"][index].point + 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"] @@ -222,7 +308,7 @@ def _loop_guest(self, blackboard): header=Header(frame_id="map"), point=point ) yasmin.YASMIN_LOG_INFO(id) - yasmin.yasmin_LOG_INFO(str(point)) + 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"] = ( 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 index 43c048d5a..994f43f69 100644 --- a/tasks/HRI/HRI/states/recognise.py +++ b/tasks/HRI/HRI/states/recognise.py @@ -17,13 +17,14 @@ 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/threed", + srv_name="/lasr_vision_reid/recognise", create_request_handler=self._create_request, response_handler=self._handle_resp, outcomes=["no_detections"], @@ -31,6 +32,8 @@ def __init__(self): 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, @@ -80,15 +83,18 @@ def _create_request(self, blackboard): 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.5 + 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: @@ -100,9 +106,10 @@ def _handle_resp(self, blackboard, response): blackboard["guest_data"][detection.name][ "seated_point" ] = detection.point - return "succeeded" + blackboard["seat_indexes"][detection.name] = blackboard["person_index"] + detected = True - return "aborted" + return "aborted" if not detected else "succeeded" def check(blackboard): @@ -121,6 +128,12 @@ def main(): 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(), diff --git a/tasks/HRI/HRI/states/seat_guest.py b/tasks/HRI/HRI/states/seat_guest.py index 98f313954..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, @@ -31,6 +30,8 @@ StopEyeTracker, ) +from HRI.states import HRILearnFaces + from yasmin_viewer import YasminViewerPub @@ -78,12 +79,14 @@ def execute(self, blackboard): 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): @@ -97,6 +100,17 @@ def execute(self, blackboard): ): non_sofa_chairs.update({detection_point: False}) + yasmin.YASMIN_LOG_INFO( + "Detected this many people in sweep: " + str(len(people)) + ) + + if len(people) == 1: + blackboard["pointstamped"] = PointStamped( + header=Header(frame_id="map"), point=people[0].point + ) + else: + 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: @@ -116,7 +130,7 @@ def execute(self, blackboard): 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 take sit down in the seat I am looking at." + "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"), @@ -149,7 +163,7 @@ class SeatGuest(StateMachine): def __init__( self, - learn_host: bool = False, + guest_id: str, ): super().__init__(outcomes=["succeeded", "failed"]) self.add_input_key("guest_data") @@ -183,7 +197,7 @@ def __init__( DetectAllInPolygon( 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, ), @@ -191,6 +205,8 @@ def __init__( remappings={"detected_objects": "seat_detections"}, ) + transition = "LOOK_HOST" if guest_id == "guest1" else "LOOK_TO_SEAT" + self.add_state( "PROCESS_DETECTIONS", ProcessDetections( @@ -199,6 +215,33 @@ def __init__( left_sofa_area=self.left_sofa_area, right_sofa_area=self.right_sofa_area, ), + transitions={"succeeded": transition, "failed": "failed"}, + ) + + self.add_state( + "LOOK_HOST", + LookToPoint(), + transitions={ + "succeeded": "SAY_HOST", + "aborted": "SAY_HOST", + "canceled": "SAY_HOST", + "timeout": "SAY_HOST", + }, + ) + + 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"}, ) @@ -326,14 +369,12 @@ def __init__(self): 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() diff --git a/tasks/HRI/config/lab.yaml b/tasks/HRI/config/lab.yaml index ba01852cc..5236db1b1 100644 --- a/tasks/HRI/config/lab.yaml +++ b/tasks/HRI/config/lab.yaml @@ -33,35 +33,37 @@ hri: # the `hri` Node's Parameters # Where to position self for seating guests seat_pose: position: - x: 0.8439305474571136 - y: -0.7982808179074764 + x: 0.9241805428867255 + y: -0.37066992922907555 z: 0.0 orientation: x: 0.0 y: 0.0 - z: -0.7507265515887908 - w: 0.6606130824768782 + z: -0.798049181701085 + w: 0.6025923195546294 + + # Where the robot looks at the general sofa sofa_point: - x: 0.40349432826042175 - y: -2.7426514625549316 + 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: [0.941597044467926, -3.303898811340332] - top_right: [-0.664279580116272, -2.5707762241363525] - bottom_right: [-0.34006959199905396, -1.5264309644699097] - bottom_left: [1.3503732681274414, -1.9663374423980713] + 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: [0.8116632699966431, -3.0669937133789062] - top_right: [-0.058406829833984375, -2.6968655586242676] - bottom_right: [0.03105384111404419, -2.327255964279175] - bottom_left: [0.9847490191459656, -2.6538729667663574] + 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/setup.py b/tasks/HRI/setup.py index 449a54e32..74b688b48 100644 --- a/tasks/HRI/setup.py +++ b/tasks/HRI/setup.py @@ -52,6 +52,7 @@ def run(self): "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", ], }, ) From 1b67a4349d82b555e1218e79ed36ada8664a39a8 Mon Sep 17 00:00:00 2001 From: Hayeong Lee Date: Fri, 26 Jun 2026 18:49:11 +0100 Subject: [PATCH 16/21] Vlm detection fix --- .../config/params.yaml | 2 +- .../lasr_vision_open_vocabulary/node.py | 7 +- skills/src/lasr_skills/go_to_location.py | 2 +- skills/src/lasr_skills/say.py | 2 +- tasks/pick_and_place/config/config.yaml | 41 ++++++-- .../launch/pick_and_place.launch.py | 38 +++---- .../pick_and_place/state_machine.py | 98 +++++++++++-------- .../pick_and_place/states/__init__.py | 1 + .../pick_and_place/states/detect_objects.py | 5 +- .../pick_and_place/states/serve_breakfast.py | 12 +-- .../pick_and_place/test_serve_breakfast.py | 37 +++---- .../pick_and_place/vlm_classifier.py | 11 ++- 12 files changed, 153 insertions(+), 103 deletions(-) diff --git a/common/vision/lasr_vision_open_vocabulary/config/params.yaml b/common/vision/lasr_vision_open_vocabulary/config/params.yaml index 5327ad088..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/robocup/Yara_ws/src/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 ae1e38560..b28295138 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,6 +14,9 @@ VitSam, ) +CLIP_CANDIDATES = [ + "pringles", "iced tea", "apple", "milk", "can", "coke", "cup", "sprite", "water bottle", "banana", "bowl", "cereal", +] class OpenVocabNode(Node): def __init__(self): @@ -39,8 +42,8 @@ def __init__(self): # CLIP recognition rerank: detector localises (boxes), CLIP re-labels each # crop against a candidate list. OFF by default - enabled per-task via # params (e.g. pick_and_place passes clip_rerank:=true + clip_candidates). - self.declare_parameter("clip_rerank", False) - self.declare_parameter("clip_candidates", [""]) + self.declare_parameter("clip_rerank", True) + self.declare_parameter("clip_candidates", 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) diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py index db4e75c09..c665720e2 100755 --- a/skills/src/lasr_skills/go_to_location.py +++ b/skills/src/lasr_skills/go_to_location.py @@ -112,4 +112,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/skills/src/lasr_skills/say.py b/skills/src/lasr_skills/say.py index b0f927b43..ee70941e4 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 = False +HAS_TTS_MSGS: bool = True try: diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index 87066e392..b0dd5399f 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -4,24 +4,46 @@ # Category the referee designates as trash (announced on Setup Days). # Objects of this category are routed to the trash bin. "" disables it. - trash_category: "snack" + trash_category: "trash" # Open-vocab query words for table detection (common nouns). - objects: ["cup", "can", "bottle", "bowl", "box", "apple"] + objects: ["cup", "can", "bottle", "dish", "box", "fruit", "utensil", "snack", "carton", "food" ] table: pose: - position: {x: 0.5154840308921795, y: -3.297018781464196, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.7230045994832588, w: 0.6908432160237608} - look_point: [5.25, 2.27, 0.75] + position: {x: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 6114615932519716} + observe_pose: + position: {x: 0.5876252953308133, y: -1.0687492206833655, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.996990359390238, w: 0.07752562984538826} + look_point: [5.25, 2.27, 0.78] 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] z_min: 0.7 z_max: 1.5 + collision: + detect: true + frame_id: map + size: [1.02, 1.2, 0.9] + position: [1.2, -3.9, 0.37] + + # ── DESTINATION 1: dishwasher (tableware + cutlery) ── + dishwasher: + pose: + position: {x: 0.8212924899016142, y: 0.299479767042867, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.7527398351858691, w: 0.6583181149902764} + + # ── DESTINATION 2: trash bin ── + trash_bin: + pose: + position: {x: 0.8212924899016142, y: 0.299479767042867, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: -0.7527398351858691, w: 0.6583181149902764} + + # ── DESTINATION 3: cabinet (fake: boxes with objects) ── cabinet: pose: - position: {x: -0.13809676070081464, y: -0.30871316659971554, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: 0.9938913879096476, w: 0.11036262519093311} + position: {x: -0.24395824472217295, y: -1.7992161774429396, z: 0.0} # <-- ВСТАВ pose + orientation: {x: 0.0, y: 0.0, z: 0.9801022379942061, w: 0.13519825869117716} # Shelves — used by ScanShelves in the NEXT deliverable. # shelf_order is the iteration list; shelves. holds per-shelf config. @@ -46,6 +68,7 @@ polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] z_min: 0.3 z_max: 0.6 + grasp: enable: false reach: 0.80 @@ -55,5 +78,5 @@ # ── BREAKFAST SURFACE (bowl and spoon pickup location) ── breakfast_surface: pose: - position: {x: -0.5679924129320376, y: -2.8240229666620253, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.9940136153079541, w: 0.10925627022011415} \ No newline at end of file + position: {x: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 0.6114615932519716} \ 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 d1cf83ffb..431713282 100644 --- a/tasks/pick_and_place/launch/pick_and_place.launch.py +++ b/tasks/pick_and_place/launch/pick_and_place.launch.py @@ -12,7 +12,7 @@ # then CLIP re-labels each crop against THIS list (fixes "Pringles -> cup"). Names # must be CATEGORY_MAP-friendly so routing works (see classify_category.py). CLIP_CANDIDATES = [ - "pringles", "iced tea", "apple", "bottle", "can", "coke", "cup", "sprite", "water bottle", "banana", + "pringles", "iced tea", "apple", "milk", "can", "coke", "cup", "sprite","bowl", "spoon", "water bottle", "banana", "cereal" ] def generate_launch_description(): @@ -48,16 +48,16 @@ def generate_launch_description(): ), # ── Perception: open-vocab detection + CLIP recognition rerank ──────── - Node( - package="lasr_vision_open_vocabulary", - executable="open_vocabulary_node", - name="lasr_vision_open_vocabulary", - output="screen", - parameters=[ - ov_params, - {"clip_rerank": False, "clip_candidates": CLIP_CANDIDATES}, - ], - ), + # Node( + # package="lasr_vision_open_vocabulary", + # executable="open_vocabulary_node", + # name="lasr_vision_open_vocabulary", + # output="screen", + # parameters=[ + # ov_params, + # {"clip_rerank": True, "clip_candidates": CLIP_CANDIDATES}, + # ], + # ), Node( package="lasr_vision_open_vocabulary", executable="detection_visualizer", @@ -66,14 +66,14 @@ def generate_launch_description(): ), # ── Optional: LLM category-fallback service (CPU-forced) ───────────── - Node( - condition=IfCondition(use_llm), - package="lasr_llm", - executable="storing_groceries_service", - name="storing_groceries_query_llm_service", - output="screen", - additional_env={"CUDA_VISIBLE_DEVICES": ""}, - ), + # Node( + # condition=IfCondition(use_llm), + # package="lasr_llm", + # executable="storing_groceries_service", + # name="storing_groceries_query_llm_service", + # output="screen", + # additional_env={"CUDA_VISIBLE_DEVICES": ""}, + # ), # ── Task: state machine ────────────────────────────────────────────── Node( 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 771fe2323..2597fcec3 100644 --- a/tasks/pick_and_place/pick_and_place/state_machine.py +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -19,8 +19,9 @@ InstructPick, InstructPlace, AddTableCollision, - GraspObject, - ApproachTable + # GraspObject, + ApproachTable, + ServeBreakfast ) from rclpy.executors import MultiThreadedExecutor as Executor @@ -76,18 +77,18 @@ def __init__(self): "ADD_TABLE_COLLISION", AddTableCollision(head_tilt=-0.6), # детект усього столу здалеку transitions={ - "succeeded": "GO_TO_TABLE_FOR_PICK", # було "DETECT_OBJECTS" + "succeeded": "DETECT_OBJECTS", # було "DETECT_OBJECTS" }, ) - self.add_state( - "GO_TO_TABLE_FOR_PICK", - GoToLocation(location_param="pick_and_place.table.pose"), - transitions={ - "succeeded": "DETECT_OBJECTS", - "failed": "DETECT_OBJECTS", # все одно пробуємо детект - }, - ) + # 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( @@ -125,70 +126,72 @@ def __init__(self): "DECIDE_DESTINATION", DecideDestination(), transitions={ - "cabinet": "CHOOSE_SHELF", + "cabinet": "INSTRUCT_PICK", "other": "INSTRUCT_PICK", }, ) # ── Choose which cabinet shelf to place object on ───────────────────── - self.add_state( - "CHOOSE_SHELF", - ChooseShelf(), - transitions={ - "succeeded": "INSTRUCT_PICK", - "failed": "INSTRUCT_PICK", # announce anyway - }, - ) + # 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": "GRASP", + "succeeded": "INSTRUCT_PLACE", "failed": "INSTRUCT_PICK", # retry instruction }, ) - self.add_state( - "GRASP", GraspObject(), - transitions={"succeeded": "GO_TO_DESTINATION", "failed": "GO_TO_DESTINATION"}, - ) + + # self.add_state( + # "GRASP", GraspObject(), + # transitions={"succeeded": "GO_TO_DESTINATION", "failed": "GO_TO_DESTINATION"}, + # ) + # ── Navigate to the chosen destination (pose set by DecideDestination)─ - self.add_state( - "GO_TO_DESTINATION", - GoToLocation(), # reads blackboard["location"] - transitions={ - "succeeded": "INSTRUCT_PLACE", - "failed": "INSTRUCT_PLACE", # announce even if nav failed - }, - ) + # self.add_state( + # "GO_TO_DESTINATION", + # GoToLocation(), # reads blackboard["location"] + # 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_TABLE", + "succeeded": "FINISH", "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": "SELECT_OBJECT", # loop back for next object - "failed": "GO_TO_TABLE", # retry navigation - }, - ) + # self.add_state( + # "GO_TO_TABLE", + # GoToLocation(location_param="pick_and_place.table.pose"), + # transitions={ + # "succeeded": "SELECT_OBJECT", # loop back for next object + # "failed": "GO_TO_TABLE", # retry navigation + # }, + # ) # ── Done ────────────────────────────────────────────────────────────── self.add_state( "FINISH", Say( text="I have sorted all the objects I could see on the table. " - "Pick and place complete." + "I will now set up breakfast." ), transitions={ "succeeded": "succeeded", @@ -197,6 +200,15 @@ def __init__(self): }, ) + # self.add_state( + # "SERVE_BREAKFAST", + # ServeBreakfast(), + # transitions={ + # "succeeded": "succeeded", + # "failed": "succeeded", + # }, + # ) + class PickAndPlaceNode(Node): def __init__(self): super().__init__( 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 c562c2994..6522e4cf6 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -13,3 +13,4 @@ # from .grasp_object import GraspObject from .approach_table import ApproachTable +from .serve_breakfast import ServeBreakfast \ 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 a3c09b8da..693f16b75 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 @@ -60,17 +60,18 @@ class DetectObjects(yasmin.State): # ── VLM naming (Ollama). DINO finds the boxes; the VLM says what each is. # Run clip_rerank:=false in the launch — the VLM replaces CLIP here. - VLM_ENABLE = True + VLM_ENABLE = False VLM_MODEL = "moondream" VLM_HOST = "http://localhost:11434" VLM_TIMEOUT = 60.0 - def __init__(self): + def __init__(self, queries:list = None): super().__init__(outcomes=["succeeded", "failed"]) self.add_output_key("detected_objects") self.node = yasmin_ros.logger_node self.bridge = CvBridge() + if queries is not None: self._queries = queries else: 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 index 2f8b2fb4c..96bfe74f8 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -32,7 +32,7 @@ class ServeBreakfast(yasmin.StateMachine): def __init__(self): super().__init__(outcomes=["succeeded", "failed"], handle_sigint=True) - # # Navigate to breakfast surface + # Navigate to breakfast surface self.add_state( "GO_TO_BREAKFAST_SURFACE", GoToLocation(location_param="pick_and_place.breakfast_surface.pose"), @@ -93,7 +93,7 @@ def __init__(self): "INSTRUCT_PICK_SPOON", InstructPick(), transitions={ - "succeeded": "INSTRUCT_PICK_SPOON", + "succeeded": "GO_TO_TABLE_1", "failed": "INSTRUCT_PICK_SPOON", }, ) @@ -140,7 +140,7 @@ def __init__(self): # Cereal self.add_state( "DETECT_CEREAL", - DetectObjects(queries=["box"]), + DetectObjects(queries=["cereal"]), transitions={ "succeeded": "SELECT_CEREAL", "failed": "DETECT_CEREAL", @@ -148,7 +148,7 @@ def __init__(self): ) self.add_state( "SELECT_CEREAL", - SelectAndVisualiseObject(target_name="box"), + SelectAndVisualiseObject(target_name="cereal"), transitions={ "succeeded": "INSTRUCT_PICK_CEREAL", "finished": "DETECT_CEREAL", # not found, retry detection @@ -166,7 +166,7 @@ def __init__(self): # Milk self.add_state( "DETECT_MILK", - DetectObjects(queries=["bottle"]), + DetectObjects(queries=["milk"]), transitions={ "succeeded": "SELECT_MILK", "failed": "DETECT_MILK", @@ -174,7 +174,7 @@ def __init__(self): ) self.add_state( "SELECT_MILK", - SelectAndVisualiseObject(target_name="bottle"), + SelectAndVisualiseObject(target_name="milk"), transitions={ "succeeded": "INSTRUCT_PICK_MILK", "finished": "DETECT_MILK", # not found, retry detection 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 index fe84748c2..6de1203c6 100644 --- a/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/test_serve_breakfast.py @@ -5,44 +5,47 @@ from pick_and_place.states.serve_breakfast import ServeBreakfast +from pick_and_place.state_machine import PickAndPlaceNode + def main(): rclpy.init() - yasmin_ros.set_ros_loggers() + node = PickAndPlaceNode() # has allow_undeclared_parameters=True + yasmin_ros.set_ros_loggers(node) + bb = yasmin.Blackboard() - bb["detected_objects"] = [] + bb["detected_objects"] = [] - bb["debug_images"] = [] - - bb["selected_object"] = None + bb["selected_object"] = None bb["selected_object_name"] = "" - bb["last_rgb_image"] = None - - bb["object_name"] = "" + bb["object_name"] = "" - # Add these three for InstructPick/InstructPlace + bb["object_category"] = "breakfast" - bb["object_category"] = "breakfast" + bb["destination_str"] = "the dining table" - bb["destination_str"] = "the dining table" + bb["chosen_shelf"] = "" - bb["chosen_shelf"] = "" + bb["chosen_shelf_str"] = "" - 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(): - rclpy.shutdown() - + node.destroy_node() -if __name__ == "__main__": - main() + 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 index bbfb1acad..34755a32c 100644 --- a/tasks/pick_and_place/pick_and_place/vlm_classifier.py +++ b/tasks/pick_and_place/pick_and_place/vlm_classifier.py @@ -9,7 +9,7 @@ # Specific product labels the VLM must choose from — EDIT for your items. CANDIDATES = [ - "iced tea", "water bottle", "coke can", "sprite can", "pringles", + "water bottle", "iced tea", "coke can", "sprite can", "pringles", "red bull", "apple", "banana", "cup", "mug", "bowl", "sponge", "unknown", ] @@ -26,7 +26,7 @@ def classify_crop( box_xywh_center, candidates=None, *, - model="gemma3:4b", + model="moondream", host="http://localhost:11434", timeout=60.0, pad=0.12, @@ -43,6 +43,13 @@ def classify_crop( if x2 <= x1 or y2 <= y1: return None crop = rgb_bgr[y1:y2, x1:x2] + + import os + debug_dir = "/tmp/vlm_crops" + os.makedirs(debug_dir, exist_ok=True) + debug_count = len(os.listdir(debug_dir)) + cv2.imwrite(f"{debug_dir}/crop_{debug_count}.jpg", crop) + ok, buf = cv2.imencode(".jpg", crop) if not ok: return None From 01151fdedd4868fda8a0fe204161a9ae38661672 Mon Sep 17 00:00:00 2001 From: Yara Alkhelaiwi Date: Wed, 1 Jul 2026 10:38:17 +0100 Subject: [PATCH 17/21] Change from vlm to YOLO detection --- tasks/pick_and_place/config/config.yaml | 172 ++++++-- .../states/classify_category.py | 4 +- .../pick_and_place/states/detect_objects.py | 389 ++++++------------ .../states/detect_trash_floor.py | 137 ++---- .../states/extra_surface_cleanup.py | 2 +- .../pick_and_place/states/scan_shelves.py | 19 +- .../pick_and_place/states/serve_breakfast.py | 8 +- .../pick_and_place/states/table_cleanup.py | 18 +- 8 files changed, 334 insertions(+), 415 deletions(-) diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index b0dd5399f..e4c8cab3d 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -2,81 +2,187 @@ ros__parameters: pick_and_place: - # Category the referee designates as trash (announced on Setup Days). - # Objects of this category are routed to the trash bin. "" disables it. + # Category the referee designates as trash. trash_category: "trash" - # Open-vocab query words for table detection (common nouns). - objects: ["cup", "can", "bottle", "dish", "box", "fruit", "utensil", "snack", "carton", "food" ] + # objects: ["cup", "can", "bottle", "bowl", "box", "apple", "fork", + # "knife", "spoon", "plate"] + # DINING TABLE table: + # Where the robot stands to pick objects from the table pose: - position: {x: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 6114615932519716} - observe_pose: - position: {x: 0.5876252953308133, y: -1.0687492206833655, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.996990359390238, w: 0.07752562984538826} - look_point: [5.25, 2.27, 0.78] - 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: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 0.6114615932519716} + + # # Where the robot stands to observe/detect the full table + # observe_pose: + # position: {x: 0.5876252953308133, y: -1.0687492206833655, z: 0.0} + # orientation: {x: 0.0, y: 0.0, z: 0.996990359390238, w: 0.07752562984538826} + + # Detection polygon — rectangle around the table top in map frame + polygon: + top_left: [3.5, 3.5] + top_right: [5.5, 3.5] + bottom_right: [5.5, 1.0] + bottom_left: [3.5, 1.0] + + # # Search polygon for FindAndGoToTable — larger than detection polygon + # search_polygon: + # top_left: [5.3, 5.9] + # top_right: [7.8, 5.9] + # bottom_right: [7.8, 3.8] + # bottom_left: [5.3, 3.8] + + # 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 (tableware + cutlery) ── + # DESTINATION 1: dishwasher dishwasher: pose: - position: {x: 0.8212924899016142, y: 0.299479767042867, z: 0.0} + position: {x: 0.821, y: 0.299, z: 0.0} # update with real pose orientation: {x: 0.0, y: 0.0, z: -0.7527398351858691, w: 0.6583181149902764} - # ── DESTINATION 2: trash bin ── + # DESTINATION 2: trash bin trash_bin: pose: - position: {x: 0.8212924899016142, y: 0.299479767042867, z: 0.0} # <-- ВСТАВ pose - orientation: {x: 0.0, y: 0.0, z: -0.7527398351858691, w: 0.6583181149902764} + position: {x: 0.0, y: 0.0, z: 0.0} # update with real pose + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} - # ── DESTINATION 3: cabinet (fake: boxes with objects) ── + # DESTINATION 3: cabinet cabinet: pose: - position: {x: -0.24395824472217295, y: -1.7992161774429396, z: 0.0} # <-- ВСТАВ pose + position: {x: -0.24395824472217295, y: -1.7992161774429396, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.9801022379942061, w: 0.13519825869117716} - # Shelves — used by ScanShelves in the NEXT deliverable. - # shelf_order is the iteration list; shelves. holds per-shelf config. - # (Tune look_point / polygon / z when the fake cabinet is built.) + # Detection polygon for cereal/milk detection inside cabinet + polygon: + top_left: [0.0, 0.0] + top_right: [1.0, 0.0] + bottom_right: [1.0, 1.0] + bottom_left: [0.0, 1.0] + + # Shelves — used by ScanShelves shelf_order: ["top", "middle", "bottom"] shelves: top: torso_lift_joint: 0.30 look_point: [0.0, 0.0, 1.0] - polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + polygon: + top_left: [0.0, 0.0] + top_right: [1.0, 0.0] + bottom_right: [1.0, 1.0] + bottom_left: [0.0, 1.0] z_min: 0.9 z_max: 1.3 middle: torso_lift_joint: 0.15 look_point: [0.0, 0.0, 0.7] - polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + polygon: + top_left: [0.0, 0.0] + top_right: [1.0, 0.0] + bottom_right: [1.0, 1.0] + bottom_left: [0.0, 1.0] z_min: 0.6 z_max: 0.9 bottom: torso_lift_joint: 0.0 look_point: [0.0, 0.0, 0.4] - polygon: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + polygon: + top_left: [0.0, 0.0] + top_right: [1.0, 0.0] + bottom_right: [1.0, 1.0] + bottom_left: [0.0, 1.0] z_min: 0.3 z_max: 0.6 - grasp: - enable: false - reach: 0.80 - use_moveit: true - publish_box: true - - # ── BREAKFAST SURFACE (bowl and spoon pickup location) ── + # BREAKFAST SURFACE (bowl and spoon pickup location) breakfast_surface: pose: position: {x: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 0.6114615932519716} \ No newline at end of file + orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 0.6114615932519716} + # update with real coordinates + polygon: + top_left: [0.0, 0.0] + top_right: [1.0, 0.0] + bottom_right: [1.0, 1.0] + bottom_left: [0.0, 1.0] + + # EXTRA SURFACE (two common objects for cleanup) + extra_surface: + pose: + position: {x: 0.0, y: 0.0, z: 0.0} # update with real pose + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + # update with real coordinates + polygon: + top_left: [0.0, 0.0] + top_right: [1.0, 0.0] + bottom_right: [1.0, 1.0] + bottom_left: [0.0, 1.0] + + # 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: "dishe" + 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/pick_and_place/states/classify_category.py b/tasks/pick_and_place/pick_and_place/states/classify_category.py index 639bf0f92..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 @@ -17,7 +17,7 @@ "carrot", "tomato", "cucumber", "lettuce", "onion", "broccoli", "cabbage", "pepper", "zucchini", "radish", "corn", "potato", "garlic", }, - "beverage": { + "drink": { "bottle", "water bottle", "juice", "milk", "soda can", "coffee cup", "energy drink", "thermos", "coke", "red bull", "iced tea", }, @@ -34,7 +34,7 @@ }, "dish": { "fork", "knife", "spoon", "plate", "bowl", "wine glass", - "mug", "chopsticks", + "mug", "chopsticks", "cup", }, } 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 693f16b75..4e9a982a0 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,305 +1,158 @@ -import time -import numpy as np - import yasmin import yasmin_ros -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 geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Header +from shapely import Polygon as ShapelyPolygon -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 - -from pick_and_place.vlm_classifier import classify_crop +from lasr_skills import DetectAllInPolygon 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. + + Uses DetectAllInPolygon (already ported to ROS 2 YASMIN in lasr_skills) + with a custom or generic YOLO model specified at construction time. - 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. VLM naming: a local VLM (Ollama) names each kept crop (replaces CLIP). - 6. Project each kept box centre → 3D via depth + TF (for manipulation later). + The polygon is loaded from ROS 2 params. - ROS 2 params: - pick_and_place.objects — query words (COMMON NOUNS). Empty → default. + 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.65 - - 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", "apple"] - BOX_THRESHOLD = 0.25 - TEXT_THRESHOLD = 0.10 - NMS_IOU = 0.5 - - # ── VLM naming (Ollama). DINO finds the boxes; the VLM says what each is. - # Run clip_rerank:=false in the launch — the VLM replaces CLIP here. - VLM_ENABLE = False - VLM_MODEL = "moondream" - VLM_HOST = "http://localhost:11434" - VLM_TIMEOUT = 60.0 - - def __init__(self, queries:list = None): + def __init__( + self, + location_param: str = "table", + object_filter: list = None, + model: str = "yolo11n-seg.pt", + 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: + 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}': {coords}" + ) + 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() + + # Load look point from config + try: + 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 as e: + yasmin.YASMIN_LOG_WARN( + f"Could not load look_point for '{location_param}': {e}. " + "Skipping head orientation." + ) + self._look_point = None - if queries is not None: - self._queries = queries + # ── Load object filter from config or use passed-in list ─────────────── + if object_filter is not None: + self._object_filter = object_filter else: try: - q = list( + self._object_filter = list( self.node.get_parameter("pick_and_place.objects") .get_parameter_value() .string_array_value ) - 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 + 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})" + ) - def _project_3d(self, cx, cy): - if self._depth is None or self._info is None or self._rgb is None: - return None + # 2. Detect objects within the polygon 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 - try: - tr = self._tf.lookup_transform( - "map", - cam_frame, - self._rgb.header.stamp, - timeout=ROS2Duration(seconds=0.5), + detector = DetectAllInPolygon( + polygon=self._polygon, + object_filter=self._object_filter, + min_confidence=self._min_confidence, + model=self._model, ) - except Exception: - try: - tr = self._tf.lookup_transform( - "map", - cam_frame, - ROS2Time(seconds=0), - timeout=ROS2Duration(seconds=0.5), - ) - except Exception: - return None - try: - return do_transform_point(ps, tr).point - except Exception: - return None - - # ── main ── - def execute(self, blackboard): - self._look_down() - 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" + blackboard["detected_objects"] = [] + blackboard["debug_images"] = [] - if not self._ovd.wait_for_service(timeout_sec=10.0): - yasmin.YASMIN_LOG_ERROR("open_vocab/detect service not available.") - return "failed" + outcome = detector.execute(blackboard) - 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}") + if outcome == "failed": + yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed.") + return "failed" - 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" + detected = blackboard["detected_objects"] - 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}") + if not detected: + yasmin.YASMIN_LOG_INFO("No objects detected.") + return "failed" - cleaned = [(self._clean_label(n), c, b) for n, c, b in raw] - kept = self._nms(cleaned) - - # VLM naming: convert the RGB once, then let the VLM name each kept crop. - rgb_cv = None - if self.VLM_ENABLE: - try: - rgb_cv = self.bridge.imgmsg_to_cv2(self._rgb, "bgr8") - except Exception as e: - yasmin.YASMIN_LOG_WARN( - f"VLM: cannot convert RGB ({e}); keeping DINO labels." - ) - - detected = [] - for name, conf, (cx, cy, w, h) in kept: - if rgb_cv is not None: - vlm_name = classify_crop( - rgb_cv, (cx, cy, w, h), - model=self.VLM_MODEL, host=self.VLM_HOST, timeout=self.VLM_TIMEOUT, - ) - if vlm_name and vlm_name != name: - yasmin.YASMIN_LOG_INFO(f"VLM: '{name}' -> '{vlm_name}'") - name = vlm_name - - d3 = Detection3D() - d3.name = name - d3.confidence = float(conf) - 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["last_rgb_image"] = self._rgb - 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" + except Exception as e: + yasmin.YASMIN_LOG_ERROR(f"Detection failed: {e}") + return "failed" \ No newline at end of file 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 index 284f941d3..6ae8c67e7 100644 --- 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 @@ -9,16 +9,12 @@ from rclpy.duration import Duration as ROS2Duration from rclpy.time import Time as ROS2Time -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 Point, PointStamped from std_msgs.msg import Header from lasr_skills import LookToPoint -from lasr_vision_interfaces.srv import OpenVocabDetect +from lasr_vision_yolo.srv import YoloDetection3D class DetectFloorTrash(yasmin.State): @@ -30,17 +26,21 @@ class DetectFloorTrash(yasmin.State): 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 + 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 + 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" @@ -51,19 +51,19 @@ def __init__(self): self.add_output_key("detected_objects") self.node = yasmin_ros.logger_node - self.bridge = CvBridge() - #Object query list, same as DetectObjects + # ── Object query list from nested config dict ────────────────────────── try: - self._queries = list( - self.node.get_parameter("pick_and_place.objects") - .get_parameter_value() - .string_array_value - ) + 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 + # ── Compute sweep points around the trash bin ────────────────────────── try: bin_x = self.node.get_parameter( "pick_and_place.trash_bin.pose.position.x" @@ -78,7 +78,7 @@ def __init__(self): ) self._sweep_points = [Point(x=0.0, y=0.0, z=0.1)] - #Synchronized camera capture + # ── Synchronized camera capture ──────────────────────────────────────── self._latest = None # (rgb, depth, info) tuple, set by sync callback cam_qos = QoSProfile( @@ -97,12 +97,10 @@ def __init__(self): ) self._ts.registerCallback(self._sync_cb) - self._ovd = self.node.create_client(OpenVocabDetect, "open_vocab/detect") + # ── YOLO 3D detection client ─────────────────────────────────────────── + self._yolo = self.node.create_client(YoloDetection3D, "/yolo/detect3d") - self._tf = tf2_ros.Buffer(cache_time=ROS2Duration(seconds=30)) - self._tf_listener = tf2_ros.TransformListener(self._tf, self.node) - - #Sweep point generation + # ── Sweep point generation ───────────────────────────────────────────────── def _compute_sweep_points(self, bin_x: float, bin_y: float) -> list: """ @@ -118,7 +116,7 @@ def _compute_sweep_points(self, bin_x: float, bin_y: float) -> list: points.append(Point(x=x, y=y, z=0.1)) # floor height return points - #Camera sync callback + # ── Camera sync callback ─────────────────────────────────────────────────── def _sync_cb(self, img, depth): info = self._info_cache.getLast() @@ -134,7 +132,7 @@ def _wait_for_synced_frame(self, timeout: float = 3.0): time.sleep(0.05) return self._latest - #Detection at a single sweep point + # ── Detection at a single sweep point ───────────────────────────────────── @staticmethod def _wait_future(future, timeout=15.0): @@ -148,50 +146,12 @@ def _wait_future(future, timeout=15.0): except Exception: return None - def _project_3d(self, cx, cy, depth_img, info, rgb_header): - """Projects a pixel coordinate to a 3D point in the map frame.""" - h, w = depth_img.shape[:2] - px = int(min(max(cx, 0), w - 1)) - py = int(min(max(cy, 0), h - 1)) - d = float(depth_img[py, px]) - if d <= 0.0: - return None - - K = info.k - fx, fy, cxp, cyp = K[0], K[4], K[2], K[5] - cam_frame = rgb_header.frame_id - - ps = PointStamped() - ps.header.frame_id = cam_frame - ps.header.stamp = rgb_header.stamp - ps.point.x = (px - cxp) * d / fx - ps.point.y = (py - cyp) * d / fy - ps.point.z = d - - try: - tr = self._tf.lookup_transform( - "map", cam_frame, 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: - return None - - try: - return do_transform_point(ps, tr).point - except Exception: - return None - def _detect_at_current_point(self): """ - Captures a synchronized frame and runs open_vocab/detect on it, - returning a list of (name, confidence, point3d) for floor-level - objects only. + 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: @@ -200,41 +160,30 @@ def _detect_at_current_point(self): rgb, depth, info = frame - if not self._ovd.wait_for_service(timeout_sec=5.0): - yasmin.YASMIN_LOG_WARN("open_vocab/detect service not available.") + if not self._yolo.wait_for_service(timeout_sec=5.0): + yasmin.YASMIN_LOG_WARN("YOLO service not available.") return [] - req = OpenVocabDetect.Request() - req.image = rgb - req.queries = list(self._queries) - req.box_threshold = 0.25 - req.text_threshold = 0.15 + req = YoloDetection3D.Request() + req.image_raw = rgb + req.depth_image = depth + req.camera_info = info + req.dataset = "robocup.pt" # TODO: update to your trained model name + req.confidence = 0.25 + req.nms = 0.3 - resp = self._wait_future(self._ovd.call_async(req), timeout=15.0) + resp = self._wait_future(self._yolo.call_async(req), timeout=15.0) if resp is None: return [] - try: - depth_img = self.bridge.imgmsg_to_cv2(depth, "32FC1") - except Exception: - return [] - - floor_objects = [] - for d in resp.detections: - if len(d.xywh) < 4: - continue - cx = d.xywh[0] + d.xywh[2] / 2 - cy = d.xywh[1] + d.xywh[3] / 2 - point3d = self._project_3d(cx, cy, depth_img, info, rgb.header) - if point3d is None: - continue - if point3d.z < self.FLOOR_Z_MAX: - d.point = point3d - floor_objects.append(d) - + # 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 + # ── Main execution ───────────────────────────────────────────────────────── def execute(self, blackboard) -> str: yasmin.YASMIN_LOG_INFO( 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 index ee096b69e..c152d113d 100644 --- 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 @@ -59,7 +59,7 @@ def __init__(self): # Detect both objects on the extra surface self.add_state( "DETECT_OBJECTS", - DetectObjects(), + DetectObjects(location_param="extra_surface", model="yolo11n-seg.pt"), transitions={ "succeeded": "SELECT_OBJECT", "failed": "DETECT_OBJECTS", 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..9a4dec1fc 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 @@ -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 ) @@ -149,13 +146,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") 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 index 415768543..1641e7c45 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -45,7 +45,7 @@ def __init__(self): # Bowl self.add_state( "DETECT_BOWL", - DetectObjects(queries=["bowl"]), + DetectObjects(location_param="breakfast_surface", object_filter=["bowl"], model="yolo11n-seg.pt"), transitions={ "succeeded": "SELECT_BOWL", "failed": "DETECT_BOWL", @@ -73,7 +73,7 @@ def __init__(self): # Spoon self.add_state( "DETECT_SPOON", - DetectObjects(queries=["spoon"]), + DetectObjects(location_param="breakfast_surface", object_filter=["spoon"], model="yolo11n-seg.pt"), transitions={ "succeeded": "SELECT_SPOON", "failed": "DETECT_SPOON", @@ -140,7 +140,7 @@ def __init__(self): # Cereal self.add_state( "DETECT_CEREAL", - DetectObjects(queries=["cereal"]), + DetectObjects(location_param="cabinet", object_filter=["cereal"], model="yolo11n-seg.pt"), transitions={ "succeeded": "SELECT_CEREAL", "failed": "DETECT_CEREAL", @@ -166,7 +166,7 @@ def __init__(self): # Milk self.add_state( "DETECT_MILK", - DetectObjects(queries=["milk"]), + DetectObjects(location_param="cabinet", object_filter=["milk"], model="yolo11n-seg.pt"), transitions={ "succeeded": "SELECT_MILK", "failed": "DETECT_MILK", 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 index 8237cf7c7..0dcb69fba 100644 --- a/tasks/pick_and_place/pick_and_place/states/table_cleanup.py +++ b/tasks/pick_and_place/pick_and_place/states/table_cleanup.py @@ -59,7 +59,7 @@ def __init__(self): # Detect all objects on the table (done ONCE) self.add_state( "DETECT_OBJECTS", - DetectObjects(), + DetectObjects(location_param="table", model="yolo11n-seg.pt"), transitions={ "succeeded": "SELECT_OBJECT", "failed": "DETECT_OBJECTS", @@ -174,7 +174,7 @@ def __init__(self): "DETECT_FLOOR_TRASH", DetectFloorTrash(), transitions={ - "succeeded": "SELECT_FLOOR_TRASH", + "succeeded": "SET_FLOOR_TRASH_CONTEXT", "failed": "succeeded", # nothing found, floor trash optional }, ) @@ -188,6 +188,20 @@ def __init__(self): }, ) + 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(), From 0624c5f800f2f6682a30b85f39fa6cbc30bcda3e Mon Sep 17 00:00:00 2001 From: Yara Alkhelaiwi Date: Thu, 2 Jul 2026 03:17:53 +0100 Subject: [PATCH 18/21] Fixes and config file values --- .../lasr_vision_yolo/service.py | 33 +++- .../launch/service_launch.xml | 2 +- common/vision/lasr_vision_yolo/setup.py | 1 + tasks/pick_and_place/config/config.yaml | 164 +++++++++--------- .../launch/pick_and_place.launch.py | 76 +++----- .../pick_and_place/states/detect_objects.py | 8 +- 6 files changed, 135 insertions(+), 149 deletions(-) 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 8f4c2930e..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,14 +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", "yolo11n.pt", "yolo11n-pose.pt"] + "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 + self.preload_param_list = self.node.get_parameter("preload").value for model in self.preload_param_list: self._maybe_load_model(model) @@ -371,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( 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/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index e4c8cab3d..1a4edac4e 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -3,36 +3,21 @@ pick_and_place: # Category the referee designates as trash. - trash_category: "trash" - - # objects: ["cup", "can", "bottle", "bowl", "box", "apple", "fork", - # "knife", "spoon", "plate"] + trash_category: "fruit" # DINING TABLE table: - # Where the robot stands to pick objects from the table + # Where the robot stands to pick objects from the table yes pose: - position: {x: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 0.6114615932519716} - - # # Where the robot stands to observe/detect the full table - # observe_pose: - # position: {x: 0.5876252953308133, y: -1.0687492206833655, z: 0.0} - # orientation: {x: 0.0, y: 0.0, z: 0.996990359390238, w: 0.07752562984538826} + 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 + # Detection polygon — rectangle around the table top in map frame yes polygon: - top_left: [3.5, 3.5] - top_right: [5.5, 3.5] - bottom_right: [5.5, 1.0] - bottom_left: [3.5, 1.0] - - # # Search polygon for FindAndGoToTable — larger than detection polygon - # search_polygon: - # top_left: [5.3, 5.9] - # top_right: [7.8, 5.9] - # bottom_right: [7.8, 3.8] - # bottom_left: [5.3, 3.8] + 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 @@ -45,88 +30,101 @@ size: [1.02, 1.2, 0.9] position: [1.2, -3.9, 0.37] - # DESTINATION 1: dishwasher + # DESTINATION 1: dishwasher yes dishwasher: pose: - position: {x: 0.821, y: 0.299, z: 0.0} # update with real pose - orientation: {x: 0.0, y: 0.0, z: -0.7527398351858691, w: 0.6583181149902764} + 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 + # DESTINATION 2: trash bin yes trash_bin: pose: - position: {x: 0.0, y: 0.0, z: 0.0} # update with real pose - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + 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 + # DESTINATION 3: cabinet yes cabinet: pose: - position: {x: -0.24395824472217295, y: -1.7992161774429396, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.9801022379942061, w: 0.13519825869117716} + 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 + # Detection polygon for cereal/milk detection inside cabinet (from real) polygon: - top_left: [0.0, 0.0] - top_right: [1.0, 0.0] - bottom_right: [1.0, 1.0] - bottom_left: [0.0, 1.0] + 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 - shelf_order: ["top", "middle", "bottom"] + # Shelves — used by ScanShelves also from real + shelf_order: ["top", "middle", "bottom", "extra_bottom"] shelves: - top: - torso_lift_joint: 0.30 - look_point: [0.0, 0.0, 1.0] - polygon: - top_left: [0.0, 0.0] - top_right: [1.0, 0.0] - bottom_right: [1.0, 1.0] - bottom_left: [0.0, 1.0] - z_min: 0.9 - z_max: 1.3 - middle: - torso_lift_joint: 0.15 - look_point: [0.0, 0.0, 0.7] - polygon: - top_left: [0.0, 0.0] - top_right: [1.0, 0.0] - bottom_right: [1.0, 1.0] - bottom_left: [0.0, 1.0] - z_min: 0.6 - z_max: 0.9 - bottom: - torso_lift_joint: 0.0 - look_point: [0.0, 0.0, 0.4] - polygon: - top_left: [0.0, 0.0] - top_right: [1.0, 0.0] - bottom_right: [1.0, 1.0] - bottom_left: [0.0, 1.0] - z_min: 0.3 - z_max: 0.6 + top: + torso_lift_joint: 0.30 + look_point: [-10.44567584991455, 21.501087188720703, 1.1] + 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: 0.5444910497763044, y: -2.4412505621588583, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: -0.7912741117828641, w: 0.6114615932519716} + 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: [0.0, 0.0] - top_right: [1.0, 0.0] - bottom_right: [1.0, 1.0] - bottom_left: [0.0, 1.0] + 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: 0.0, y: 0.0, z: 0.0} # update with real pose - orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} + 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: [0.0, 0.0] - top_right: [1.0, 0.0] - bottom_right: [1.0, 1.0] - bottom_left: [0.0, 1.0] + 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: @@ -145,7 +143,7 @@ objects: apple: category: "fruit" fork: - category: "dishe" + category: "dish" knife: category: "dish" spoon: 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 7eadb8b3a..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,75 +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 -from launch.actions import ExecuteProcess + def generate_launch_description(): """ - One-shot launch for the Pick and Place task. + Launch file for the Pick and Place task using YOLO detection. - Perception = open-vocab detection (localisation) + CLIP rerank (recognition), - both inside the lasr_vision_open_vocabulary node (its venv already has - transformers/torch — CLIP comes for free, no new deps). + Launch SEPARATELY before running this: + - Simulator / robot bringup + - Nav2 + localisation - Still launch SEPARATELY: simulator / robot bringup + nav2 + localisation. - - Start after the model has loaded: + 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") - ov_params = os.path.join( - get_package_share_directory("lasr_vision_open_vocabulary"), - "config", - "params.yaml", - ) - - use_llm = LaunchConfiguration("use_llm") + use_sim = LaunchConfiguration("use_sim") return LaunchDescription([ DeclareLaunchArgument( - "use_llm", - default_value="false", - description="Also start the storing_groceries LLM service " - "(category fallback). Forced onto CPU. Default off.", + "use_sim", + default_value="true", + description="Set to false when running on the real robot " + "to disable the point_head_stub.", ), - # ── Perception: open-vocab detection + CLIP recognition rerank ──────── - # Node( - # package="lasr_vision_open_vocabulary", - # executable="open_vocabulary_node", - # name="lasr_vision_open_vocabulary", - # output="screen", - # parameters=[ - # ov_params, - # {"clip_rerank": True, "clip_candidates": CLIP_CANDIDATES}, - # ], - # ), - + # ── Perception: YOLO detection ──────────────────────────────────────── Node( - package="lasr_vision_open_vocabulary", - executable="detection_visualizer", - name="detection_visualizer", + package="lasr_vision_yolo", + executable="yolo_service_node", + name="lasr_vision_yolo", output="screen", + parameters=[{ + "preload": ["/path/to/lasr_vision_yolo/models/best.pt"]}], ), - # ── Optional: LLM category-fallback service (CPU-forced) ───────────── - # Node( - # condition=IfCondition(use_llm), - # package="lasr_llm", - # executable="storing_groceries_service", - # name="storing_groceries_query_llm_service", - # output="screen", - # additional_env={"CUDA_VISIBLE_DEVICES": ""}, - # ), - - # ── Task: state machine ────────────────────────────────────────────── + # ── Task: state machine ─────────────────────────────────────────────── Node( package="pick_and_place", executable="state_machine", @@ -78,19 +50,13 @@ def generate_launch_description(): parameters=[config], ), - # ── Head stub ──────────────────────────────────────────────────────── + # ── 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", output="screen", ), - ExecuteProcess( - cmd=["bash", "-c", - "curl -sf localhost:11434/api/tags >/dev/null 2>&1 " - "&& echo 'ollama already running' " - "|| exec ollama serve"], - name="ollama_serve", - output="screen", - ), ]) \ 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 4e9a982a0..7520a495e 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 @@ -7,7 +7,11 @@ from lasr_skills import DetectAllInPolygon - +_MODEL_PATH = os.path.join( + get_package_share_directory("lasr_vision_yolo"), + "models", + "best.pt" +) class DetectObjects(yasmin.State): """ Looks at a configured surface and detects objects within its polygon @@ -36,7 +40,7 @@ def __init__( self, location_param: str = "table", object_filter: list = None, - model: str = "yolo11n-seg.pt", + model: str = _MODEL_PATH, min_confidence: float = 0.1, ): super().__init__(outcomes=["succeeded", "failed"]) From 958f4ed6899be1304ef603c5d0247c3ef0dc6824 Mon Sep 17 00:00:00 2001 From: yarakmk Date: Thu, 2 Jul 2026 04:35:42 +0100 Subject: [PATCH 19/21] temp --- tasks/pick_and_place/config/config.yaml | 190 +++++++++--------- .../pick_and_place/states/__init__.py | 5 +- .../pick_and_place/states/detect_objects.py | 4 +- .../states/detect_trash_floor.py | 2 +- .../pick_and_place/states/serve_breakfast.py | 9 +- .../pick_and_place/states/start.py | 10 +- .../pick_and_place/states/table_cleanup.py | 3 +- 7 files changed, 113 insertions(+), 110 deletions(-) diff --git a/tasks/pick_and_place/config/config.yaml b/tasks/pick_and_place/config/config.yaml index 1a4edac4e..f0e2c75f7 100644 --- a/tasks/pick_and_place/config/config.yaml +++ b/tasks/pick_and_place/config/config.yaml @@ -1,4 +1,4 @@ -/**: +pick_and_place: ros__parameters: pick_and_place: @@ -58,49 +58,49 @@ # 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.1] - 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 + 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: @@ -133,54 +133,54 @@ 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 + 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/pick_and_place/states/__init__.py b/tasks/pick_and_place/pick_and_place/states/__init__.py index 6522e4cf6..ab790e2ad 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -13,4 +13,7 @@ # from .grasp_object import GraspObject from .approach_table import ApproachTable -from .serve_breakfast import ServeBreakfast \ No newline at end of file +from .serve_breakfast import ServeBreakfast +from .table_cleanup import TableCleanup +from .extra_surface_cleanup import ExtraSurfaceCleanup +from .detect_trash_floor import DetectFloorTrash \ 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 7520a495e..85bcc7166 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,10 +1,10 @@ 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 _MODEL_PATH = os.path.join( 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 index 6ae8c67e7..e0386187b 100644 --- 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 @@ -14,7 +14,7 @@ from std_msgs.msg import Header from lasr_skills import LookToPoint -from lasr_vision_yolo.srv import YoloDetection3D +from lasr_vision_interfaces.srv import YoloDetection3D class DetectFloorTrash(yasmin.State): 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 index 1641e7c45..6a9e1aafe 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -66,7 +66,8 @@ def __init__(self): Say(text="I have detected a bowl. Please pick it up and hold it ready."), transitions={ "succeeded": "DETECT_SPOON", - "failed": "INSTRUCT_PICK_BOWL", + "aborted": "INSTRUCT_PICK_BOWL", + "canceled": "INSTRUCT_PICK_BOWL", }, ) @@ -94,7 +95,7 @@ def __init__(self): Say(text="I have detected a spoon. Please pick it up and hold it ready."), transitions={ "succeeded": "GO_TO_TABLE_1", - "failed": "INSTRUCT_PICK_SPOON", + "canceled": "INSTRUCT_PICK_SPOON", }, ) @@ -159,7 +160,7 @@ def __init__(self): Say(text="I have detected cereal. Please pick it up and hold it ready."), transitions={ "succeeded": "DETECT_MILK", - "failed": "INSTRUCT_PICK_CEREAL", + "canceled": "INSTRUCT_PICK_CEREAL", }, ) @@ -185,7 +186,7 @@ def __init__(self): Say(text="I have detected milk. Please pick it up and hold it ready."), transitions={ "succeeded": "GO_TO_TABLE_2", - "failed": "INSTRUCT_PICK_MILK", + "canceled": "INSTRUCT_PICK_MILK", }, ) 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 212647883..c1ef4bbf9 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): @@ -52,9 +52,9 @@ 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", + "succeeded": "WAIT_FOR_DOOR", + "aborted": "WAIT_FOR_DOOR", + "canceled": "WAIT_FOR_DOOR", }, ) @@ -63,7 +63,7 @@ def wait_cb(blackboard, msg): "WAIT_FOR_DOOR", StartDoorSM(), transitions={ - "door_opened": "SAY_GOING_TO_TABLE", + "succeeded": "SAY_GOING_TO_TABLE", "failed": "WAIT_FOR_DOOR", }, ) 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 index 0dcb69fba..f3747ada2 100644 --- a/tasks/pick_and_place/pick_and_place/states/table_cleanup.py +++ b/tasks/pick_and_place/pick_and_place/states/table_cleanup.py @@ -10,8 +10,7 @@ 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_floor_trash import DetectFloorTrash - +from pick_and_place.states.detect_trash_floor import DetectFloorTrash class TableCleanup(yasmin.StateMachine): """ From eda5211aa499e98cd3d9b8913a4e3e4291f2ef56 Mon Sep 17 00:00:00 2001 From: Yara Alkhelaiwi Date: Thu, 2 Jul 2026 07:45:44 +0100 Subject: [PATCH 20/21] Kill node --- .../pick_and_place/state_machine.py | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) 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 712d41a2e..33f52f5bd 100644 --- a/tasks/pick_and_place/pick_and_place/state_machine.py +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -3,7 +3,7 @@ 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 @@ -135,7 +135,7 @@ def __init__(self): ) 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() @@ -168,15 +168,28 @@ def main(): bb["debug_images"] = [] bb["last_rgb_image"] = None + 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__": From edcebbcf0905d19175a4be9e5d4d73a2434db154 Mon Sep 17 00:00:00 2001 From: yarakmk Date: Fri, 3 Jul 2026 01:52:57 +0100 Subject: [PATCH 21/21] Bug fixes --- skills/src/lasr_skills/say.py | 2 +- tasks/pick_and_place/config/sim.yaml | 185 ++++++++++++++++++ tasks/pick_and_place/pick_and_place/shelf.py | 71 +++++++ .../pick_and_place/state_machine.py | 55 +++--- .../pick_and_place/states/__init__.py | 3 +- .../pick_and_place/states/choose_shelf.py | 51 +++-- .../pick_and_place/states/detect_objects.py | 19 +- .../states/detect_trash_floor.py | 49 +++-- .../states/extra_surface_cleanup.py | 30 +-- .../pick_and_place/states/scan_shelves.py | 25 +-- .../states/scan_shelves_if_needed.py | 41 ++++ .../pick_and_place/states/serve_breakfast.py | 22 ++- .../pick_and_place/states/start.py | 33 ++-- .../pick_and_place/states/table_cleanup.py | 116 ++++++++--- tasks/pick_and_place/setup.py | 1 + 15 files changed, 550 insertions(+), 153 deletions(-) create mode 100644 tasks/pick_and_place/config/sim.yaml create mode 100644 tasks/pick_and_place/pick_and_place/shelf.py create mode 100644 tasks/pick_and_place/pick_and_place/states/scan_shelves_if_needed.py 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/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/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 33f52f5bd..788216017 100644 --- a/tasks/pick_and_place/pick_and_place/state_machine.py +++ b/tasks/pick_and_place/pick_and_place/state_machine.py @@ -47,7 +47,7 @@ def __init__(self): Start(), transitions={ "succeeded": "SAY_STARTING_CLEANUP", - "failed": "failed", + "failed": "failed", }, ) @@ -57,8 +57,8 @@ def __init__(self): Say(text="I will now clean up the dining table."), transitions={ "succeeded": "TABLE_CLEANUP", - "aborted": "TABLE_CLEANUP", - "canceled": "TABLE_CLEANUP", + "aborted": "TABLE_CLEANUP", + "canceled": "TABLE_CLEANUP", }, ) @@ -67,7 +67,7 @@ def __init__(self): TableCleanup(), transitions={ "succeeded": "SAY_STARTING_BREAKFAST", - "failed": "SAY_STARTING_BREAKFAST", # continue regardless + "failed": "SAY_STARTING_BREAKFAST", # continue regardless }, ) @@ -77,8 +77,8 @@ def __init__(self): Say(text="I will now set up breakfast."), transitions={ "succeeded": "SERVE_BREAKFAST", - "aborted": "SERVE_BREAKFAST", - "canceled": "SERVE_BREAKFAST", + "aborted": "SERVE_BREAKFAST", + "canceled": "SERVE_BREAKFAST", }, ) @@ -87,7 +87,7 @@ def __init__(self): ServeBreakfast(), transitions={ "succeeded": "SAY_STARTING_EXTRA_SURFACE", - "failed": "SAY_STARTING_EXTRA_SURFACE", + "failed": "SAY_STARTING_EXTRA_SURFACE", }, ) @@ -97,8 +97,8 @@ def __init__(self): Say(text="I will now check the extra surface."), transitions={ "succeeded": "EXTRA_SURFACE_CLEANUP", - "aborted": "EXTRA_SURFACE_CLEANUP", - "canceled": "EXTRA_SURFACE_CLEANUP", + "aborted": "EXTRA_SURFACE_CLEANUP", + "canceled": "EXTRA_SURFACE_CLEANUP", }, ) @@ -107,7 +107,7 @@ def __init__(self): ExtraSurfaceCleanup(), transitions={ "succeeded": "SAY_TASK_COMPLETE", - "failed": "SAY_TASK_COMPLETE", + "failed": "SAY_TASK_COMPLETE", }, ) @@ -116,12 +116,12 @@ def __init__(self): "SAY_TASK_COMPLETE", Say( text="I have completed the pick and place task. " - "The table is clean and breakfast is ready." + "The table is clean and breakfast is ready." ), transitions={ "succeeded": "succeeded", - "aborted": "succeeded", - "canceled": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", }, ) @@ -153,20 +153,21 @@ 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["destination"] = "" - bb["destination_str"] = "" - bb["location"] = None - bb["table_pose"] = None - bb["debug_images"] = [] - bb["last_rgb_image"] = None + 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...") @@ -193,4 +194,4 @@ def shutdown(sig=None, frame=None): 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 ab790e2ad..0ac1d1ab0 100644 --- a/tasks/pick_and_place/pick_and_place/states/__init__.py +++ b/tasks/pick_and_place/pick_and_place/states/__init__.py @@ -16,4 +16,5 @@ from .serve_breakfast import ServeBreakfast from .table_cleanup import TableCleanup from .extra_surface_cleanup import ExtraSurfaceCleanup -from .detect_trash_floor import DetectFloorTrash \ No newline at end of file +from .detect_trash_floor import DetectFloorTrash +from .scan_shelves_if_needed import ScanShelvesIfNeeded 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 a30486bad..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"] = "" + 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/detect_objects.py b/tasks/pick_and_place/pick_and_place/states/detect_objects.py index 85bcc7166..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 @@ -8,10 +8,10 @@ from lasr_skills import DetectAllInPolygon _MODEL_PATH = os.path.join( - get_package_share_directory("lasr_vision_yolo"), - "models", - "best.pt" + get_package_share_directory("lasr_vision_yolo"), "models", "best.pt" ) + + class DetectObjects(yasmin.State): """ Looks at a configured surface and detects objects within its polygon @@ -68,7 +68,7 @@ def __init__( ] self._polygon = ShapelyPolygon(polygon_points) yasmin.YASMIN_LOG_INFO( - f"Loaded polygon for '{location_param}': {coords}" + f"Loaded polygon for '{location_param}': {polygon_points}" ) except Exception as e: yasmin.YASMIN_LOG_WARN( @@ -134,9 +134,9 @@ def execute(self, blackboard) -> str: ) blackboard["detected_objects"] = [] - blackboard["debug_images"] = [] + blackboard["debug_images"] = [] - outcome = detector.execute(blackboard) + outcome = detector(blackboard) if outcome == "failed": yasmin.YASMIN_LOG_WARN("DetectAllInPolygon failed.") @@ -148,10 +148,7 @@ def execute(self, blackboard) -> str: yasmin.YASMIN_LOG_INFO("No objects detected.") return "failed" - labels = [ - f"{obj.name} ({obj.confidence:.2f})" - for obj in detected - ] + labels = [f"{obj.name} ({obj.confidence:.2f})" for obj in detected] yasmin.YASMIN_LOG_INFO( f"Detected {len(detected)} object(s): {', '.join(labels)}" ) @@ -159,4 +156,4 @@ def execute(self, blackboard) -> str: except Exception as e: yasmin.YASMIN_LOG_ERROR(f"Detection failed: {e}") - return "failed" \ No newline at end of file + 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 index e0386187b..e3fea7b3f 100644 --- 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 @@ -38,13 +38,13 @@ class DetectFloorTrash(yasmin.State): 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 + 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" + RGB_TOPIC = "/head_front_camera/rgb/image_raw" DEPTH_TOPIC = "/head_front_camera/depth/image_raw" - INFO_TOPIC = "/head_front_camera/rgb/camera_info" + INFO_TOPIC = "/head_front_camera/rgb/camera_info" def __init__(self): super().__init__(outcomes=["succeeded", "failed"]) @@ -54,10 +54,12 @@ def __init__(self): # ── 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() - )) + 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: @@ -87,9 +89,15 @@ def __init__(self): 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) + 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( @@ -165,12 +173,12 @@ def _detect_at_current_point(self): return [] req = YoloDetection3D.Request() - req.image_raw = rgb - req.depth_image = depth - req.camera_info = info - req.dataset = "robocup.pt" # TODO: update to your trained model name - req.confidence = 0.25 - req.nms = 0.3 + 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: @@ -178,8 +186,7 @@ def _detect_at_current_point(self): # 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 + d for d in resp.detected_objects if d.point.z < self.FLOOR_Z_MAX ] return floor_objects @@ -222,4 +229,4 @@ def execute(self, blackboard) -> str: return "succeeded" yasmin.YASMIN_LOG_INFO("No object found on the floor after full sweep.") - return "failed" \ No newline at end of file + 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 index c152d113d..03a8225a4 100644 --- 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 @@ -42,8 +42,8 @@ def __init__(self): 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", + "aborted": "GO_TO_EXTRA_SURFACE", + "canceled": "GO_TO_EXTRA_SURFACE", }, ) @@ -52,17 +52,17 @@ def __init__(self): GoToLocation(location_param="pick_and_place.extra_surface.pose"), transitions={ "succeeded": "DETECT_OBJECTS", - "failed": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", }, ) # Detect both objects on the extra surface self.add_state( "DETECT_OBJECTS", - DetectObjects(location_param="extra_surface", model="yolo11n-seg.pt"), + DetectObjects(location_param="extra_surface", model="best.pt"), transitions={ "succeeded": "SELECT_OBJECT", - "failed": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", }, ) @@ -72,7 +72,7 @@ def __init__(self): SelectAndVisualiseObject(), transitions={ "succeeded": "CLASSIFY_CATEGORY", - "finished": "succeeded", # both objects processed + "finished": "succeeded", # both objects processed }, ) @@ -82,8 +82,8 @@ def __init__(self): ClassifyCategory(task="object"), transitions={ "succeeded": "CHOOSE_SHELF", - "failed": "CHOOSE_SHELF", # proceed with unknown category - "empty": "SELECT_OBJECT", # nothing to classify, next object + "failed": "CHOOSE_SHELF", # proceed with unknown category + "empty": "SELECT_OBJECT", # nothing to classify, next object }, ) @@ -93,7 +93,7 @@ def __init__(self): ChooseShelf(), transitions={ "succeeded": "INSTRUCT_PICK", - "failed": "INSTRUCT_PICK", # announce anyway + "failed": "INSTRUCT_PICK", # announce anyway }, ) @@ -103,7 +103,7 @@ def __init__(self): InstructPick(), transitions={ "succeeded": "GO_TO_CABINET", - "failed": "INSTRUCT_PICK", + "failed": "INSTRUCT_PICK", }, ) @@ -113,7 +113,7 @@ def __init__(self): GoToLocation(location_param="pick_and_place.cabinet.pose"), transitions={ "succeeded": "INSTRUCT_PLACE", - "failed": "INSTRUCT_PLACE", # announce even if nav failed + "failed": "INSTRUCT_PLACE", # announce even if nav failed }, ) @@ -123,7 +123,7 @@ def __init__(self): InstructPlace(), transitions={ "succeeded": "GO_TO_EXTRA_SURFACE_LOOP", - "failed": "INSTRUCT_PLACE", + "failed": "INSTRUCT_PLACE", }, ) @@ -132,7 +132,7 @@ def __init__(self): "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", + "succeeded": "SELECT_OBJECT", # loop back for next object + "failed": "GO_TO_EXTRA_SURFACE_LOOP", }, - ) \ No newline at end of file + ) 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 9a4dec1fc..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): """ @@ -111,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( @@ -147,10 +148,10 @@ def _get_shelf_config(self, shelf_id: str) -> bool: ) 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.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) @@ -194,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( @@ -213,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/serve_breakfast.py b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py index 6a9e1aafe..c262d2071 100644 --- a/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py +++ b/tasks/pick_and_place/pick_and_place/states/serve_breakfast.py @@ -45,7 +45,11 @@ def __init__(self): # Bowl self.add_state( "DETECT_BOWL", - DetectObjects(location_param="breakfast_surface", object_filter=["bowl"], model="yolo11n-seg.pt"), + DetectObjects( + location_param="breakfast_surface", + object_filter=["bowl"], + model="best.pt", + ), transitions={ "succeeded": "SELECT_BOWL", "failed": "DETECT_BOWL", @@ -74,7 +78,11 @@ def __init__(self): # Spoon self.add_state( "DETECT_SPOON", - DetectObjects(location_param="breakfast_surface", object_filter=["spoon"], model="yolo11n-seg.pt"), + DetectObjects( + location_param="breakfast_surface", + object_filter=["spoon"], + model="best.pt", + ), transitions={ "succeeded": "SELECT_SPOON", "failed": "DETECT_SPOON", @@ -141,7 +149,11 @@ def __init__(self): # Cereal self.add_state( "DETECT_CEREAL", - DetectObjects(location_param="cabinet", object_filter=["cereal"], model="yolo11n-seg.pt"), + DetectObjects( + location_param="cabinet", + object_filter=["cereal"], + model="best.pt", + ), transitions={ "succeeded": "SELECT_CEREAL", "failed": "DETECT_CEREAL", @@ -167,7 +179,9 @@ def __init__(self): # Milk self.add_state( "DETECT_MILK", - DetectObjects(location_param="cabinet", object_filter=["milk"], model="yolo11n-seg.pt"), + DetectObjects( + location_param="cabinet", object_filter=["milk"], model="best.pt" + ), transitions={ "succeeded": "SELECT_MILK", "failed": "DETECT_MILK", 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 c1ef4bbf9..6fe8ef0f4 100644 --- a/tasks/pick_and_place/pick_and_place/states/start.py +++ b/tasks/pick_and_place/pick_and_place/states/start.py @@ -42,8 +42,8 @@ def wait_cb(blackboard, msg): ), transitions={ "succeeded": "SAY_START", - "failed": "WAIT_START", - "canceled": "failed", + "failed": "WAIT_START", + "canceled": "failed", }, ) @@ -53,8 +53,8 @@ def wait_cb(blackboard, msg): Say(text="Start of Pick and Place task."), transitions={ "succeeded": "WAIT_FOR_DOOR", - "aborted": "WAIT_FOR_DOOR", - "canceled": "WAIT_FOR_DOOR", + "aborted": "WAIT_FOR_DOOR", + "canceled": "WAIT_FOR_DOOR", }, ) @@ -64,41 +64,38 @@ def wait_cb(blackboard, msg): StartDoorSM(), transitions={ "succeeded": "SAY_GOING_TO_TABLE", - "failed": "WAIT_FOR_DOOR", + "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.observe_pose"), + 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="" - ), + 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 index f3747ada2..3f45acc88 100644 --- a/tasks/pick_and_place/pick_and_place/states/table_cleanup.py +++ b/tasks/pick_and_place/pick_and_place/states/table_cleanup.py @@ -11,6 +11,8 @@ 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): """ @@ -51,17 +53,17 @@ def __init__(self): GoToLocation(location_param="pick_and_place.table.pose"), transitions={ "succeeded": "DETECT_OBJECTS", - "failed": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", }, ) # Detect all objects on the table (done ONCE) self.add_state( "DETECT_OBJECTS", - DetectObjects(location_param="table", model="yolo11n-seg.pt"), + DetectObjects(location_param="table", model="best.pt"), transitions={ "succeeded": "SELECT_OBJECT", - "failed": "DETECT_OBJECTS", + "failed": "DETECT_OBJECTS", }, ) @@ -71,7 +73,17 @@ def __init__(self): SelectAndVisualiseObject(), transitions={ "succeeded": "CLASSIFY_CATEGORY", - "finished": "SAY_CLEANUP_DONE", + "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", }, ) @@ -81,8 +93,8 @@ def __init__(self): ClassifyCategory(task="object"), transitions={ "succeeded": "DECIDE_DESTINATION", - "failed": "DECIDE_DESTINATION", - "empty": "SELECT_OBJECT", + "failed": "DECIDE_DESTINATION", + "empty": "SELECT_OBJECT", }, ) @@ -92,7 +104,7 @@ def __init__(self): DecideDestination(), transitions={ "cabinet": "CHOOSE_SHELF", - "other": "INSTRUCT_PICK", + "other": "INSTRUCT_PICK", }, ) @@ -102,7 +114,7 @@ def __init__(self): ChooseShelf(), transitions={ "succeeded": "INSTRUCT_PICK", - "failed": "INSTRUCT_PICK", + "failed": "INSTRUCT_PICK", }, ) @@ -112,17 +124,37 @@ def __init__(self): InstructPick(), transitions={ "succeeded": "GO_TO_DESTINATION", - "failed": "INSTRUCT_PICK", + "failed": "INSTRUCT_PICK", }, ) - # Navigate to the chosen destination (pose set by DecideDestination) + # 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", - "failed": "INSTRUCT_PLACE", + "skipped": "INSTRUCT_PLACE", }, ) @@ -132,7 +164,7 @@ def __init__(self): InstructPlace(), transitions={ "succeeded": "GO_TO_TABLE", - "failed": "INSTRUCT_PLACE", + "failed": "INSTRUCT_PLACE", }, ) @@ -142,7 +174,7 @@ def __init__(self): GoToLocation(location_param="pick_and_place.table.pose"), transitions={ "succeeded": "SELECT_OBJECT", - "failed": "GO_TO_TABLE", + "failed": "GO_TO_TABLE", }, ) @@ -151,12 +183,12 @@ def __init__(self): "SAY_CLEANUP_DONE", Say( text="I have finished cleaning the table. " - "Let me check the floor near the trash bin." + "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", + "aborted": "GO_TO_TRASH_BIN_FLOOR", + "canceled": "GO_TO_TRASH_BIN_FLOOR", }, ) @@ -165,7 +197,7 @@ def __init__(self): GoToLocation(location_param="pick_and_place.trash_bin.pose"), transitions={ "succeeded": "DETECT_FLOOR_TRASH", - "failed": "DETECT_FLOOR_TRASH", + "failed": "DETECT_FLOOR_TRASH", }, ) @@ -174,7 +206,7 @@ def __init__(self): DetectFloorTrash(), transitions={ "succeeded": "SET_FLOOR_TRASH_CONTEXT", - "failed": "succeeded", # nothing found, floor trash optional + "failed": "succeeded", # nothing found, floor trash optional }, ) @@ -183,7 +215,7 @@ def __init__(self): SelectAndVisualiseObject(), transitions={ "succeeded": "INSTRUCT_PICK_FLOOR", - "finished": "succeeded", + "finished": "succeeded", }, ) @@ -196,17 +228,18 @@ def __init__(self): bb.__setitem__("destination_str", "the trash bin"), bb.__setitem__("chosen_shelf", ""), bb.__setitem__("chosen_shelf_str", ""), - ] and "succeeded", + ] + and "succeeded", ), transitions={"succeeded": "INSTRUCT_PICK_FLOOR"}, ) - + self.add_state( "INSTRUCT_PICK_FLOOR", InstructPick(), transitions={ "succeeded": "INSTRUCT_PLACE_FLOOR", - "failed": "INSTRUCT_PICK_FLOOR", + "failed": "INSTRUCT_PICK_FLOOR", }, ) @@ -215,7 +248,40 @@ def __init__(self): Say(text="Please place it in the trash bin."), transitions={ "succeeded": "succeeded", - "aborted": "succeeded", - "canceled": "succeeded", + "aborted": "succeeded", + "canceled": "succeeded", }, - ) \ No newline at end of file + ) + + +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/setup.py b/tasks/pick_and_place/setup.py index a5b8da03d..157b3b9f3 100644 --- a/tasks/pick_and_place/setup.py +++ b/tasks/pick_and_place/setup.py @@ -30,6 +30,7 @@ "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", ], }, )