Hotfix/ros2 receptionist skills part2

common/vision/lasr_vision_yolo/lasr_vision_yolo/__init__.py

@@ -1 +0,0 @@
-from .yolo import AccessNode, load_model, start_tf_buffer, detect

common/vision/lasr_vision_yolo/lasr_vision_yolo/service.py

@@ -167,7 +167,7 @@ def _detect3d(
         depth_im = self._bridge.imgmsg_to_cv2(
             req.depth_image, desired_encoding="passthrough"
         )
-        K = req.depth_camera_info.K
+        K = req.depth_camera_info.k
         fx, fy = K[0], K[4]
         cx, cy = K[2], K[5]
 
@@ -227,7 +227,7 @@ def _detect3d(
                 points = np.stack((x, y, z), axis=1)
                 x, y, z = np.median(points, axis=0)
 
-                point = Point(x, y, z)
+                point = Point(x=float(x), y=float(y), z=float(z))
                 point_stamped = PointStamped()
                 point_stamped.header = req.depth_image.header
                 point_stamped.point = point
@@ -260,7 +260,7 @@ def _detect_keypoints(
                 y = result.keypoints.xy.squeeze()[idx, 1].round().int().item()
                 conf = result.keypoints.conf.squeeze()[idx].item()
                 if conf > 0.0:
-                    keypoints.keypoints.append(Keypoint(name, x, y))
+                    keypoints.keypoints.append(Keypoint(keypoint_name=name, x=x, y=y))
             response.detections.append(keypoints)
 
         self._publish_results(req, results, response)
@@ -277,7 +277,7 @@ def _detect_keypoints3d(
         depth_im = self._bridge.imgmsg_to_cv2(
             req.depth_image, desired_encoding="passthrough"
         )
-        K = req.depth_camera_info.K
+        K = req.depth_camera_info.k
         fx, fy = K[0], K[4]
         cx, cy = K[2], K[5]
 
@@ -318,7 +318,7 @@ def _detect_keypoints3d(
                     if np.isnan(x) or np.isnan(y) or np.isnan(z):
                         continue
 
-                    point = Point(x, y, z)
+                    point = Point(x=float(x), y=float(y), z=float(z))
                     point_stamped = PointStamped()
                     point_stamped.header = req.depth_image.header
                     point_stamped.point = point
@@ -327,7 +327,9 @@ def _detect_keypoints3d(
                     )
                     point = point_stamped_transformed.point
 
-                    keypoints.keypoints.append(Keypoint3D(name, point))
+                    keypoints.keypoints.append(
+                        Keypoint3D(keypoint_name=name, point=point)
+                    )
             response.detections.append(keypoints)
 
         self._publish_results(req, results, response)

common/vision/lasr_vision_yolo/package.xml

@@ -11,14 +11,16 @@
     <exec_depend>ros2launch</exec_depend>
     <exec_depend>cv2_img</exec_depend>
     <exec_depend>cv2_pcl</exec_depend>
-    <!--  <exec_depend>markers</exec_depend> uncomment for 3D detection -->
+    <exec_depend>markers</exec_depend>
 
     <test_depend>ament_copyright</test_depend>
     <test_depend>ament_flake8</test_depend>
     <test_depend>ament_pep257</test_depend>
     <test_depend>python3-pytest</test_depend>
 
+    <build_depend>ament_virtualenv</build_depend>
     <export>
         <build_type>ament_python</build_type>
+        <pip_requirements>requirements.txt</pip_requirements>
     </export>
 </package>

common/vision/lasr_vision_yolo/setup.py

@@ -1,9 +1,24 @@
 from setuptools import find_packages, setup
 import os
 from glob import glob
+import setuptools.command.install
+import ament_virtualenv.install
 
 package_name = "lasr_vision_yolo"
 
+
+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",
+        )
+        return
+
+
 setup(
     name=package_name,
     version="0.0.0",
@@ -23,6 +38,7 @@
     description="YOLO object detection service",
     license="MIT",
     tests_require=["pytest"],
+    cmdclass={"install": InstallCommand},
     entry_points={
         "console_scripts": [
             "yolo_service_node = lasr_vision_yolo.service:main",

skills/src/lasr_skills/detect_3d.py

@@ -91,7 +91,7 @@ def callback(image_msg, depth_msg, cam_info_msg):
         self.ts.registerCallback(callback)
 
         while not self.data:
-            time.sleep(0.1)
+            rclpy.spin_once(self.node, timeout_sec=0.1)
 
         if len(self.data) == 4:
             image_msg, depth_msg, cam_info_msg, pcl_msg = self.data
@@ -100,16 +100,27 @@ def callback(image_msg, depth_msg, cam_info_msg):
             pcl_msg = None
 
         try:
-            resp = self.yolo(
+            request = YoloDetection3D.Request(
                 image_raw=image_msg,
                 depth_image=depth_msg,
                 depth_camera_info=cam_info_msg,
                 model=self.model,
-                models=self.models,
+                # models=self.models,
                 confidence=self.confidence,
                 filter=self.filter,
                 target_frame=self.target_frame,
             )
+            future = self.yolo.call_async(request)
+            rclpy.spin_until_future_complete(self.node, future)
+
+            resp = future.result()
+
+            self.node.get_logger().info(f"Got {len(resp.detected_objects)} detections")
+            for det in resp.detected_objects:
+                self.node.get_logger().info(
+                    f"  {det.name} at ({det.point.x:.2f}, {det.point.y:.2f}, {det.point.z:.2f})"
+                )
+
             userdata.detections_3d = resp
             userdata.image_raw = image_msg
             userdata.pcl = pcl_msg
@@ -122,7 +133,8 @@ def callback(image_msg, depth_msg, cam_info_msg):
 def main():
     rclpy.init()
     node = rclpy.create_node("detect")
-    while not rclpy.ok():
+
+    while rclpy.ok():
         detect = Detect3D(node=node, slop=10.0)
         sm = StateMachine(outcomes=["succeeded", "failed"])
         with sm:
@@ -133,6 +145,9 @@ def main():
             )
         sm.execute()
 
+    node.destroy_node()
+    rclpy.shutdown()
+
 
 if __name__ == "__main__":
     main()

skills/src/lasr_skills/detect_3d_in_area.py

@@ -10,7 +10,7 @@
 
 from std_msgs.msg import Header
 from geometry_msgs.msg import Polygon, Point, Point32, PolygonStamped
-from shapely.geometry import Point
+from shapely.geometry import Point as ShapelyPoint
 from shapely.geometry.polygon import Polygon as ShapelyPolygon
 
 
@@ -72,7 +72,7 @@ def execute(self, userdata):
                 PolygonStamped(polygon=polygon_msg, header=Header(frame_id="map"))
             )
             satisfied_points = [
-                area_polygon.contains(Point(object.point.x, object.point.y))
+                area_polygon.contains(ShapelyPoint(object.point.x, object.point.y))
                 for object in detected_objects
             ]
             filtered_detections = [

skills/src/lasr_skills/detect_all_in_polygon.py

@@ -1,13 +1,15 @@
 #!/usr/bin/env python3
 import rclpy
 from rclpy.node import Node
+from rclpy.wait_for_message import wait_for_message
 
 import smach
 from smach_ros import RosState
 
 # import tf2_ros as tf
 import numpy as np
 import cv2
+from threading import Thread
 
 # from tf_pcl import pcl_transform
 from typing import List, Optional, Tuple
@@ -111,11 +113,13 @@ def euclidean_distance(point1: Point, point2: Point) -> float:
 from rclpy.time import Time
 from rclpy.qos import QoSProfile, QoSDurabilityPolicy
 from rclpy.publisher import Publisher
+from rclpy.executors import MultiThreadedExecutor
 
 import smach
 from smach_ros import RosState
 
 import tf2_ros
+from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_point
 
 # import tf2_geometry_msgs
 import numpy as np
@@ -137,6 +141,7 @@ def euclidean_distance(point1: Point, point2: Point) -> float:
 from sensor_msgs.msg import CameraInfo
 from image_geometry import PinholeCameraModel
 from typing import List, Tuple
+import threading
 
 
 class CalculateSweepPoints(RosState):
@@ -175,9 +180,12 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon:
             ShapelyPolygon: Footprint of camera FOV in map frame.
         """
 
-        camera_info = rclpy.wait_for_message(
-            "/xtion/depth_registered/camera_info", CameraInfo
+        success, camera_info = wait_for_message(
+            CameraInfo, self.node, "/xtion/depth_registered/camera_info"
         )
+        if not success or camera_info is None:
+            self.node.get_logger().warn("Timed out waiting for camera info")
+
         model = PinholeCameraModel()
         model.fromCameraInfo(camera_info)
 
@@ -195,16 +203,25 @@ def _get_camera_fov_polygon(self) -> ShapelyPolygon:
             ray = model.projectPixelTo3dRay((u, v))
             point_cam = PointStamped()
             point_cam.header.frame_id = camera_info.header.frame_id
-            point_cam.header.stamp = Time()
+            point_cam.header.stamp = Time().to_msg()
             point_cam.point.x = ray[0] * self._fov_depth
             point_cam.point.y = ray[1] * self._fov_depth
             point_cam.point.z = ray[2] * self._fov_depth
 
             # Transform to map frame
-            point_map = self._tf_buffer.transform(
-                point_cam, "map", Duration(seconds=1.0)
-            )
-            transformed_points.append((point_map.point.x, point_map.point.y))
+            try:
+                transform = self._tf_buffer.lookup_transform(
+                    "map",
+                    camera_info.header.frame_id,
+                    Time(),
+                    timeout=Duration(seconds=1.0),
+                )
+                point_map = do_transform_point(point_cam, transform)
+                transformed_points.append((point_map.point.x, point_map.point.y))
+            except Exception as e:
+                self.node.get_logger().error(
+                    f"Transform failed with camera timestamp: {e}. Retrying with latest TF data."
+                )
 
         return ShapelyPolygon(transformed_points)
 
@@ -286,7 +303,7 @@ def _calculate_sweep_points(self) -> List[PointStamped]:
         self.node.get_logger().info("Waiting for camera info and TF to map frame...")
         fov_polygon = self._get_camera_fov_polygon()
 
-        # Optional: visualize FOV #TODO: Verify
+        # Optional: visualize FOV
 
         qos = QoSProfile(
             depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
@@ -296,7 +313,9 @@ def _calculate_sweep_points(self) -> List[PointStamped]:
 
         pub.publish(
             PolygonStamped(
-                header=Header(frame_id="map", stamp=self.node.get_clock().now()),
+                header=Header(
+                    frame_id="map", stamp=self.node.get_clock().now().to_msg()
+                ),
                 polygon=ROSPolygon(
                     points=[
                         Point32(x=x, y=y, z=0.0) for x, y in fov_polygon.exterior.coords
@@ -320,7 +339,7 @@ def _calculate_sweep_points(self) -> List[PointStamped]:
         sweep_points = [
             PointStamped(
                 header=Header(frame_id="map"),
-                point=Point(fp.centroid.x, fp.centroid.y, self._z_axis),
+                point=Point(x=fp.centroid.x, y=fp.centroid.y, z=self._z_axis),
             )
             for fp in selected_footprints
         ]
@@ -448,7 +467,7 @@ def _nap(self, userdata) -> str:
         Returns:
             str: Outcome of the state, "succeeded".
         """
-        self._node.get_clock().sleep_for(Duration(seconds=0.25))
+        rclpy.spin_once(self._node, timeout_sec=0.25)
         return "succeeded"
 
     def _publish_detected_objects(self, userdata: smach.UserData) -> str:
@@ -688,6 +707,13 @@ def main():
 
     rclpy.init()
     node = rclpy.create_node("detect_all_in_polygon")
+
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+
+    spin_thread = threading.Thread(target=executor.spin, daemon=True)
+    spin_thread.start()
+
     sm = DetectAllInPolygon(
         node,
         seat_polygon,

skills/src/lasr_skills/look_to_point.py

@@ -25,13 +25,22 @@ def __init__(
         self._pointstamped = pointstamped
 
         self.client = ActionClient(
-            self.node, PointHead, "/head_controller/point_head_action"
+            self.node,
+            PointHead,
+            "/head_controller/point_head_action",  # TODO: Action server doesnt exist
         )
         self.goal_future = None
         self.result_future = None
-        self.node.get_logger().info("Created State")
+        self.node.get_logger().info("LookToPoint - Created State.")
 
-        self.client.wait_for_server()
+        if not self.client.wait_for_server(timeout_sec=1.0):
+            self.node.get_logger().warn(
+                "Head controller PointHead action server not found. Skipping..."
+            )
+        else:
+            self.node.get_logger().info(
+                "Head controller PointHead action server working."
+            )
 
     def execute(self, userdata):
         # Define the goal
@@ -47,16 +56,14 @@ def execute(self, userdata):
         )
 
         # Send the goal
-        self.node.get_logger().info("Sending goal")
-        future = self.client.send_goal_async(
-            goal
-        )  # can't call send_goal in cb because of deadlock
-        rclpy.spin_until_future_complete(self, future)
+        self.node.get_logger().info("Sending goal - PointHead")
+        future = self.client.send_goal_async(goal)
+        rclpy.spin_until_future_complete(self.node, future)
 
         # Wait for the result with a timeout of 2 seconds
         goal_handle = future.result()
         result_future = goal_handle.get_result_async()
-        rclpy.spin_until_future_complete(self, future, timeout_sec=2.0)
+        rclpy.spin_until_future_complete(self.node, result_future, timeout_sec=2.0)
 
         if result_future.done():
             state = result_future.result()