Spring clean 01 hotfix + slight cleanup (LASR-at-Home#141)

* fix/refactor: tf and markers. * chore: use python3.8 to get PyKDL to work. * refactor: reinvent GoTo* states to be SimpleActionStates. * fix: use shapely directly, rather than legacy (deprecated) ROS package Co-authored by: Paul Makles <[email protected]> * chore: add markers dependancy for yolo. * refactor: LookToPoint. * refactor: cleanup. * fix/refactor: filter for people and general cleanup.
jws-1 · Mar 8, 2024 · 6b9f223 · 6b9f223
1 parent b4aae40
commit 6b9f223
Show file tree

Hide file tree

Showing 11 changed files with 133 additions and 182 deletions.
diff --git a/common/vision/lasr_vision_yolov8/CMakeLists.txt b/common/vision/lasr_vision_yolov8/CMakeLists.txt
@@ -19,7 +19,7 @@ find_package(catkin REQUIRED catkin_virtualenv)
 catkin_python_setup()
 catkin_generate_virtualenv(
   INPUT_REQUIREMENTS requirements.in
-  PYTHON_INTERPRETER python3.10
+  PYTHON_INTERPRETER python3.8
 )
 
 ################################################

diff --git a/common/vision/lasr_vision_yolov8/package.xml b/common/vision/lasr_vision_yolov8/package.xml
@@ -51,7 +51,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>catkin_virtualenv</build_depend>
   <depend>lasr_vision_msgs</depend>
-
+  <exec_depend>markers</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

diff --git a/common/vision/lasr_vision_yolov8/requirements.txt b/common/vision/lasr_vision_yolov8/requirements.txt
@@ -1,55 +1,53 @@
-certifi==2023.7.22        # via requests
-charset-normalizer==3.2.0  # via requests
-cmake==3.27.4.1           # via triton
-contourpy==1.1.0          # via matplotlib
-cycler==0.11.0            # via matplotlib
+certifi==2024.2.2         # via requests
+charset-normalizer==3.3.2  # via requests
+contourpy==1.1.1          # via matplotlib
+cycler==0.12.1            # via matplotlib
 dill==0.3.7               # via -r requirements.in
-filelock==3.12.3          # via torch, triton
-fonttools==4.42.1         # via matplotlib
-idna==3.4                 # via requests
-jinja2==3.1.2             # via torch
+filelock==3.13.1          # via torch, triton
+fonttools==4.49.0         # via matplotlib
+fsspec==2024.2.0          # via torch
+idna==3.6                 # via requests
+importlib-resources==6.1.2  # via matplotlib
+jinja2==3.1.3             # via torch
 kiwisolver==1.4.5         # via matplotlib
-lit==16.0.6               # via triton
-markupsafe==2.1.3         # via jinja2
-matplotlib==3.7.3         # via seaborn, ultralytics
+markupsafe==2.1.5         # via jinja2
+matplotlib==3.7.5         # via seaborn, ultralytics
 mpmath==1.3.0             # via sympy
 networkx==3.1             # via torch
-numpy==1.25.2             # via contourpy, matplotlib, opencv-python, pandas, scipy, seaborn, torchvision, ultralytics
-nvidia-cublas-cu11==11.10.3.66  # via nvidia-cudnn-cu11, nvidia-cusolver-cu11, torch
-nvidia-cuda-cupti-cu11==11.7.101  # via torch
-nvidia-cuda-nvrtc-cu11==11.7.99  # via torch
-nvidia-cuda-runtime-cu11==11.7.99  # via torch
-nvidia-cudnn-cu11==8.5.0.96  # via torch
-nvidia-cufft-cu11==10.9.0.58  # via torch
-nvidia-curand-cu11==10.2.10.91  # via torch
-nvidia-cusolver-cu11==11.4.0.1  # via torch
-nvidia-cusparse-cu11==11.7.4.91  # via torch
-nvidia-nccl-cu11==2.14.3  # via torch
-nvidia-nvtx-cu11==11.7.91  # via torch
-opencv-python==4.8.0.76   # via ultralytics
-packaging==23.1           # via matplotlib
-pandas==2.1.0             # via seaborn, ultralytics
-pillow==10.0.0            # via matplotlib, torchvision, ultralytics
-psutil==5.9.5             # via ultralytics
+numpy==1.24.4             # via contourpy, matplotlib, opencv-python, pandas, scipy, seaborn, torchvision, ultralytics
+nvidia-cublas-cu12==12.1.3.1  # via nvidia-cudnn-cu12, nvidia-cusolver-cu12, torch
+nvidia-cuda-cupti-cu12==12.1.105  # via torch
+nvidia-cuda-nvrtc-cu12==12.1.105  # via torch
+nvidia-cuda-runtime-cu12==12.1.105  # via torch
+nvidia-cudnn-cu12==8.9.2.26  # via torch
+nvidia-cufft-cu12==11.0.2.54  # via torch
+nvidia-curand-cu12==10.3.2.106  # via torch
+nvidia-cusolver-cu12==11.4.5.107  # via torch
+nvidia-cusparse-cu12==12.1.0.106  # via nvidia-cusolver-cu12, torch
+nvidia-nccl-cu12==2.19.3  # via torch
+nvidia-nvjitlink-cu12==12.4.99  # via nvidia-cusolver-cu12, nvidia-cusparse-cu12
+nvidia-nvtx-cu12==12.1.105  # via torch
+opencv-python==4.9.0.80   # via ultralytics
+packaging==23.2           # via matplotlib
+pandas==2.0.3             # via seaborn, ultralytics
+pillow==10.2.0            # via matplotlib, torchvision, ultralytics
+psutil==5.9.8             # via ultralytics
 py-cpuinfo==9.0.0         # via ultralytics
-pyparsing==3.1.1          # via matplotlib
-python-dateutil==2.8.2    # via matplotlib, pandas
-pytz==2023.3.post1        # via pandas
+pyparsing==3.1.2          # via matplotlib
+python-dateutil==2.9.0.post0  # via matplotlib, pandas
+pytz==2024.1              # via pandas
 pyyaml==6.0.1             # via ultralytics
-requests==2.31.0          # via torchvision, ultralytics
-scipy==1.11.2             # via ultralytics
-seaborn==0.12.2           # via ultralytics
+requests==2.31.0          # via ultralytics
+scipy==1.10.1             # via ultralytics
+seaborn==0.13.2           # via ultralytics
 six==1.16.0               # via python-dateutil
 sympy==1.12               # via torch
-torch==2.0.1              # via torchvision, triton, ultralytics
-torchvision==0.15.2       # via ultralytics
-tqdm==4.66.1              # via ultralytics
-triton==2.0.0             # via torch
-typing-extensions==4.7.1  # via filelock, torch
-tzdata==2023.3            # via pandas
+torch==2.2.1              # via torchvision, ultralytics
+torchvision==0.17.1       # via ultralytics
+tqdm==4.66.2              # via ultralytics
+triton==2.2.0             # via torch
+typing-extensions==4.10.0  # via torch
+tzdata==2024.1            # via pandas
 ultralytics==8.0.168      # via -r requirements.in
-urllib3==2.0.4            # via requests
-wheel==0.41.2             # via nvidia-cublas-cu11, nvidia-cuda-cupti-cu11, nvidia-cuda-runtime-cu11, nvidia-curand-cu11, nvidia-cusparse-cu11, nvidia-nvtx-cu11
-
-# The following packages are considered to be unsafe in a requirements file:
-# setuptools
+urllib3==2.2.1            # via requests
+zipp==3.17.0              # via importlib-resources
diff --git a/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py b/common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py
@@ -3,47 +3,22 @@
 import cv2_pcl
 import numpy as np
 
-from PIL import Image
 from ultralytics import YOLO
 
-from sensor_msgs.msg import Image as SensorImage
 from lasr_vision_msgs.msg import Detection, Detection3D
 from lasr_vision_msgs.srv import (
     YoloDetectionRequest,
     YoloDetectionResponse,
     YoloDetection3DRequest,
     YoloDetection3DResponse,
 )
-import cv2
-import ros_numpy as rnp
+
+import markers
 
 from geometry_msgs.msg import Point, PointStamped
 
 import tf2_ros as tf
-import tf2_geometry_msgs
-
-from visualization_msgs.msg import Marker
-
-
-def create_point_marker(x, y, z, idx, g=1.0):
-    marker_msg = Marker()
-    marker_msg.header.frame_id = "map"
-    marker_msg.header.stamp = rospy.Time.now()
-    marker_msg.id = idx
-    marker_msg.type = Marker.SPHERE
-    marker_msg.action = Marker.ADD
-    marker_msg.pose.position.x = x
-    marker_msg.pose.position.y = y
-    marker_msg.pose.position.z = z
-    marker_msg.pose.orientation.w = 1.0
-    marker_msg.scale.x = 0.1
-    marker_msg.scale.y = 0.1
-    marker_msg.scale.z = 0.1
-    marker_msg.color.a = 1.0
-    marker_msg.color.r = 0.0
-    marker_msg.color.g = g
-    marker_msg.color.b = 0.0
-    return marker_msg
+import tf2_geometry_msgs  # noqa
 
 
 # model cache
@@ -121,14 +96,7 @@ def detect_3d(
     """
     Run YOLO 3D inference on given detection request
     """
-
-    rospy.loginfo("Waiting for transformation to become available")
     tf_buffer = tf.Buffer()
-    # Wait for the transformation to become available
-    while not tf_buffer.can_transform(
-        "map", request.pcl.header.frame_id, rospy.Time(0)
-    ):
-        rospy.sleep(0.01)
 
     # Extract rgb image from pointcloud
     rospy.loginfo("Decoding")
@@ -154,26 +122,31 @@ def detect_3d(
         detection.confidence = float(result.boxes.conf.cpu().numpy()[i])
         detection.xywh = result.boxes.xywh[i].cpu().numpy().astype(int).tolist()
 
-        # copy segmented mask if available
+        # copy segmented mask if available, and estimate 3D position
         if has_segment_masks:
             detection.xyseg = result.masks.xy[i].flatten().astype(int).tolist()
 
-            # xyz_points = [pcl_xyz[x][y] for x, y in indices]
             centroid = cv2_pcl.seg_to_centroid(request.pcl, np.array(detection.xyseg))
 
             point = Point(*centroid)
             point_stamped = PointStamped()
             point_stamped.point = point
             point_stamped.header.frame_id = request.pcl.header.frame_id
-            detection.point = tf_buffer.transform(
-                point_stamped, request.target_frame
-            ).point
+            point_stamped.header.stamp = rospy.Time(0)
+
+            # TODO: handle tf errors properly
+            while not rospy.is_shutdown():
+                try:
+                    detection.point = tf_buffer.transform(point_stamped, "map").point
+                    break
+                except Exception:
+                    continue
 
+            # publish to debug topic
             if debug_point_publisher is not None:
-                marker = create_point_marker(
-                    detection.point.x, detection.point.y, detection.point.z, i
+                markers.create_and_publish_marker(
+                    debug_point_publisher, detection.point
                 )
-                debug_point_publisher.publish(marker)
 
         detected_objects.append(detection)
 

diff --git a/skills/src/lasr_skills/detect_3d.py b/skills/src/lasr_skills/detect_3d.py
@@ -1,4 +1,3 @@
-#!/usr/bin/env python3
 import rospy
 import smach
 

diff --git a/skills/src/lasr_skills/detect_3d_in_area.py b/skills/src/lasr_skills/detect_3d_in_area.py
@@ -1,45 +1,43 @@
-#!/usr/bin/env python3
-
 import smach
 
 from lasr_skills import Detect3D
-from lasr_shapely import LasrShapely
-
 
 from typing import List
 
+from shapely.geometry import Point
+from shapely.geometry.polygon import Polygon
+
 
 class Detect3DInArea(smach.StateMachine):
 
     class FilterDetections(smach.State):
 
-        def __init__(self, area_polygon):
+        def __init__(self, area_polygon: Polygon):
             smach.State.__init__(
                 self,
                 outcomes=["succeeded", "failed"],
                 input_keys=["detections_3d"],
                 output_keys=["detections_3d"],
             )
             self.area_polygon = area_polygon
-            self.shapely = LasrShapely()
 
         def execute(self, userdata):
-            satisfied_points = self.shapely.are_points_in_polygon_2d(
-                userdata.area_polygon,
-                [[pose[0], pose[1]] for (_, pose) in userdata.detections_3d],
-            ).inside
+            satisfied_points = [
+                self.area_polygon.contains(Point(pose[0], pose[1]))
+                for (_, pose) in userdata.detections_3d
+            ]
             filtered_detections = [
                 userdata.detections_3d[i]
                 for i in range(0, len(userdata.detections_3d))
                 if satisfied_points[i]
             ]
-            userdata.detections_3d = filtered_detections
 
+            userdata.detections_3d = filtered_detections
             return "succeeded"
 
     def __init__(
         self,
-        area_polygon: List[List[float]],
+        area_polygon: Polygon,
         depth_topic: str = "/xtion/depth_registered/points",
         model: str = "yolov8n-seg.pt",
         filter: List[str] | None = None,

diff --git a/skills/src/lasr_skills/go_to_location.py b/skills/src/lasr_skills/go_to_location.py
@@ -1,31 +1,18 @@
-#!/usr/bin/env python3
+import smach_ros
 
-import rospy
-import smach
-from tiago_controllers.controllers import Controllers
-from geometry_msgs.msg import Point, Quaternion, Pose
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Header
 
-class GoToLocation(smach.State):
 
-    def __init__(self):
-        smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location'])
-        self.controllers = Controllers()
-
-    def execute(self, userdata):
-        try:
-            status = self.controllers.base_controller.sync_to_pose(userdata.location)
-            if status:
-                return 'succeeded'
-            return 'failed'
-        except rospy.ERROR as e:
-            rospy.logwarn(f"Unable to go to location. {userdata.location} -> ({str(e)})")
-            return 'failed'
+class GoToLocation(smach_ros.SimpleActionState):
 
-if __name__ == '__main__':
-    rospy.init_node('go_to_location')
-    sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
-    loc = rospy.get_param('/living_room/table/location')
-    sm.userdata.location = Pose(position=Point(**loc['position']), orientation=Quaternion(**loc['orientation']))
-    with sm:
-        smach.StateMachine.add('GoToLocation', GoToLocation(), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
-    sm.execute()
+    def __init__(self):
+        super(GoToLocation, self).__init__(
+            "move_base",
+            MoveBaseAction,
+            goal_cb=lambda ud, _: MoveBaseGoal(
+                target_pose=PoseStamped(pose=ud.location, header=Header(frame_id="map"))
+            ),
+            input_keys=["location"],
+        )
diff --git a/skills/src/lasr_skills/go_to_semantic_location.py b/skills/src/lasr_skills/go_to_semantic_location.py
@@ -1,30 +1,28 @@
-#!/usr/bin/env python3
-
 import rospy
-import smach
-from tiago_controllers.controllers import Controllers
-from geometry_msgs.msg import Point, Quaternion, Pose
+import smach_ros
+
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+from std_msgs.msg import Header
+
 
-class GoToSemanticLocation(smach.State):
+class GoToSemanticLocation(smach_ros.SimpleActionState):
 
     def __init__(self):
-        smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location'])
-        self.controllers = Controllers()
+        super(GoToSemanticLocation, self).__init__(
+            "move_base",
+            MoveBaseAction,
+            goal_cb=lambda ud, _: MoveBaseGoal(
+                target_pose=PoseStamped(
+                    pose=self._to_pose(rospy.get_param(f"{ud.location}/location")),
+                    header=Header(frame_id="map"),
+                )
+            ),
+            input_keys=["location"],
+        )
 
-    def execute(self, userdata):
-        loc = rospy.get_param(f"{userdata.location}/location")
-        try:
-            status = self.controllers.base_controller.sync_to_pose(Pose(position=Point(**loc['position']), orientation=Quaternion(**loc['orientation'])))
-            if status:
-                return 'succeeded'
-            return 'failed'
-        except rospy.ERROR as e:
-            rospy.logwarn(f"Unable to go to location. {loc} -> ({str(e)})")
-            return 'failed'
-if __name__ == '__main__':
-    rospy.init_node('go_to_semantic_location')
-    sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
-    sm.userdata.location = '/living_room/table'
-    with sm:
-        smach.StateMachine.add('GoToSemanticLocation', GoToSemanticLocation(), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
-    sm.execute()
+    def _to_pose(self, location):
+        return Pose(
+            position=Point(**location["position"]),
+            orientation=Quaternion(**location["orientation"]),
+        )