Bodypix update

common/navigation/lasr_person_following/src/lasr_person_following/person_following.py

@@ -124,7 +124,7 @@ def __init__(
         if not self._transcribe_speech_client_available:
             rospy.logwarn("Transcribe speech client not available")
 
-        self._detect_wave = rospy.ServiceProxy("/detect_wave", DetectWave)
+        self._detect_wave = rospy.ServiceProxy("/bodypix/detect_wave", DetectWave)
         if not self._detect_wave.wait_for_service(rospy.Duration.from_sec(10.0)):
             rospy.logwarn("Detect wave service not available")
 

common/vision/lasr_vision_bodypix/CMakeLists.txt

@@ -160,8 +160,7 @@ include_directories(
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-  nodes/mask_service.py
-  nodes/keypoint_service.py
+  nodes/bodypix_services.py
   examples/mask_relay.py
   examples/keypoint_relay.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

common/vision/lasr_vision_bodypix/launch/bodypix.launch

@@ -0,0 +1,13 @@
+<launch>
+    <description>Start BodyPix services</description>
+    <usage doc="BodyPix service"></usage>
+    <usage doc="Preload models and enable debug topic">debug:=true preload:=['resnet50', 'mobilenet50']</usage>
+
+    <arg name="preload" default="['resnet50']" doc="Array of models to preload when starting the service" />
+
+
+    <node name="bodypix_services" pkg="lasr_vision_bodypix" type="bodypix_services.py" output="screen">
+        <param name="preload" type="yaml" value="$(arg preload)" />
+    </node>
+
+</launch>

common/vision/lasr_vision_bodypix/launch/camera_keypoint.launch

@@ -7,8 +7,7 @@
     <arg name="model" default="resnet50" doc="Model to use for the demo" />
 
     <!-- BodyPix service -->
-    <include file="$(find lasr_vision_bodypix)/launch/keypoint_service.launch">
-        <arg name="debug" value="true" />
+    <include file="$(find lasr_vision_bodypix)/launch/bodypix.launch">
         <arg name="preload" value="['$(arg model)']" />
     </include>
 

common/vision/lasr_vision_bodypix/launch/camera_mask.launch

@@ -7,11 +7,10 @@
     <arg name="model" default="resnet50" doc="Model to use for the demo" />
 
     <!-- BodyPix service -->
-    <include file="$(find lasr_vision_bodypix)/launch/mask_service.launch">
-        <arg name="debug" value="true" />
+    <include file="$(find lasr_vision_bodypix)/launch/bodypix.launch">
         <arg name="preload" value="['$(arg model)']" />
     </include>
-    
+
     <!-- show debug topic -->
     <node name="image_view" pkg="rqt_image_view" type="rqt_image_view" respawn="false" output="screen" args="/bodypix/debug/$(arg model)" />
 
@@ -22,4 +21,5 @@
    <include file="$(find video_stream_opencv)/launch/camera.launch">
         <arg name="visualize" value="true" />
    </include>
+
 </launch>

common/vision/lasr_vision_bodypix/nodes/gesture_service.py → common/vision/lasr_vision_bodypix/nodes/bodypix_services.py

@@ -1,18 +1,10 @@
-#!/usr/bin/env python3.9
-
+#!/usr/bin/env python3
 import rospy
-from typing import List, Union
-from sensor_msgs.msg import Image
 import lasr_vision_bodypix as bodypix
-import cv2
-import cv2_img
-import ros_numpy as rnp
-from geometry_msgs.msg import PointStamped, Point
-from visualization_msgs.msg import Marker
-from markers import create_and_publish_marker
-from cv2_pcl import pcl_to_img_msg
-
 from lasr_vision_msgs.srv import (
+    BodyPixMaskDetection,
+    BodyPixMaskDetectionRequest,
+    BodyPixMaskDetectionResponse,
     BodyPixKeypointDetection,
     BodyPixKeypointDetectionRequest,
     BodyPixKeypointDetectionResponse,
@@ -21,13 +13,38 @@
     DetectWaveResponse,
 )
 
+from typing import Union
+from sensor_msgs.msg import Image
+import ros_numpy as rnp
+from geometry_msgs.msg import PointStamped, Point
 from std_msgs.msg import Header
 import numpy as np
+from cv2_pcl import pcl_to_img_msg
 
-rospy.init_node("detect_wave_service")
+# Initialise rospy
+rospy.init_node("bodypix_mask_service")
 
-DEBUG = rospy.get_param("~debug", True)
-marker_pub = rospy.Publisher("waving_person", Marker, queue_size=1)
+# Determine variables
+PRELOAD = rospy.get_param("~preload", [])  # List of models to preload
+
+for model in PRELOAD:
+    bodypix.load_model_cached(model)
+
+
+def detect_masks(request: BodyPixMaskDetectionRequest) -> BodyPixMaskDetectionResponse:
+    """
+    Hand off detection request to bodypix library
+    """
+    return bodypix.detect_masks(request)
+
+
+def detect_keypoints(
+    request: BodyPixKeypointDetectionRequest,
+) -> BodyPixKeypointDetectionResponse:
+    """
+    Hand off detection request to bodypix library
+    """
+    return bodypix.detect_keypoints(request)
 
 
 def detect_wave(
@@ -126,7 +143,8 @@ def detect_wave(
     )
 
 
-# rospy.Service("/detect_wave", DetectWave, lambda req: detect_wave(req, rospy.Publisher("debug_waving", Image, queue_size=1)))
-rospy.Service("/detect_wave", DetectWave, detect_wave)
-rospy.loginfo("Detect wave service started")
+rospy.Service("/bodypix/mask_detection", BodyPixMaskDetection, detect_masks)
+rospy.Service("/bodypix/keypoint_detection", BodyPixKeypointDetection, detect_keypoints)
+rospy.Service("/bodypix/detect_wave", DetectWave, detect_wave)
+rospy.loginfo("BodyPix service started")
 rospy.spin()

common/vision/lasr_vision_bodypix/requirements.in

@@ -1,8 +1,7 @@
 tf-bodypix==0.4.2
-tensorflow==2.14.0
 opencv-python==4.8.1.78
 Pillow==10.1.0
 matplotlib==3.8.1
 
 # The following was manually added and freezed into requirements.txt:
-# tfjs-graph-converter==1.6.3
+tfjs-graph-converter==1.6.3

common/vision/lasr_vision_bodypix/requirements.txt

@@ -0,0 +1,77 @@
+absl-py==2.1.0            # via chex, keras, optax, orbax-checkpoint, tensorboard, tensorflow, tensorflow-decision-forests, ydf
+astunparse==1.6.3         # via tensorflow
+certifi==2024.7.4         # via requests
+charset-normalizer==3.3.2  # via requests
+chex==0.1.86              # via optax
+contourpy==1.2.1          # via matplotlib
+cycler==0.12.1            # via matplotlib
+etils[epath,epy]==1.5.2   # via orbax-checkpoint
+flatbuffers==24.3.25      # via tensorflow
+flax==0.8.5               # via tensorflowjs
+fonttools==4.53.1         # via matplotlib
+fsspec==2024.6.1          # via etils
+gast==0.6.0               # via tensorflow
+google-pasta==0.2.0       # via tensorflow
+grpcio==1.64.1            # via tensorboard, tensorflow
+h5py==3.11.0              # via keras, tensorflow
+idna==3.7                 # via requests
+importlib-metadata==8.0.0  # via jax, markdown
+importlib-resources==6.4.0  # via etils, matplotlib, tensorflowjs
+jax==0.4.30               # via chex, flax, optax, orbax-checkpoint, tensorflowjs
+jaxlib==0.4.30            # via chex, jax, optax, orbax-checkpoint, tensorflowjs
+keras==3.4.1              # via tensorflow
+kiwisolver==1.4.5         # via matplotlib
+libclang==18.1.1          # via tensorflow
+markdown==3.6             # via tensorboard
+markdown-it-py==3.0.0     # via rich
+markupsafe==2.1.5         # via werkzeug
+matplotlib==3.8.1         # via -r requirements.in
+mdurl==0.1.2              # via markdown-it-py
+ml-dtypes==0.3.2          # via jax, jaxlib, keras, tensorflow, tensorstore
+msgpack==1.0.8            # via flax, orbax-checkpoint
+namex==0.0.8              # via keras
+nest-asyncio==1.6.0       # via orbax-checkpoint
+numpy==1.26.4             # via chex, contourpy, flax, h5py, jax, jaxlib, keras, matplotlib, ml-dtypes, opencv-python, opt-einsum, optax, orbax-checkpoint, pandas, scipy, tensorboard, tensorflow, tensorflow-decision-forests, tensorflow-hub, tensorstore, ydf
+opencv-python==4.8.1.78   # via -r requirements.in
+opt-einsum==3.3.0         # via jax, tensorflow
+optax==0.2.2              # via flax
+optree==0.12.1            # via keras
+orbax-checkpoint==0.5.20  # via flax
+packaging==23.2           # via keras, matplotlib, tensorflow, tensorflowjs
+pandas==2.2.2             # via tensorflow-decision-forests
+pillow==10.1.0            # via -r requirements.in, matplotlib
+protobuf==4.25.3          # via orbax-checkpoint, tensorboard, tensorflow, tensorflow-hub, ydf
+pygments==2.18.0          # via rich
+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 flax, orbax-checkpoint
+requests==2.32.3          # via tensorflow, tf-bodypix
+rich==13.7.1              # via flax, keras
+scipy==1.13.1             # via jax, jaxlib
+six==1.16.0               # via astunparse, google-pasta, python-dateutil, tensorboard, tensorflow, tensorflow-decision-forests, tensorflowjs
+tensorboard==2.16.2       # via tensorflow
+tensorboard-data-server==0.7.2  # via tensorboard
+tensorflow==2.16.2        # via tensorflow-decision-forests, tensorflowjs, tf-keras
+tensorflow-decision-forests==1.9.1  # via tensorflowjs
+tensorflow-hub==0.16.1    # via tensorflowjs
+tensorflow-io-gcs-filesystem==0.37.1  # via tensorflow
+tensorflowjs==4.20.0      # via tfjs-graph-converter
+tensorstore==0.1.63       # via flax, orbax-checkpoint
+termcolor==2.4.0          # via tensorflow
+tf-bodypix==0.4.2         # via -r requirements.in
+tf-keras==2.16.0          # via tensorflow-decision-forests, tensorflow-hub, tensorflowjs
+tfjs-graph-converter==1.6.3  # via -r requirements.in
+toolz==0.12.1             # via chex
+typing-extensions==4.12.2  # via chex, etils, flax, optree, orbax-checkpoint, tensorflow
+tzdata==2024.1            # via pandas
+urllib3==2.2.2            # via requests
+werkzeug==3.0.3           # via tensorboard
+wheel==0.43.0             # via astunparse, tensorflow-decision-forests
+wrapt==1.16.0             # via tensorflow
+wurlitzer==3.1.1          # via tensorflow-decision-forests
+ydf==0.5.0                # via tensorflow-decision-forests
+zipp==3.19.2              # via etils, importlib-metadata, importlib-resources
+
+# The following packages are considered to be unsafe in a requirements file:
+# setuptools

common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py

@@ -24,11 +24,7 @@
 import rospkg
 
 # model cache
-# preload resnet 50 model so that it won't waste the time
-# doing that in the middle of the task.
-loaded_models = {
-    "resnet50": load_model(download_model(BodyPixModelPaths.RESNET50_FLOAT_STRIDE_16))
-}
+loaded_models = {}
 r = rospkg.RosPack()
 
 
@@ -47,20 +43,27 @@ def load_model_cached(dataset: str):
     """
     model = None
     if dataset in loaded_models:
+        rospy.loginfo(f"Using cached {dataset} model")
         model = loaded_models[dataset]
     else:
         if dataset == "resnet50":
+            rospy.loginfo("Downloading resnet50 model")
             name = download_model(BodyPixModelPaths.RESNET50_FLOAT_STRIDE_16)
+            rospy.loginfo("Loading resnet50 model")
             model = load_model(name)
         elif dataset == "mobilenet50":
+            rospy.loginfo("Downloading mobilenet50 model")
             name = download_model(BodyPixModelPaths.MOBILENET_FLOAT_50_STRIDE_8)
+            rospy.loginfo("Loading mobilenet50 model")
             model = load_model(name)
         elif dataset == "mobilenet100":
+            rospy.loginfo("Downloading mobilenet100 model")
             name = download_model(BodyPixModelPaths.MOBILENET_FLOAT_100_STRIDE_8)
+            rospy.loginfo("Loading mobilenet100 model")
             model = load_model(name)
         else:
             model = load_model(dataset)
-        rospy.loginfo(f"Loaded {dataset} model")
+        rospy.loginfo(f"Loaded {dataset} model into cache")
         loaded_models[dataset] = model
     return model
 

skills/launch/unit_test_describe_people.launch

@@ -4,15 +4,9 @@
         <arg name="preload" value="['yolov8n-seg.pt']" />
     </include>
 
-    <node name="bodypix_keypoint_service" pkg="lasr_vision_bodypix" type="keypoint_service.py" output="screen">
-        <param name="debug" type="bool" value="true" />
+    <include file="$(find lasr_vision_bodypix)/launch/bodypix_services.launch">
         <param name="preload" type="yaml" value='resnet50' />
-    </node>
-
-    <node name="bodypix_mask_service" pkg="lasr_vision_bodypix" type="mask_service.py" output="screen">
-        <param name="debug" type="bool" value="true" />
-        <param name="preload" type="yaml" value='resnet50' />
-    </node>
+    </include>
 
     <node pkg="lasr_vision_feature_extraction" type="service" name="torch_service" output="screen"/>