From 62c5f7465ef38716e29c2ed79dc2e7974693652b Mon Sep 17 00:00:00 2001 From: MBT <59250708+MBTMBTMBT@users.noreply.github.com> Date: Tue, 23 Apr 2024 22:15:05 +0100 Subject: [PATCH] feat/refactor: feature extraction. (#159) * testtesttest * Test * Test * Message Change * fixed message writting and reading * fixed image crop * Updated the feature extraction node and changed the messages. * fixed that torsal and head frames were inversed. * Changed colour format from BGR to RGB within the detection process. * keep the saved model * keep the model file * keep the saved model * Cancel saving the images (but sitll cannot see use cv2.imshow) * Runnable demo * added the hair colour distribution matching method * retrained model is very robust so changed the threshold * Moving the head to meet the person. * xyz axis readable. * (Hopefully) Runnable with 3d input. * Speak normally. * Try to move the head. * testtesttest * Test * Test * Message Change * fixed message writting and reading * fixed image crop * Updated the feature extraction node and changed the messages. * fixed that torsal and head frames were inversed. * Changed colour format from BGR to RGB within the detection process. * keep the saved model * keep the model file * keep the saved model * Cancel saving the images (but sitll cannot see use cv2.imshow) * Runnable demo * added the hair colour distribution matching method * retrained model is very robust so changed the threshold * Moving the head to meet the person. * xyz axis readable. * (Hopefully) Runnable with 3d input. * ah * At least the head moves, not looking at me though. * Cleaned the file to have only one model appear. * Replace the old model with the new one. * correct the lost module. * info update * fixed issues in the service * fixed a stupic typo * runnable version for full demo * Recover the state machine for demo. * Added a simple loop to refresh the frame taken, should work fine. * Cleaned some commented code. * removed loading the pretrained parameters * removed load pretrained parameter. * renamed torch_module into feature_extractor (Recompile needed!!!) * renamed torch_module into feature_extractor * renamed lasr_vision_torch to lasr_vision_feature_extraction * removed some unused code comments * removed colour estimation module * removed colour_estimation dependence * cleaned usused comments * cleaned comments * renamed torch_module to feature_extractor * removed unused import, launch file and functions. * reset * Remade to achieve easier bodipix model loading * added a break in the loop * I don't really understand why this is in my branch, please ignore this commit when merge. * Replace string return with json string return. * pcl functioned remoeved because appeared repetitively. * replaced the model and predictor initialization, put "__main__" * Merged model classes file into predictor's file * Merged helper functions into perdictor's file. * Deleted feature extractor module * Cleaned load model method, restart to use downloaded model. * Removed unused files and cleaned the files. * Cleaned usless comments and refilled package description. * Removed useless colour messages. * Brought aruco service package back. * Removed useless keys from state machine. * changed log messages. * Fixed a stupid naming issue of feature extractor. * Update common/helpers/numpy2message/package.xml Co-authored-by: Jared Swift * Update common/helpers/numpy2message/package.xml Co-authored-by: Jared Swift * Update common/vision/lasr_vision_feature_extraction/package.xml Co-authored-by: Jared Swift * Canceled the default neck coordinates and leave it to be a failure. * Canceled the loop of getting mixed images, and renamed the keys. * renamed the function. * Update skills/src/lasr_skills/vision/get_image.py Co-authored-by: Jared Swift * Update skills/src/lasr_skills/vision/get_image.py Co-authored-by: Jared Swift * Update skills/src/lasr_skills/vision/image_msg_to_cv2.py Co-authored-by: Jared Swift * Update the new names in init. * removed a print and rename the imports. --------- Co-authored-by: Benteng Ma Co-authored-by: Jared Swift --- .../helpers/colour_estimation/CMakeLists.txt | 202 ---------- common/helpers/colour_estimation/README.md | 63 ---- .../helpers/colour_estimation/doc/EXAMPLE.md | 10 - .../colour_estimation/doc/PREREQUISITES.md | 1 - common/helpers/colour_estimation/doc/USAGE.md | 12 - common/helpers/colour_estimation/setup.py | 11 - .../src/colour_estimation/__init__.py | 20 - .../src/colour_estimation/rgb.py | 68 ---- .../CMakeLists.txt | 10 +- common/helpers/numpy2message/doc/usage.md | 1 + .../package.xml | 10 +- .../{torch_module => numpy2message}/setup.py | 2 +- .../src/numpy2message/__init__.py | 19 + .../helpers/torch_module/doc/PREREQUISITES.md | 1 - common/helpers/torch_module/package.xml | 59 --- .../torch_module/src/torch_module/__init__.py | 0 .../src/torch_module/helpers/__init__.py | 78 ---- .../src/torch_module/modules/__init__.py | 131 ------- .../.gitignore | 0 .../src/lasr_vision_bodypix/bodypix.py | 47 ++- .../lasr_vision_feature_extraction/.gitignore | 2 + .../CMakeLists.txt | 10 +- .../launch/service.launch | 2 +- .../models/.gitkeep | 0 .../nodes/service | 38 ++ .../package.xml | 10 +- .../requirements.in | 0 .../requirements.txt | 0 .../setup.py | 2 +- .../__init__.py | 345 ++++++++++++++++++ .../categories_and_attributes.py | 62 ++++ .../image_with_masks_and_attributes.py | 161 ++++++++ common/vision/lasr_vision_msgs/CMakeLists.txt | 4 +- .../lasr_vision_msgs/msg/BodyPixPose.msg | 3 +- .../lasr_vision_msgs/msg/ColourPrediction.msg | 7 - .../msg/FeatureWithColour.msg | 5 - .../srv/TorchFaceFeatureDetection.srv | 12 +- .../TorchFaceFeatureDetectionDescription.srv | 14 + common/vision/lasr_vision_torch/nodes/service | 70 ---- .../src/lasr_vision_torch/__init__.py | 21 -- .../launch/unit_test_describe_people.launch | 2 +- skills/scripts/unit_test_describe_people.py | 2 +- skills/src/lasr_skills/describe_people.py | 81 ++-- skills/src/lasr_skills/vision/__init__.py | 4 +- skills/src/lasr_skills/vision/get_image.py | 54 ++- .../lasr_skills/vision/image_msg_to_cv2.py | 16 + tasks/coffee_shop/config/config:=full.yaml | 108 ------ tasks/receptionist/launch/setup.launch | 3 + 48 files changed, 821 insertions(+), 962 deletions(-) delete mode 100644 common/helpers/colour_estimation/CMakeLists.txt delete mode 100644 common/helpers/colour_estimation/README.md delete mode 100644 common/helpers/colour_estimation/doc/EXAMPLE.md delete mode 100644 common/helpers/colour_estimation/doc/PREREQUISITES.md delete mode 100644 common/helpers/colour_estimation/doc/USAGE.md delete mode 100644 common/helpers/colour_estimation/setup.py delete mode 100644 common/helpers/colour_estimation/src/colour_estimation/__init__.py delete mode 100644 common/helpers/colour_estimation/src/colour_estimation/rgb.py rename common/helpers/{torch_module => numpy2message}/CMakeLists.txt (96%) create mode 100644 common/helpers/numpy2message/doc/usage.md rename common/helpers/{colour_estimation => numpy2message}/package.xml (87%) rename common/helpers/{torch_module => numpy2message}/setup.py (86%) create mode 100644 common/helpers/numpy2message/src/numpy2message/__init__.py delete mode 100644 common/helpers/torch_module/doc/PREREQUISITES.md delete mode 100644 common/helpers/torch_module/package.xml delete mode 100644 common/helpers/torch_module/src/torch_module/__init__.py delete mode 100644 common/helpers/torch_module/src/torch_module/helpers/__init__.py delete mode 100644 common/helpers/torch_module/src/torch_module/modules/__init__.py rename common/vision/{lasr_vision_torch => lasr_vision_bodypix}/.gitignore (100%) create mode 100644 common/vision/lasr_vision_feature_extraction/.gitignore rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/CMakeLists.txt (95%) rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/launch/service.launch (52%) rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/models/.gitkeep (100%) create mode 100644 common/vision/lasr_vision_feature_extraction/nodes/service rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/package.xml (86%) rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/requirements.in (100%) rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/requirements.txt (100%) rename common/vision/{lasr_vision_torch => lasr_vision_feature_extraction}/setup.py (81%) create mode 100644 common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py create mode 100644 common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/categories_and_attributes.py create mode 100644 common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py delete mode 100644 common/vision/lasr_vision_msgs/msg/ColourPrediction.msg delete mode 100644 common/vision/lasr_vision_msgs/msg/FeatureWithColour.msg create mode 100644 common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetectionDescription.srv delete mode 100644 common/vision/lasr_vision_torch/nodes/service delete mode 100644 common/vision/lasr_vision_torch/src/lasr_vision_torch/__init__.py mode change 100644 => 100755 skills/src/lasr_skills/describe_people.py delete mode 100644 tasks/coffee_shop/config/config:=full.yaml diff --git a/common/helpers/colour_estimation/CMakeLists.txt b/common/helpers/colour_estimation/CMakeLists.txt deleted file mode 100644 index e693284eb..000000000 --- a/common/helpers/colour_estimation/CMakeLists.txt +++ /dev/null @@ -1,202 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(colour_estimation) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES colour_estimation -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/colour_estimation.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/colour_estimation_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_colour_estimation.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/common/helpers/colour_estimation/README.md b/common/helpers/colour_estimation/README.md deleted file mode 100644 index 0d579ad70..000000000 --- a/common/helpers/colour_estimation/README.md +++ /dev/null @@ -1,63 +0,0 @@ -# colour_estimation - -Python utilities for estimating the name of given colours. - -This package is maintained by: -- [Paul Makles](mailto:me@insrt.uk) - -## Prerequisites - -This package depends on the following ROS packages: -- catkin (buildtool) - -Ensure numpy is available wherever this package is imported. - -## Usage - -Find the closest colours to a given colour: - -```python -import numpy as np -from colour_estimation import closest_colours, RGB_COLOURS, RGB_HAIR_COLOURS - -# find the closest colour from RGB_COLOURS dict -closest_colours(np.array([255, 0, 0]), RGB_COLOURS) - -# find the closest colour from RGB_HAIR_COLOURS dict -closest_colours(np.array([200, 150, 0]), RGB_HAIR_COLOURS) -``` - -## Example - -Find the name of the median colour in an image: - -```python -import numpy as np -from colour_estimation import closest_colours, RGB_COLOURS - -# let `img` be a cv2 image / numpy array - -closest_colours(np.median(img, axis=0), RGB_COLOURS) -``` - -## Technical Overview - -Ask the package maintainer to write a `doc/TECHNICAL.md` for their package! - -## ROS Definitions - -### Launch Files - -This package has no launch files. - -### Messages - -This package has no messages. - -### Services - -This package has no services. - -### Actions - -This package has no actions. diff --git a/common/helpers/colour_estimation/doc/EXAMPLE.md b/common/helpers/colour_estimation/doc/EXAMPLE.md deleted file mode 100644 index 5092381b8..000000000 --- a/common/helpers/colour_estimation/doc/EXAMPLE.md +++ /dev/null @@ -1,10 +0,0 @@ -Find the name of the median colour in an image: - -```python -import numpy as np -from colour_estimation import closest_colours, RGB_COLOURS - -# let `img` be a cv2 image / numpy array - -closest_colours(np.median(img, axis=0), RGB_COLOURS) -``` diff --git a/common/helpers/colour_estimation/doc/PREREQUISITES.md b/common/helpers/colour_estimation/doc/PREREQUISITES.md deleted file mode 100644 index 693e4d848..000000000 --- a/common/helpers/colour_estimation/doc/PREREQUISITES.md +++ /dev/null @@ -1 +0,0 @@ -Ensure numpy is available wherever this package is imported. diff --git a/common/helpers/colour_estimation/doc/USAGE.md b/common/helpers/colour_estimation/doc/USAGE.md deleted file mode 100644 index 20741b2f2..000000000 --- a/common/helpers/colour_estimation/doc/USAGE.md +++ /dev/null @@ -1,12 +0,0 @@ -Find the closest colours to a given colour: - -```python -import numpy as np -from colour_estimation import closest_colours, RGB_COLOURS, RGB_HAIR_COLOURS - -# find the closest colour from RGB_COLOURS dict -closest_colours(np.array([255, 0, 0]), RGB_COLOURS) - -# find the closest colour from RGB_HAIR_COLOURS dict -closest_colours(np.array([200, 150, 0]), RGB_HAIR_COLOURS) -``` diff --git a/common/helpers/colour_estimation/setup.py b/common/helpers/colour_estimation/setup.py deleted file mode 100644 index 55caf62e8..000000000 --- a/common/helpers/colour_estimation/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python3 - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -setup_args = generate_distutils_setup( - packages=['colour_estimation'], - package_dir={'': 'src'} -) - -setup(**setup_args) diff --git a/common/helpers/colour_estimation/src/colour_estimation/__init__.py b/common/helpers/colour_estimation/src/colour_estimation/__init__.py deleted file mode 100644 index 54779f81d..000000000 --- a/common/helpers/colour_estimation/src/colour_estimation/__init__.py +++ /dev/null @@ -1,20 +0,0 @@ -from .rgb import RGB_COLOURS, RGB_HAIR_COLOURS - -import numpy as np - - -def closest_colours(requested_colour, colours): - ''' - Find the closest colours to the requested colour - - This returns the closest three matches - ''' - - distances = {color: np.linalg.norm( - np.array(rgb_val) - requested_colour) for color, rgb_val in colours.items()} - sorted_colors = sorted(distances.items(), key=lambda x: x[1]) - top_three_colors = sorted_colors[:3] - formatted_colors = [(color_name, distance) - for color_name, distance in top_three_colors] - - return formatted_colors diff --git a/common/helpers/colour_estimation/src/colour_estimation/rgb.py b/common/helpers/colour_estimation/src/colour_estimation/rgb.py deleted file mode 100644 index 9854f110e..000000000 --- a/common/helpers/colour_estimation/src/colour_estimation/rgb.py +++ /dev/null @@ -1,68 +0,0 @@ -import numpy as np - -RGB_COLOURS = { - "red": [255, 0, 0], - "green": [0, 255, 0], - "blue": [0, 0, 255], - "white": [255, 255, 255], - "black": [0, 0, 0], - "yellow": [255, 255, 0], - "cyan": [0, 255, 255], - "magenta": [255, 0, 255], - "gray": [128, 128, 128], - "orange": [255, 165, 0], - "purple": [128, 0, 128], - "brown": [139, 69, 19], - "pink": [255, 182, 193], - "beige": [245, 245, 220], - "maroon": [128, 0, 0], - "olive": [128, 128, 0], - "navy": [0, 0, 128], - "lime": [50, 205, 50], - "golden": [255, 223, 0], - "teal": [0, 128, 128], - "coral": [255, 127, 80], - "salmon": [250, 128, 114], - "turquoise": [64, 224, 208], - "violet": [238, 130, 238], - "platinum": [229, 228, 226], - "ochre": [204, 119, 34], - "burntsienna": [233, 116, 81], - "chocolate": [210, 105, 30], - "tan": [210, 180, 140], - "ivory": [255, 255, 240], - "goldenrod": [218, 165, 32], - "orchid": [218, 112, 214], - "honey": [238, 220, 130] -} - -RGB_HAIR_COLOURS = { - 'midnight black': (9, 8, 6), - 'off black': (44, 34, 43), - 'strong dark brown': (58, 48, 36), - 'medium dark brown': (78, 67, 63), - - 'chestnut brown': (106, 78, 66), - 'light chestnut brown': (106, 78, 66), - 'dark golden brown': (95, 72, 56), - 'light golden brown': (167, 133, 106), - - 'dark honey blonde': (184, 151, 128), - 'bleached blonde': (220, 208, 186), - 'light ash blonde': (222, 288, 153), - 'light ash brown': (151, 121, 97), - - 'lightest blonde': (230, 206, 168), - 'pale golden blonde': (229, 200, 168), - 'strawberry blonde': (165, 137, 70), - 'light auburn': (145, 85, 61), - - 'dark auburn': (83, 61, 53), - 'darkest gray': (113, 99, 93), - 'medium gray': (183, 166, 158), - 'light gray': (214, 196, 194), - - 'white blonde': (255, 24, 225), - 'platinum blonde': (202, 191, 177), - 'russet red': (145, 74, 67), - 'terra cotta': (181, 82, 57)} diff --git a/common/helpers/torch_module/CMakeLists.txt b/common/helpers/numpy2message/CMakeLists.txt similarity index 96% rename from common/helpers/torch_module/CMakeLists.txt rename to common/helpers/numpy2message/CMakeLists.txt index 30963cd1d..fa6585225 100644 --- a/common/helpers/torch_module/CMakeLists.txt +++ b/common/helpers/numpy2message/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(torch_module) +project(numpy2message) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -100,7 +100,7 @@ catkin_python_setup() ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES torch_module +# LIBRARIES numpy2message # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) @@ -118,7 +118,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/torch_module.cpp +# src/${PROJECT_NAME}/numpy2message.cpp # ) ## Add cmake target dependencies of the library @@ -129,7 +129,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/torch_module_node.cpp) +# add_executable(${PROJECT_NAME}_node src/numpy2message_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -193,7 +193,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_torch_module.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_feature_extractor.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/common/helpers/numpy2message/doc/usage.md b/common/helpers/numpy2message/doc/usage.md new file mode 100644 index 000000000..3f19ff99e --- /dev/null +++ b/common/helpers/numpy2message/doc/usage.md @@ -0,0 +1 @@ +To send numpy arrays with messages. \ No newline at end of file diff --git a/common/helpers/colour_estimation/package.xml b/common/helpers/numpy2message/package.xml similarity index 87% rename from common/helpers/colour_estimation/package.xml rename to common/helpers/numpy2message/package.xml index ad3ef2d8b..62391bce1 100644 --- a/common/helpers/colour_estimation/package.xml +++ b/common/helpers/numpy2message/package.xml @@ -1,13 +1,13 @@ - colour_estimation + numpy2message 0.0.0 - Python utilities for estimating the name of given colours. + Helper functions converting between numpy arrays and ROS messages. - Paul Makles + Benteng Ma @@ -19,13 +19,13 @@ - + - + Benteng Ma diff --git a/common/helpers/torch_module/setup.py b/common/helpers/numpy2message/setup.py similarity index 86% rename from common/helpers/torch_module/setup.py rename to common/helpers/numpy2message/setup.py index 2151223c9..d79792c72 100644 --- a/common/helpers/torch_module/setup.py +++ b/common/helpers/numpy2message/setup.py @@ -4,7 +4,7 @@ from catkin_pkg.python_setup import generate_distutils_setup setup_args = generate_distutils_setup( - packages=['torch_module'], + packages=['numpy2message'], package_dir={'': 'src'} ) diff --git a/common/helpers/numpy2message/src/numpy2message/__init__.py b/common/helpers/numpy2message/src/numpy2message/__init__.py new file mode 100644 index 000000000..7a330ca5f --- /dev/null +++ b/common/helpers/numpy2message/src/numpy2message/__init__.py @@ -0,0 +1,19 @@ +import numpy as np + + +def numpy2message(np_array: np.ndarray) -> list: + data = np_array.tobytes() + shape = list(np_array.shape) + dtype = str(np_array.dtype) + return data, shape, dtype + + +def message2numpy(data:bytes, shape:list, dtype:str) -> np.ndarray: + array_shape = tuple(shape) + array_dtype = np.dtype(dtype) + + deserialized_array = np.frombuffer(data, dtype=array_dtype) + deserialized_array = deserialized_array.reshape(array_shape) + + return deserialized_array + diff --git a/common/helpers/torch_module/doc/PREREQUISITES.md b/common/helpers/torch_module/doc/PREREQUISITES.md deleted file mode 100644 index b5bd0f897..000000000 --- a/common/helpers/torch_module/doc/PREREQUISITES.md +++ /dev/null @@ -1 +0,0 @@ -Ensure torch is available wherever this package is imported. diff --git a/common/helpers/torch_module/package.xml b/common/helpers/torch_module/package.xml deleted file mode 100644 index e8752c9f4..000000000 --- a/common/helpers/torch_module/package.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - torch_module - 0.0.0 - Various PyTorch helpers and utilties - - - - - Paul Makles - - - - - - MIT - - - - - - - - - - - - Benteng Ma - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - diff --git a/common/helpers/torch_module/src/torch_module/__init__.py b/common/helpers/torch_module/src/torch_module/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/common/helpers/torch_module/src/torch_module/helpers/__init__.py b/common/helpers/torch_module/src/torch_module/helpers/__init__.py deleted file mode 100644 index 21368bfc9..000000000 --- a/common/helpers/torch_module/src/torch_module/helpers/__init__.py +++ /dev/null @@ -1,78 +0,0 @@ -import torch -import torch.nn.functional as F - - -def load_torch_model(model, optimizer, path="model.pth", cpu_only=False): - if cpu_only: - checkpoint = torch.load(path, map_location=torch.device('cpu')) - else: - checkpoint = torch.load(path) - model.load_state_dict(checkpoint['model_state_dict']) - if optimizer is not None: - optimizer.load_state_dict(checkpoint['optimizer_state_dict']) - epoch = checkpoint['epoch'] - best_val_loss = checkpoint.get('best_val_loss', float('inf')) - return model, optimizer, epoch, best_val_loss - - -def binary_erosion_dilation(tensor, thresholds, erosion_iterations=1, dilation_iterations=1): - """ - Apply binary threshold, followed by erosion and dilation to a tensor. - - :param tensor: Input tensor (N, C, H, W) - :param thresholds: List of threshold values for each channel - :param erosion_iterations: Number of erosion iterations - :param dilation_iterations: Number of dilation iterations - :return: Processed tensor - """ - - # Check if the length of thresholds matches the number of channels - if len(thresholds) != tensor.size(1): - raise ValueError( - "Length of thresholds must match the number of channels") - - # Binary thresholding - for i, threshold in enumerate(thresholds): - tensor[:, i] = (tensor[:, i] > threshold/2).float() / 4 - tensor[:, i] += (tensor[:, i] > threshold).float() - tensor[:, i] /= max(tensor[:, i].clone()) - - # Define the 3x3 kernel for erosion and dilation - kernel = torch.tensor([[1, 1, 1], - [1, 1, 1], - [1, 1, 1]], dtype=torch.float32).unsqueeze(0).unsqueeze(0) - - # Replicate the kernel for each channel - kernel = kernel.repeat(tensor.size(1), 1, 1, 1).to(tensor.device) - - # Erosion - for _ in range(erosion_iterations): - # 3x3 convolution with groups - tensor = F.conv2d(tensor, kernel, padding=1, groups=tensor.size(1)) - tensor = (tensor == 9).float() # Check if all neighboring pixels are 1 - - # Dilation - for _ in range(dilation_iterations): - # 3x3 convolution with groups - tensor_dilated = F.conv2d( - tensor, kernel, padding=1, groups=tensor.size(1)) - # Combine the original and dilated tensors - tensor = torch.clamp(tensor + tensor_dilated, 0, 1) - - return tensor - - -def median_color_float(rgb_image: torch.Tensor, mask: torch.Tensor) -> torch.Tensor: - mask = mask.bool() - median_colors = torch.zeros((rgb_image.size(0), mask.size( - 1), rgb_image.size(1)), device=rgb_image.device) - for i in range(rgb_image.size(0)): - for j in range(mask.size(1)): - for k in range(rgb_image.size(1)): - valid_pixels = torch.masked_select(rgb_image[i, k], mask[i, j]) - if valid_pixels.numel() > 0: - median_value = valid_pixels.median() - else: - median_value = torch.tensor(0.0, device=rgb_image.device) - median_colors[i, j, k] = median_value - return median_colors # / 255.0 diff --git a/common/helpers/torch_module/src/torch_module/modules/__init__.py b/common/helpers/torch_module/src/torch_module/modules/__init__.py deleted file mode 100644 index b0eac41aa..000000000 --- a/common/helpers/torch_module/src/torch_module/modules/__init__.py +++ /dev/null @@ -1,131 +0,0 @@ -import torch -import torch.nn as nn -import torch.nn.functional as F -import torchvision.models as models - - -class CombinedModelNoRegression(nn.Module): - def __init__(self, segment_model: nn.Module, predict_model: nn.Module, cat_layers: int = None): - super(CombinedModelNoRegression, self).__init__() - self.segment_model = segment_model - self.predict_model = predict_model - self.cat_layers = cat_layers - - def forward(self, x: torch.Tensor): - seg_masks = self.segment_model(x) - if self.cat_layers: - seg_masks_ = seg_masks[:, 0:self.cat_layers] - x = torch.cat((x, seg_masks_), dim=1) - else: - x = torch.cat((x, seg_masks), dim=1) - logic_outputs = self.predict_model(x) - return seg_masks, logic_outputs - - -class ASPP(nn.Module): - def __init__(self, in_channels, out_channels): - super(ASPP, self).__init__() - self.atrous_block1 = nn.Conv2d(in_channels, out_channels, 1, 1) - self.atrous_block6 = nn.Conv2d( - in_channels, out_channels, 3, padding=6, dilation=6) - self.atrous_block12 = nn.Conv2d( - in_channels, out_channels, 3, padding=12, dilation=12) - self.atrous_block18 = nn.Conv2d( - in_channels, out_channels, 3, padding=18, dilation=18) - self.conv_out = nn.Conv2d(out_channels * 4, out_channels, 1, 1) - - def forward(self, x): - x1 = self.atrous_block1(x) - x6 = self.atrous_block6(x) - x12 = self.atrous_block12(x) - x18 = self.atrous_block18(x) - x = torch.cat([x1, x6, x12, x18], dim=1) - return self.conv_out(x) - - -class DeepLabV3PlusMobileNetV3(nn.Module): - def __init__(self, num_classes, in_channels=3, sigmoid=True): - super(DeepLabV3PlusMobileNetV3, self).__init__() - self.sigmoid = sigmoid - mobilenet_v3 = models.mobilenet_v3_large(pretrained=True) - - if in_channels != 3: - mobilenet_v3.features[0][0] = nn.Conv2d( - in_channels, 16, kernel_size=3, stride=2, padding=1, bias=False - ) - - self.encoder = mobilenet_v3.features - - intermediate_channel = self.encoder[-1].out_channels - self.aspp = ASPP(intermediate_channel, 256) - - self.decoder = nn.Sequential( - # Concatenated with original input - nn.Conv2d(256 + in_channels, 256, kernel_size=3, padding=1), - nn.ReLU(inplace=True), - nn.Conv2d(256, 256, kernel_size=3, padding=1), - nn.ReLU(inplace=True), - nn.Conv2d(256, num_classes, kernel_size=1) - ) - - def forward(self, x): - original_input = x - x_encoded = self.encoder(x) - x_aspp = self.aspp(x_encoded) - - x = F.interpolate( - x_aspp, size=original_input.shape[2:], mode='bilinear', align_corners=False) - # Concatenate with original input - x = torch.cat([x, original_input], dim=1) - x = self.decoder(x) - - if self.sigmoid: - x = torch.sigmoid(x) - - return x - - -class MultiLabelMobileNetV3Small(nn.Module): - def __init__(self, num_labels, input_channels=3, sigmoid=True, pretrained=True): - super(MultiLabelMobileNetV3Small, self).__init__() - mobilenet_v3_small = models.mobilenet_v3_small(pretrained=pretrained) - self.sigmoid = sigmoid - - if input_channels != 3: - mobilenet_v3_small.features[0][0] = nn.Conv2d( - input_channels, 16, kernel_size=3, stride=2, padding=1, bias=False - ) - - self.model = mobilenet_v3_small - - num_ftrs = self.model.classifier[3].in_features - self.model.classifier[3] = nn.Linear(num_ftrs, num_labels) - - def forward(self, x): - x = self.model(x) - if self.sigmoid: - x = torch.sigmoid(x) - return x - - -class MultiLabelMobileNetV3Large(nn.Module): - def __init__(self, num_labels, input_channels=3, sigmoid=True, pretrained=True): - super(MultiLabelMobileNetV3Large, self).__init__() - mobilenet_v3_small = models.mobilenet_v3_large(pretrained=pretrained) - self.sigmoid = sigmoid - - if input_channels != 3: - mobilenet_v3_small.features[0][0] = nn.Conv2d( - input_channels, 16, kernel_size=3, stride=2, padding=1, bias=False - ) - - self.model = mobilenet_v3_small - - num_ftrs = self.model.classifier[3].in_features - self.model.classifier[3] = nn.Linear(num_ftrs, num_labels) - - def forward(self, x): - x = self.model(x) - if self.sigmoid: - x = torch.sigmoid(x) - return x diff --git a/common/vision/lasr_vision_torch/.gitignore b/common/vision/lasr_vision_bodypix/.gitignore similarity index 100% rename from common/vision/lasr_vision_torch/.gitignore rename to common/vision/lasr_vision_bodypix/.gitignore diff --git a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py index 81a924640..b1cab9f83 100644 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -9,31 +9,33 @@ from tf_bodypix.api import download_model, load_model, BodyPixModelPaths from sensor_msgs.msg import Image as SensorImage -from lasr_vision_msgs.msg import BodyPixMask +from lasr_vision_msgs.msg import BodyPixMask, BodyPixPose from lasr_vision_msgs.srv import BodyPixDetectionRequest, BodyPixDetectionResponse +import rospkg + # model cache loaded_models = {} +r = rospkg.RosPack() def load_model_cached(dataset: str) -> None: ''' Load a model into cache ''' - model = None if dataset in loaded_models: model = loaded_models[dataset] else: if dataset == 'resnet50': - model = load_model(download_model(BodyPixModelPaths.RESNET50_FLOAT_STRIDE_16)) + name = download_model(BodyPixModelPaths.RESNET50_FLOAT_STRIDE_16) + model = load_model(name) elif dataset == 'mobilenet50': - model = load_model(download_model(BodyPixModelPaths.MOBILENET_FLOAT_50_STRIDE_16)) + name = download_model(BodyPixModelPaths.MOBILENET_FLOAT_50_STRIDE_16) + model = load_model(name) else: model = load_model(dataset) - rospy.loginfo(f'Loaded {dataset} model') loaded_models[dataset] = model - return model def detect(request: BodyPixDetectionRequest, debug_publisher: rospy.Publisher | None) -> BodyPixDetectionResponse: @@ -65,8 +67,35 @@ def detect(request: BodyPixDetectionRequest, debug_publisher: rospy.Publisher | bodypix_mask.shape = list(part_mask.shape) masks.append(bodypix_mask) - # construct pose response - # TODO + # construct poses response and neck coordinates + poses = result.get_poses() + rospy.loginfo(str(poses)) + + neck_coordinates = [] + for pose in poses: + left_shoulder_keypoint = pose.keypoints.get(5) # 5 is the typical index for left shoulder + right_shoulder_keypoint = pose.keypoints.get(6) # 6 is the typical index for right shoulder + + if left_shoulder_keypoint and right_shoulder_keypoint: + # If both shoulders are detected, calculate neck as midpoint + left_shoulder = left_shoulder_keypoint.position + right_shoulder = right_shoulder_keypoint.position + neck_x = (left_shoulder.x + right_shoulder.x) / 2 + neck_y = (left_shoulder.y + right_shoulder.y) / 2 + elif left_shoulder_keypoint: + # Only left shoulder detected, use it as neck coordinate + left_shoulder = left_shoulder_keypoint.position + neck_x = left_shoulder.x + neck_y = left_shoulder.y + elif right_shoulder_keypoint: + # Only right shoulder detected, use it as neck coordinate + right_shoulder = right_shoulder_keypoint.position + neck_x = right_shoulder.x + neck_y = right_shoulder.y + + pose = BodyPixPose() + pose.coord = np.array([neck_x, neck_y]).astype(np.int32) + neck_coordinates.append(pose) # publish to debug topic if debug_publisher is not None: @@ -80,9 +109,9 @@ def detect(request: BodyPixDetectionRequest, debug_publisher: rospy.Publisher | keypoints_color=(255, 100, 100), skeleton_color=(100, 100, 255), ) - debug_publisher.publish(cv2_img.cv2_img_to_msg(coloured_mask)) response = BodyPixDetectionResponse() response.masks = masks + response.poses = neck_coordinates return response diff --git a/common/vision/lasr_vision_feature_extraction/.gitignore b/common/vision/lasr_vision_feature_extraction/.gitignore new file mode 100644 index 000000000..2de3a7027 --- /dev/null +++ b/common/vision/lasr_vision_feature_extraction/.gitignore @@ -0,0 +1,2 @@ +models/* +!models/.gitkeep \ No newline at end of file diff --git a/common/vision/lasr_vision_torch/CMakeLists.txt b/common/vision/lasr_vision_feature_extraction/CMakeLists.txt similarity index 95% rename from common/vision/lasr_vision_torch/CMakeLists.txt rename to common/vision/lasr_vision_feature_extraction/CMakeLists.txt index da57acfe7..1d4613622 100644 --- a/common/vision/lasr_vision_torch/CMakeLists.txt +++ b/common/vision/lasr_vision_feature_extraction/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(lasr_vision_torch) +project(lasr_vision_feature_extraction) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -104,7 +104,7 @@ catkin_generate_virtualenv( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES lasr_vision_torch +# LIBRARIES lasr_vision_feature_extraction # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) @@ -122,7 +122,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/lasr_vision_torch.cpp +# src/${PROJECT_NAME}/lasr_vision_feature_extraction.cpp # ) ## Add cmake target dependencies of the library @@ -133,7 +133,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/lasr_vision_torch_node.cpp) +# add_executable(${PROJECT_NAME}_node src/lasr_vision_feature_extraction_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -197,7 +197,7 @@ catkin_install_python(PROGRAMS ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_lasr_vision_torch.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_lasr_vision_feature_extraction.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/common/vision/lasr_vision_torch/launch/service.launch b/common/vision/lasr_vision_feature_extraction/launch/service.launch similarity index 52% rename from common/vision/lasr_vision_torch/launch/service.launch rename to common/vision/lasr_vision_feature_extraction/launch/service.launch index 96af5a527..11bdbbebb 100644 --- a/common/vision/lasr_vision_torch/launch/service.launch +++ b/common/vision/lasr_vision_feature_extraction/launch/service.launch @@ -2,5 +2,5 @@ Start the torch service - + \ No newline at end of file diff --git a/common/vision/lasr_vision_torch/models/.gitkeep b/common/vision/lasr_vision_feature_extraction/models/.gitkeep similarity index 100% rename from common/vision/lasr_vision_torch/models/.gitkeep rename to common/vision/lasr_vision_feature_extraction/models/.gitkeep diff --git a/common/vision/lasr_vision_feature_extraction/nodes/service b/common/vision/lasr_vision_feature_extraction/nodes/service new file mode 100644 index 000000000..419b89e07 --- /dev/null +++ b/common/vision/lasr_vision_feature_extraction/nodes/service @@ -0,0 +1,38 @@ +from lasr_vision_msgs.srv import TorchFaceFeatureDetectionDescription, TorchFaceFeatureDetectionDescriptionRequest, TorchFaceFeatureDetectionDescriptionResponse +from lasr_vision_feature_extraction.categories_and_attributes import CategoriesAndAttributes, CelebAMaskHQCategoriesAndAttributes + +from cv2_img import msg_to_cv2_img +from numpy2message import message2numpy +import numpy as np +import cv2 +import torch +import rospy +import rospkg +import lasr_vision_feature_extraction +from os import path + + +def detect(request: TorchFaceFeatureDetectionDescriptionRequest) -> TorchFaceFeatureDetectionDescriptionRequest: + # decode the image + rospy.loginfo('Decoding') + full_frame = msg_to_cv2_img(request.image_raw) + torso_mask_data, torso_mask_shape, torso_mask_dtype = request.torso_mask_data, request.torso_mask_shape, request.torso_mask_dtype + head_mask_data, head_mask_shape, head_mask_dtype = request.head_mask_data, request.head_mask_shape, request.head_mask_dtype + torso_mask = message2numpy(torso_mask_data, torso_mask_shape, torso_mask_dtype) + head_mask = message2numpy(head_mask_data, head_mask_shape, head_mask_dtype) + head_frame = lasr_vision_feature_extraction.extract_mask_region(full_frame, head_mask.astype(np.uint8), expand_x=0.4, expand_y=0.5) + torso_frame = lasr_vision_feature_extraction.extract_mask_region(full_frame, torso_mask.astype(np.uint8), expand_x=0.2, expand_y=0.0) + rst_str = lasr_vision_feature_extraction.predict_frame(head_frame, torso_frame, full_frame, head_mask, torso_mask, predictor=predictor) + response = TorchFaceFeatureDetectionDescriptionResponse() + response.description = rst_str + return response + + +if __name__ == '__main__': + # predictor will be global when inited, thus will be used within the function above. + model = lasr_vision_feature_extraction.load_face_classifier_model() + predictor = lasr_vision_feature_extraction.Predictor(model, torch.device('cpu'), CelebAMaskHQCategoriesAndAttributes) + rospy.init_node('torch_service') + rospy.Service('/torch/detect/face_features', TorchFaceFeatureDetectionDescription, detect) + rospy.loginfo('Torch service started') + rospy.spin() diff --git a/common/vision/lasr_vision_torch/package.xml b/common/vision/lasr_vision_feature_extraction/package.xml similarity index 86% rename from common/vision/lasr_vision_torch/package.xml rename to common/vision/lasr_vision_feature_extraction/package.xml index 975aa03e4..ed5e167a9 100644 --- a/common/vision/lasr_vision_torch/package.xml +++ b/common/vision/lasr_vision_feature_extraction/package.xml @@ -1,13 +1,14 @@ - lasr_vision_torch + lasr_vision_feature_extraction 0.0.0 - Serivce providing custom vision models using PyTorch + Person attribute extraction, including face and clothes, implemented with PyTorch custom models. Paul Makles + Benteng Ma @@ -19,13 +20,14 @@ - + + Benteng Ma @@ -52,8 +54,6 @@ catkin_virtualenv lasr_vision_msgs cv2_img - torch_module - colour_estimation diff --git a/common/vision/lasr_vision_torch/requirements.in b/common/vision/lasr_vision_feature_extraction/requirements.in similarity index 100% rename from common/vision/lasr_vision_torch/requirements.in rename to common/vision/lasr_vision_feature_extraction/requirements.in diff --git a/common/vision/lasr_vision_torch/requirements.txt b/common/vision/lasr_vision_feature_extraction/requirements.txt similarity index 100% rename from common/vision/lasr_vision_torch/requirements.txt rename to common/vision/lasr_vision_feature_extraction/requirements.txt diff --git a/common/vision/lasr_vision_torch/setup.py b/common/vision/lasr_vision_feature_extraction/setup.py similarity index 81% rename from common/vision/lasr_vision_torch/setup.py rename to common/vision/lasr_vision_feature_extraction/setup.py index b59bbda5e..9df7d2505 100644 --- a/common/vision/lasr_vision_torch/setup.py +++ b/common/vision/lasr_vision_feature_extraction/setup.py @@ -4,7 +4,7 @@ from catkin_pkg.python_setup import generate_distutils_setup setup_args = generate_distutils_setup( - packages=['lasr_vision_torch'], + packages=['lasr_vision_feature_extraction'], package_dir={'': 'src'} ) diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py new file mode 100644 index 000000000..19c4e1cc7 --- /dev/null +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py @@ -0,0 +1,345 @@ +from lasr_vision_feature_extraction.categories_and_attributes import CategoriesAndAttributes, CelebAMaskHQCategoriesAndAttributes +from lasr_vision_feature_extraction.image_with_masks_and_attributes import ImageWithMasksAndAttributes, ImageOfPerson + +import numpy as np +import cv2 +import torch +import rospkg +from os import path +import torch.nn as nn +import torch.nn.functional as F +import torchvision.models as models + + +def X2conv(in_channels, out_channels, inner_channels=None): + inner_channels = out_channels // 2 if inner_channels is None else inner_channels + down_conv = nn.Sequential( + nn.Conv2d(in_channels, inner_channels, kernel_size=3, padding=1, bias=False), + nn.BatchNorm2d(inner_channels), + nn.ReLU(inplace=True), + nn.Conv2d(inner_channels, out_channels, kernel_size=3, padding=1, bias=False), + nn.BatchNorm2d(out_channels), + nn.ReLU(inplace=True)) + return down_conv + + +class Decoder(nn.Module): + def __init__(self, in_channels, skip_channels, out_channels): + super(Decoder, self).__init__() + self.up = nn.ConvTranspose2d(in_channels, out_channels, kernel_size=2, stride=2) + self.up_conv = X2conv(out_channels + skip_channels, out_channels) + + def forward(self, x_copy, x): + x = self.up(x) + if x.size(2) != x_copy.size(2) or x.size(3) != x_copy.size(3): + x = F.interpolate(x, size=(x_copy.size(2), x_copy.size(3)), mode='bilinear', align_corners=True) + x = torch.cat((x_copy, x), dim=1) + x = self.up_conv(x) + return x + + +class UNetWithResnetEncoder(nn.Module): + def __init__(self, num_classes, in_channels=3, freeze_bn=False, sigmoid=True): + super(UNetWithResnetEncoder, self).__init__() + self.sigmoid = sigmoid + self.resnet = models.resnet34(pretrained=False) # Initialize with a ResNet model + if in_channels != 3: + self.resnet.conv1 = nn.Conv2d(in_channels, 64, kernel_size=7, stride=2, padding=3, bias=False) + + self.encoder1 = nn.Sequential(self.resnet.conv1, self.resnet.bn1, self.resnet.relu) + self.encoder2 = self.resnet.layer1 + self.encoder3 = self.resnet.layer2 + self.encoder4 = self.resnet.layer3 + self.encoder5 = self.resnet.layer4 + + self.up1 = Decoder(512, 256, 256) + self.up2 = Decoder(256, 128, 128) + self.up3 = Decoder(128, 64, 64) + self.up4 = Decoder(64, 64, 64) + + self.final_conv = nn.Conv2d(64, num_classes, kernel_size=1) + self._initialize_weights() + + if freeze_bn: + self.freeze_bn() + + def _initialize_weights(self): + for module in self.modules(): + if isinstance(module, nn.Conv2d) or isinstance(module, nn.ConvTranspose2d): + nn.init.kaiming_normal_(module.weight) + if module.bias is not None: + module.bias.data.zero_() + elif isinstance(module, nn.BatchNorm2d): + module.weight.data.fill_(1) + module.bias.data.zero_() + + def forward(self, x): + x1 = self.encoder1(x) + x2 = self.encoder2(x1) + x3 = self.encoder3(x2) + x4 = self.encoder4(x3) + x5 = self.encoder5(x4) + + x = self.up1(x4, x5) + x = self.up2(x3, x) + x = self.up3(x2, x) + x = self.up4(x1, x) + x = F.interpolate(x, size=(x.size(2) * 2, x.size(3) * 2), mode='bilinear', align_corners=True) + + x = self.final_conv(x) + + if self.sigmoid: + x = torch.sigmoid(x) + return x + + def freeze_bn(self): + for module in self.modules(): + if isinstance(module, nn.BatchNorm2d): + module.eval() + + def unfreeze_bn(self): + for module in self.modules(): + if isinstance(module, nn.BatchNorm2d): + module.train() + + +class MultiLabelResNet(nn.Module): + def __init__(self, num_labels, input_channels=3, sigmoid=True): + super(MultiLabelResNet, self).__init__() + self.model = models.resnet34(pretrained=False) + self.sigmoid = sigmoid + + if input_channels != 3: + self.model.conv1 = nn.Conv2d(input_channels, 64, kernel_size=7, stride=2, padding=3, bias=False) + + num_ftrs = self.model.fc.in_features + + self.model.fc = nn.Linear(num_ftrs, num_labels) + + def forward(self, x): + x = self.model(x) + if self.sigmoid: + x = torch.sigmoid(x) + return x + + +class CombinedModel(nn.Module): + def __init__(self, segment_model: nn.Module, predict_model: nn.Module, cat_layers: int=None): + super(CombinedModel, self).__init__() + self.segment_model = segment_model + self.predict_model = predict_model + self.cat_layers = cat_layers + self.freeze_seg = False + + def forward(self, x: torch.Tensor): + seg_masks = self.segment_model(x) + seg_masks_ = seg_masks.detach() + if self.cat_layers: + seg_masks_ = seg_masks_[:, 0:self.cat_layers] + x = torch.cat((x, seg_masks_), dim=1) + else: + x = torch.cat((x, seg_masks_), dim=1) + logic_outputs = self.predict_model(x) + return seg_masks, logic_outputs + + def freeze_segment_model(self): + self.segment_model.eval() + + def unfreeze_segment_model(self): + self.segment_model.train() + + + +class Predictor: + def __init__(self, model: torch.nn.Module, device: torch.device, categories_and_attributes: CategoriesAndAttributes): + self.model = model + self.device = device + self.categories_and_attributes = categories_and_attributes + + self._thresholds_mask: list[float] = [] + self._thresholds_pred: list[float] = [] + for key in sorted(list(self.categories_and_attributes.merged_categories.keys())): + self._thresholds_mask.append(self.categories_and_attributes.thresholds_mask[key]) + for attribute in self.categories_and_attributes.attributes: + if attribute not in self.categories_and_attributes.avoided_attributes: + self._thresholds_pred.append(self.categories_and_attributes.thresholds_pred[attribute]) + + def predict(self, rgb_image: np.ndarray) -> ImageWithMasksAndAttributes: + mean_val = np.mean(rgb_image) + image_tensor = torch.from_numpy(rgb_image).permute(2, 0, 1).unsqueeze(0).float() / 255.0 + pred_masks, pred_classes = self.model(image_tensor) + # Apply binary erosion and dilation to the masks + pred_masks = binary_erosion_dilation( + pred_masks, thresholds=self._thresholds_mask, + erosion_iterations=1, dilation_iterations=1 + ) + pred_masks = pred_masks.detach().squeeze(0).numpy().astype(np.uint8) + mask_list = [pred_masks[i, :, :] for i in range(pred_masks.shape[0])] + pred_classes = pred_classes.detach().squeeze(0).numpy() + class_list = [pred_classes[i].item() for i in range(pred_classes.shape[0])] + # print(rgb_image) + print(mean_val) + print(pred_classes) + mask_dict = {} + for i, mask in enumerate(mask_list): + mask_dict[self.categories_and_attributes.mask_categories[i]] = mask + attribute_dict = {} + class_list_iter = class_list.__iter__() + for attribute in self.categories_and_attributes.attributes: + if attribute not in self.categories_and_attributes.avoided_attributes: + attribute_dict[attribute] = class_list_iter.__next__() + for attribute in self.categories_and_attributes.mask_labels: + attribute_dict[attribute] = class_list_iter.__next__() + image_obj = ImageWithMasksAndAttributes(rgb_image, mask_dict, attribute_dict, self.categories_and_attributes) + return image_obj + + +def load_face_classifier_model(): + cat_layers = CelebAMaskHQCategoriesAndAttributes.merged_categories.keys().__len__() + segment_model = UNetWithResnetEncoder(num_classes=cat_layers) + predictions = len(CelebAMaskHQCategoriesAndAttributes.attributes) - len( + CelebAMaskHQCategoriesAndAttributes.avoided_attributes) + len(CelebAMaskHQCategoriesAndAttributes.mask_labels) + predict_model = MultiLabelResNet(num_labels=predictions, input_channels=cat_layers + 3) + model = CombinedModel(segment_model, predict_model, cat_layers=cat_layers) + model.eval() + + r = rospkg.RosPack() + model, _, _, _ = load_torch_model(model, None, path=path.join(r.get_path( + "lasr_vision_feature_extraction"), "models", "model.pth"), cpu_only=True) + return model + + +def pad_image_to_even_dims(image): + # Get the current shape of the image + height, width, _ = image.shape + + # Calculate the padding needed for height and width + height_pad = 0 if height % 2 == 0 else 1 + width_pad = 0 if width % 2 == 0 else 1 + + # Pad the image. Pad the bottom and right side of the image + padded_image = np.pad(image, ((0, height_pad), (0, width_pad), (0, 0)), mode='constant', constant_values=0) + + return padded_image + + +def extract_mask_region(frame, mask, expand_x=0.5, expand_y=0.5): + """ + Extracts the face region from the image and expands the region by the specified amount. + + :param frame: The source image. + :param mask: The mask with the face part. + :param expand_x: The percentage to expand the width of the bounding box. + :param expand_y: The percentage to expand the height of the bounding box. + :return: The extracted face region as a numpy array, or None if not found. + """ + contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + if contours: + largest_contour = max(contours, key=cv2.contourArea) + x, y, w, h = cv2.boundingRect(largest_contour) + + # Expand the bounding box + new_w = w * (1 + expand_x) + new_h = h * (1 + expand_y) + x -= (new_w - w) // 2 + y -= (new_h - h) // 2 + + # Ensure the new bounding box is within the frame dimensions + x = int(max(0, x)) + y = int(max(0, y)) + new_w = min(frame.shape[1] - x, new_w) + new_h = min(frame.shape[0] - y, new_h) + + face_region = frame[y:y+int(new_h), x:x+int(new_w)] + return face_region + return None + + +def predict_frame(head_frame, torso_frame, full_frame, head_mask, torso_mask, predictor): + full_frame = cv2.cvtColor(full_frame, cv2.COLOR_BGR2RGB) + head_frame = cv2.cvtColor(head_frame, cv2.COLOR_BGR2RGB) + torso_frame = cv2.cvtColor(torso_frame, cv2.COLOR_BGR2RGB) + + head_frame = pad_image_to_even_dims(head_frame) + torso_frame = pad_image_to_even_dims(torso_frame) + + rst = ImageOfPerson.from_parent_instance(predictor.predict(head_frame)) + + return rst.describe() + + +def load_torch_model(model, optimizer, path="model.pth", cpu_only=False): + if cpu_only: + checkpoint = torch.load(path, map_location=torch.device('cpu')) + else: + checkpoint = torch.load(path) + model.load_state_dict(checkpoint['model_state_dict']) + if optimizer is not None: + optimizer.load_state_dict(checkpoint['optimizer_state_dict']) + epoch = checkpoint['epoch'] + best_val_loss = checkpoint.get('best_val_loss', float('inf')) + return model, optimizer, epoch, best_val_loss + + +def binary_erosion_dilation(tensor, thresholds, erosion_iterations=1, dilation_iterations=1): + """ + Apply binary threshold, followed by erosion and dilation to a tensor. + + :param tensor: Input tensor (N, C, H, W) + :param thresholds: List of threshold values for each channel + :param erosion_iterations: Number of erosion iterations + :param dilation_iterations: Number of dilation iterations + :return: Processed tensor + """ + + # Check if the length of thresholds matches the number of channels + if len(thresholds) != tensor.size(1): + raise ValueError( + "Length of thresholds must match the number of channels") + + # Binary thresholding + for i, threshold in enumerate(thresholds): + tensor[:, i] = (tensor[:, i] > threshold/2).float() / 4 + tensor[:, i] += (tensor[:, i] > threshold).float() + tensor[:, i] /= max(tensor[:, i].clone()) + + # Define the 3x3 kernel for erosion and dilation + kernel = torch.tensor([[1, 1, 1], + [1, 1, 1], + [1, 1, 1]], dtype=torch.float32).unsqueeze(0).unsqueeze(0) + + # Replicate the kernel for each channel + kernel = kernel.repeat(tensor.size(1), 1, 1, 1).to(tensor.device) + + # Erosion + for _ in range(erosion_iterations): + # 3x3 convolution with groups + tensor = F.conv2d(tensor, kernel, padding=1, groups=tensor.size(1)) + tensor = (tensor == 9).float() # Check if all neighboring pixels are 1 + + # Dilation + for _ in range(dilation_iterations): + # 3x3 convolution with groups + tensor_dilated = F.conv2d( + tensor, kernel, padding=1, groups=tensor.size(1)) + # Combine the original and dilated tensors + tensor = torch.clamp(tensor + tensor_dilated, 0, 1) + + return tensor + + +def median_color_float(rgb_image: torch.Tensor, mask: torch.Tensor) -> torch.Tensor: + mask = mask.bool() + median_colors = torch.zeros((rgb_image.size(0), mask.size( + 1), rgb_image.size(1)), device=rgb_image.device) + for i in range(rgb_image.size(0)): + for j in range(mask.size(1)): + for k in range(rgb_image.size(1)): + valid_pixels = torch.masked_select(rgb_image[i, k], mask[i, j]) + if valid_pixels.numel() > 0: + median_value = valid_pixels.median() + else: + median_value = torch.tensor(0.0, device=rgb_image.device) + median_colors[i, j, k] = median_value + return median_colors # / 255.0 + diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/categories_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/categories_and_attributes.py new file mode 100644 index 000000000..d03f51cf9 --- /dev/null +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/categories_and_attributes.py @@ -0,0 +1,62 @@ +class CategoriesAndAttributes: + mask_categories: list[str] = [] + merged_categories: dict[str, list[str]] = {} + mask_labels: list[str] = [] + selective_attributes: dict[str, list[str]] = {} + plane_attributes: list[str] = [] + avoided_attributes: list[str] = [] + attributes: list[str] = [] + thresholds_mask: dict[str, float] = {} + thresholds_pred: dict[str, float] = {} + + +class CelebAMaskHQCategoriesAndAttributes(CategoriesAndAttributes): + mask_categories = ['cloth', 'r_ear', 'hair', 'l_brow', 'l_eye', 'l_lip', 'mouth', 'neck', 'nose', 'r_brow', + 'r_ear', 'r_eye', 'skin', 'u_lip', 'hat', 'l_ear', 'neck_l', 'eye_g', ] + merged_categories = { + 'ear': ['l_ear', 'r_ear',], + 'brow': ['l_brow', 'r_brow',], + 'eye': ['l_eye', 'r_eye',], + 'mouth': ['l_lip', 'u_lip', 'mouth',], + } + _categories_to_merge = [] + for key in sorted(list(merged_categories.keys())): + for cat in merged_categories[key]: + _categories_to_merge.append(cat) + for key in mask_categories: + if key not in _categories_to_merge: + merged_categories[key] = [key] + mask_labels = ['hair'] + selective_attributes = { + 'facial_hair': ['5_o_Clock_Shadow', 'Goatee', 'Mustache', 'No_Beard', 'Sideburns', ], + 'hair_colour': ['Black_Hair', 'Blond_Hair', 'Brown_Hair', 'Gray_Hair', ], + 'hair_shape': ['Straight_Hair', 'Wavy_Hair', ] + } + plane_attributes = ['Bangs', 'Eyeglasses', 'Wearing_Earrings', 'Wearing_Hat', 'Wearing_Necklace', + 'Wearing_Necktie', 'Male', ] + avoided_attributes = ['Arched_Eyebrows', 'Bags_Under_Eyes', 'Big_Lips', 'Big_Nose', 'Bushy_Eyebrows', 'Chubby', + 'Double_Chin', 'High_Cheekbones', 'Narrow_Eyes', 'Oval_Face', 'Pointy_Nose', + 'Receding_Hairline', 'Rosy_Cheeks', 'Heavy_Makeup', 'Wearing_Lipstick', 'Attractive', + 'Blurry', 'Mouth_Slightly_Open', 'Pale_Skin', 'Smiling', 'Young', ] + attributes = ["5_o_Clock_Shadow", "Arched_Eyebrows", "Attractive", "Bags_Under_Eyes", "Bald", "Bangs", "Big_Lips", + "Big_Nose", "Black_Hair", "Blond_Hair", "Blurry", "Brown_Hair", "Bushy_Eyebrows", "Chubby", + "Double_Chin", "Eyeglasses", "Goatee", "Gray_Hair", "Heavy_Makeup", "High_Cheekbones", "Male", + "Mouth_Slightly_Open", "Mustache", "Narrow_Eyes", "No_Beard", "Oval_Face", "Pale_Skin", "Pointy_Nose", + "Receding_Hairline", "Rosy_Cheeks", "Sideburns", "Smiling", "Straight_Hair", "Wavy_Hair", + "Wearing_Earrings", "Wearing_Hat", "Wearing_Lipstick", "Wearing_Necklace", "Wearing_Necktie", "Young"] + + thresholds_mask: dict[str, float] = {} + thresholds_pred: dict[str, float] = {} + + # set default thresholds: + for key in sorted(merged_categories.keys()): + thresholds_mask[key] = 0.5 + for key in attributes + mask_labels: + thresholds_pred[key] = 0.5 + + # set specific thresholds: + thresholds_mask['eye_g'] = 0.25 + thresholds_pred['Eyeglasses'] = 0.25 + thresholds_pred['Wearing_Earrings'] = 0.5 + thresholds_pred['Wearing_Necklace'] = 0.5 + thresholds_pred['Wearing_Necktie'] = 0.5 diff --git a/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py new file mode 100644 index 000000000..57858fc12 --- /dev/null +++ b/common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py @@ -0,0 +1,161 @@ +import numpy as np +from lasr_vision_feature_extraction.categories_and_attributes import CategoriesAndAttributes +import json + + +def _softmax(x: list[float]) -> list[float]: + """Compute softmax values for each set of scores in x without using NumPy.""" + # First, compute e^x for each value in x + exp_values = [_exp(val) for val in x] + # Compute the sum of all e^x values + sum_of_exp_values = sum(exp_values) + # Now compute the softmax value for each original value in x + softmax_values = [exp_val / sum_of_exp_values for exp_val in exp_values] + return softmax_values + + +def _exp(x): + """Compute e^x for a given x. A simple implementation of the exponential function.""" + return 2.718281828459045 ** x # Using an approximation of Euler's number e + + +class ImageWithMasksAndAttributes: + def __init__(self, image: np.ndarray, masks: dict[str, np.ndarray], attributes: dict[str, float], + categories_and_attributes: CategoriesAndAttributes): + self.image: np.ndarray = image + self.masks: dict[str, np.ndarray] = masks + self.attributes: dict[str, float] = attributes + self.categories_and_attributes: CategoriesAndAttributes = categories_and_attributes + + self.plane_attribute_dict: dict[str, float] = {} + for attribute in self.categories_and_attributes.plane_attributes: + self.plane_attribute_dict[attribute] = self.attributes[attribute] + + self.selective_attribute_dict: dict[str, dict[str, float]] = {} + for category in sorted(list(self.categories_and_attributes.selective_attributes.keys())): + self.selective_attribute_dict[category] = {} + temp_list: list[float] = [] + for attribute in self.categories_and_attributes.selective_attributes[category]: + temp_list.append(self.attributes[attribute]) + softmax_list = _softmax(temp_list) + for i, attribute in enumerate(self.categories_and_attributes.selective_attributes[category]): + self.selective_attribute_dict[category][attribute] = softmax_list[i] + + def describe(self) -> str: + # abstract method + pass + + +def _max_value_tuple(some_dict: dict[str, float]) -> tuple[str, float]: + max_key = max(some_dict, key=some_dict.get) + return max_key, some_dict[max_key] + + +class ImageOfPerson(ImageWithMasksAndAttributes): + def __init__(self, image: np.ndarray, masks: dict[str, np.ndarray], attributes: dict[str, float], + categories_and_attributes: CategoriesAndAttributes): + super().__init__(image, masks, attributes, categories_and_attributes) + + @classmethod + def from_parent_instance(cls, parent_instance: ImageWithMasksAndAttributes) -> 'ImageOfPerson': + """ + Creates an instance of ImageOfPerson using the properties of an + instance of ImageWithMasksAndAttributes. + """ + return cls(image=parent_instance.image, + masks=parent_instance.masks, + attributes=parent_instance.attributes, + categories_and_attributes=parent_instance.categories_and_attributes) + + def describe(self) -> str: + male = (self.attributes['Male'] > self.categories_and_attributes.thresholds_pred['Male'], self.attributes['Male']) + has_hair = ( + self.attributes['hair'] > self.categories_and_attributes.thresholds_pred['hair'], self.attributes['hair']) + hair_colour = _max_value_tuple(self.selective_attribute_dict['hair_colour']) + hair_shape = _max_value_tuple(self.selective_attribute_dict['hair_shape']) + facial_hair = _max_value_tuple(self.selective_attribute_dict['facial_hair']) + # bangs = ( + # self.attributes['Bangs'] > self.categories_and_attributes.thresholds_pred['Bangs'], + # self.attributes['Bangs']) + hat = (self.attributes['Wearing_Hat'] > self.categories_and_attributes.thresholds_pred['Wearing_Hat'], + self.attributes['Wearing_Hat']) + glasses = (self.attributes['Eyeglasses'] > self.categories_and_attributes.thresholds_pred['Eyeglasses'], + self.attributes['Eyeglasses']) + earrings = ( + self.attributes['Wearing_Earrings'] > self.categories_and_attributes.thresholds_pred['Wearing_Earrings'], + self.attributes['Wearing_Earrings']) + necklace = ( + self.attributes['Wearing_Necklace'] > self.categories_and_attributes.thresholds_pred['Wearing_Necklace'], + self.attributes['Wearing_Necklace']) + necktie = ( + self.attributes['Wearing_Necktie'] > self.categories_and_attributes.thresholds_pred['Wearing_Necktie'], + self.attributes['Wearing_Necktie']) + + description = "This customer has " + hair_colour_str = 'None' + hair_shape_str = 'None' + if has_hair[0]: + hair_shape_str = '' + if hair_shape[0] == 'Straight_Hair': + hair_shape_str = 'straight' + elif hair_shape[0] == 'Wavy_Hair': + hair_shape_str = 'wavy' + if hair_colour[0] == 'Black_Hair': + description += 'black %s hair, ' % hair_shape_str + hair_colour_str = 'black' + elif hair_colour[0] == 'Blond_Hair': + description += 'blond %s hair, ' % hair_shape_str + hair_colour_str = 'blond' + elif hair_colour[0] == 'Brown_Hair': + description += 'brown %s hair, ' % hair_shape_str + hair_colour_str = 'brown' + elif hair_colour[0] == 'Gray_Hair': + description += 'gray %s hair, ' % hair_shape_str + hair_colour_str = 'gray' + + if male: # here 'male' is only used to determine whether it is confident to decide whether the person has beard + if not facial_hair[0] == 'No_Beard': + description += 'and has beard. ' + + if hat[0] or glasses[0]: + description += 'I can also see this customer wearing ' + if hat[0] and glasses[0]: + description += 'a hat and a pair of glasses. ' + elif hat[0]: + description += 'a hat. ' + else: + description += 'a pair of glasses. ' + + if earrings[0] or necklace[0] or necktie[0]: + description += 'This customer might also wear ' + wearables = [] + if earrings[0]: + wearables.append('a pair of earrings') + if necklace[0]: + wearables.append('a necklace') + if necktie[0]: + wearables.append('a necktie') + description += ", ".join(wearables[:-2] + [" and ".join(wearables[-2:])]) + '. ' + + if description == "This customer has ": + description = "I didn't manage to get any attributes from this customer, I'm sorry." + + result = { + 'attributes': { + 'has_hair': has_hair[0], + 'hair_colour': hair_colour_str, + 'hair_shape': hair_shape_str, + 'male': male[0], + 'facial_hair': facial_hair[0], + 'hat': hat[0], + 'glasses': glasses[0], + 'earrings': earrings[0], + 'necklace': necklace[0], + 'necktie': necktie[0], + }, + 'description': description + } + + result = json.dumps(result, indent=4) + + return result diff --git a/common/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_msgs/CMakeLists.txt index 764829f05..03a9d1748 100644 --- a/common/vision/lasr_vision_msgs/CMakeLists.txt +++ b/common/vision/lasr_vision_msgs/CMakeLists.txt @@ -50,8 +50,6 @@ add_message_files( BodyPixPose.msg BodyPixMask.msg BodyPixMaskRequest.msg - ColourPrediction.msg - FeatureWithColour.msg ) ## Generate services in the 'srv' folder @@ -60,11 +58,11 @@ add_service_files( YoloDetection.srv YoloDetection3D.srv BodyPixDetection.srv - TorchFaceFeatureDetection.srv Recognise.srv LearnFace.srv Vqa.srv PointingDirection.srv + TorchFaceFeatureDetectionDescription.srv ) # Generate actions in the 'action' folder diff --git a/common/vision/lasr_vision_msgs/msg/BodyPixPose.msg b/common/vision/lasr_vision_msgs/msg/BodyPixPose.msg index a8d70142d..8c03dc6df 100644 --- a/common/vision/lasr_vision_msgs/msg/BodyPixPose.msg +++ b/common/vision/lasr_vision_msgs/msg/BodyPixPose.msg @@ -1 +1,2 @@ -# https://github.com/de-code/python-tf-bodypix/blob/develop/tf_bodypix/bodypix_js_utils/types.py +# x and y coordinates +float32[] coord diff --git a/common/vision/lasr_vision_msgs/msg/ColourPrediction.msg b/common/vision/lasr_vision_msgs/msg/ColourPrediction.msg deleted file mode 100644 index 015a789f7..000000000 --- a/common/vision/lasr_vision_msgs/msg/ColourPrediction.msg +++ /dev/null @@ -1,7 +0,0 @@ -# Colour -string colour - -# Distance to the colour -# -# Lower = higher chance -float32 distance diff --git a/common/vision/lasr_vision_msgs/msg/FeatureWithColour.msg b/common/vision/lasr_vision_msgs/msg/FeatureWithColour.msg deleted file mode 100644 index fe9ca3d71..000000000 --- a/common/vision/lasr_vision_msgs/msg/FeatureWithColour.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Feature name -string name - -# Colour predictions -lasr_vision_msgs/ColourPrediction[] colours diff --git a/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetection.srv b/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetection.srv index 2edc1abba..fe7aa0812 100644 --- a/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetection.srv +++ b/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetection.srv @@ -1,5 +1,15 @@ # Image to run inference on sensor_msgs/Image image_raw + +uint8[] head_mask_data # For serialized array data +uint32[] head_mask_shape # To store the shape of the array +string head_mask_dtype # Data type of the array elements + +uint8[] torso_mask_data +uint32[] torso_mask_shape +string torso_mask_dtype --- + # Detection result -lasr_vision_msgs/FeatureWithColour[] detected_features \ No newline at end of file +lasr_vision_msgs/FeatureWithColour[] detected_features +# string detected_features diff --git a/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetectionDescription.srv b/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetectionDescription.srv new file mode 100644 index 000000000..a08f5bb52 --- /dev/null +++ b/common/vision/lasr_vision_msgs/srv/TorchFaceFeatureDetectionDescription.srv @@ -0,0 +1,14 @@ +# Image to run inference on +sensor_msgs/Image image_raw + +uint8[] head_mask_data # For serialized array data +uint32[] head_mask_shape # To store the shape of the array +string head_mask_dtype # Data type of the array elements + +uint8[] torso_mask_data +uint32[] torso_mask_shape +string torso_mask_dtype +--- + +# Detection result +string description diff --git a/common/vision/lasr_vision_torch/nodes/service b/common/vision/lasr_vision_torch/nodes/service deleted file mode 100644 index 4a0144396..000000000 --- a/common/vision/lasr_vision_torch/nodes/service +++ /dev/null @@ -1,70 +0,0 @@ -from lasr_vision_msgs.srv import TorchFaceFeatureDetection, TorchFaceFeatureDetectionRequest, TorchFaceFeatureDetectionResponse -from lasr_vision_msgs.msg import FeatureWithColour, ColourPrediction -from colour_estimation import closest_colours, RGB_COLOURS, RGB_HAIR_COLOURS -from cv2_img import msg_to_cv2_img -from torch_module.helpers import binary_erosion_dilation, median_color_float - -import numpy as np -import torch -import rospy -import lasr_vision_torch - -model = lasr_vision_torch.load_face_classifier_model() - - -def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetectionResponse: - # decode the image - rospy.loginfo('Decoding') - frame = msg_to_cv2_img(request.image_raw) - - # 'hair', 'hat', 'glasses', 'face' - input_image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float() - input_image /= 255.0 - masks_batch_pred, pred_classes = model(input_image) - - thresholds_mask = [ - 0.5, 0.75, 0.25, 0.5, # 0.5, 0.5, 0.5, 0.5, - ] - thresholds_pred = [ - 0.6, 0.8, 0.1, 0.5, - ] - erosion_iterations = 1 - dilation_iterations = 1 - categories = ['hair', 'hat', 'glasses', 'face',] - - masks_batch_pred = binary_erosion_dilation( - masks_batch_pred, thresholds=thresholds_mask, - erosion_iterations=erosion_iterations, dilation_iterations=dilation_iterations - ) - - median_colours = (median_color_float( - input_image, masks_batch_pred).detach().squeeze(0)*255).numpy().astype(np.uint8) - - # discarded: masks = masks_batch_pred.detach().squeeze(0).numpy().astype(np.uint8) - # discarded: mask_list = [masks[i,:,:] for i in range(masks.shape[0])] - - pred_classes = pred_classes.detach().squeeze(0).numpy() - # discarded: class_list = [categories[i] for i in range( - # pred_classes.shape[0]) if pred_classes[i].item() > thresholds_pred[i]] - colour_list = [median_colours[i, :] - for i in range(median_colours.shape[0])] - - response = TorchFaceFeatureDetectionResponse() - response.detected_features = [ - FeatureWithColour(categories[i], [ - ColourPrediction(colour, distance) - for colour, distance - in closest_colours(colour_list[i], RGB_HAIR_COLOURS if categories[i] == 'hair' else RGB_COLOURS) - ]) - for i - in range(pred_classes.shape[0]) - if pred_classes[i].item() > thresholds_pred[i] - ] - - return response - - -rospy.init_node('torch_service') -rospy.Service('/torch/detect/face_features', TorchFaceFeatureDetection, detect) -rospy.loginfo('Torch service started') -rospy.spin() diff --git a/common/vision/lasr_vision_torch/src/lasr_vision_torch/__init__.py b/common/vision/lasr_vision_torch/src/lasr_vision_torch/__init__.py deleted file mode 100644 index fa62dbc3f..000000000 --- a/common/vision/lasr_vision_torch/src/lasr_vision_torch/__init__.py +++ /dev/null @@ -1,21 +0,0 @@ -from torch_module.modules import DeepLabV3PlusMobileNetV3, MultiLabelMobileNetV3Large, CombinedModelNoRegression -from torch_module.helpers import load_torch_model - -import rospkg -from os import path - - -def load_face_classifier_model(): - cat_layers = 4 - # 'cloth', 'hair', 'hat', 'glasses', 'face', - segment_model = DeepLabV3PlusMobileNetV3(num_classes=4) - # 'hair', 'hat', 'glasses', 'face', ; first three with colours, rgb - predict_model = MultiLabelMobileNetV3Large(cat_layers, 7) - model = CombinedModelNoRegression( - segment_model, predict_model, cat_layers=cat_layers) - model.eval() - - r = rospkg.RosPack() - model, _, _, _ = load_torch_model(model, None, path=path.join(r.get_path( - "lasr_vision_torch"), "models", "best_model_epoch_31.pth"), cpu_only=True) - return model diff --git a/skills/launch/unit_test_describe_people.launch b/skills/launch/unit_test_describe_people.launch index 978faaa7b..8ef21961f 100644 --- a/skills/launch/unit_test_describe_people.launch +++ b/skills/launch/unit_test_describe_people.launch @@ -9,7 +9,7 @@ - + diff --git a/skills/scripts/unit_test_describe_people.py b/skills/scripts/unit_test_describe_people.py index 3cf73d34c..364099a28 100644 --- a/skills/scripts/unit_test_describe_people.py +++ b/skills/scripts/unit_test_describe_people.py @@ -16,6 +16,6 @@ sm.execute() - print('\n\nDetected people:', sm.userdata['people']) + print('\n\nDetected people:', sm.userdata['people'][0]['features']) rospy.signal_shutdown("down") diff --git a/skills/src/lasr_skills/describe_people.py b/skills/src/lasr_skills/describe_people.py old mode 100644 new mode 100755 index 762e705c7..1530311fb --- a/skills/src/lasr_skills/describe_people.py +++ b/skills/src/lasr_skills/describe_people.py @@ -5,12 +5,11 @@ import smach import cv2_img import numpy as np - -from colour_estimation import closest_colours, RGB_COLOURS -from lasr_vision_msgs.msg import BodyPixMaskRequest, ColourPrediction, FeatureWithColour -from lasr_vision_msgs.srv import YoloDetection, BodyPixDetection, TorchFaceFeatureDetection - +from lasr_vision_msgs.msg import BodyPixMaskRequest +from lasr_vision_msgs.srv import YoloDetection, BodyPixDetection, TorchFaceFeatureDetectionDescription +from numpy2message import numpy2message from .vision import GetImage, ImageMsgToCv2 +import numpy as np class DescribePeople(smach.StateMachine): @@ -29,7 +28,7 @@ def __init__(self): default_outcome='failed', outcome_map={'succeeded': { 'SEGMENT_YOLO': 'succeeded', 'SEGMENT_BODYPIX': 'succeeded'}}, - input_keys=['img', 'img_msg'], + input_keys=['img', 'img_msg',], output_keys=['people_detections', 'bodypix_masks']) with sm_con: @@ -40,7 +39,7 @@ def __init__(self): 'succeeded': 'FEATURE_EXTRACTION'}) smach.StateMachine.add('FEATURE_EXTRACTION', self.FeatureExtraction(), transitions={ 'succeeded': 'succeeded'}) - + class SegmentYolo(smach.State): ''' Segment using YOLO @@ -73,7 +72,7 @@ class SegmentBodypix(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=[ - 'img_msg'], output_keys=['bodypix_masks']) + 'img_msg',], output_keys=['bodypix_masks']) self.bodypix = rospy.ServiceProxy( '/bodypix/detect', BodyPixDetection) @@ -81,17 +80,17 @@ def execute(self, userdata): try: torso = BodyPixMaskRequest() torso.parts = ["torso_front", "torso_back"] - head = BodyPixMaskRequest() head.parts = ["left_face", "right_face"] - masks = [torso, head] - result = self.bodypix(userdata.img_msg, "resnet50", 0.7, masks) userdata.bodypix_masks = result.masks + rospy.logdebug("Found poses: %s" % str(len(result.poses))) + neck_coord = (int(result.poses[0].coord[0]), int(result.poses[0].coord[1])) + rospy.logdebug("Coordinate of the neck is: %s" % str(neck_coord)) return 'succeeded' except rospy.ServiceException as e: - rospy.logwarn(f"Unable to perform inference. ({str(e)})") + rospy.logerr(f"Unable to perform inference. ({str(e)})") return 'failed' class FeatureExtraction(smach.State): @@ -105,7 +104,7 @@ def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=[ 'img', 'people_detections', 'bodypix_masks'], output_keys=['people']) self.torch_face_features = rospy.ServiceProxy( - '/torch/detect/face_features', TorchFaceFeatureDetection) + '/torch/detect/face_features', TorchFaceFeatureDetectionDescription) def execute(self, userdata): if len(userdata.people_detections) == 0: @@ -121,7 +120,6 @@ def execute(self, userdata): height, width, _ = img.shape people = [] - for person in userdata.people_detections: rospy.logdebug( f"\n\nFound person with confidence {person.confidence}!") @@ -132,15 +130,12 @@ def execute(self, userdata): cv2.fillPoly(mask_image, pts=np.int32( [contours]), color=(255, 255, 255)) mask_bin = mask_image > 128 - - # keep track - features = [] - + # process part masks for (bodypix_mask, part) in zip(userdata.bodypix_masks, ['torso', 'head']): part_mask = np.array(bodypix_mask.mask).reshape( bodypix_mask.shape[0], bodypix_mask.shape[1]) - + # filter out part for current person segmentation try: part_mask[mask_bin == 0] = 0 @@ -155,42 +150,25 @@ def execute(self, userdata): f'|> Person does not have {part} visible') continue - # do colour processing on the torso if part == 'torso': - try: - features.append(FeatureWithColour("torso", [ - ColourPrediction(colour, distance) - for colour, distance - in closest_colours(np.median(img[part_mask == 1], axis=0), RGB_COLOURS) - ])) - except Exception as e: - rospy.logerr(f"Failed to process colour: {e}") - - # do feature extraction on the head - if part == 'head': - try: - # crop out face - face_mask = np.array(userdata.bodypix_masks[1].mask).reshape( - userdata.bodypix_masks[1].shape[0], userdata.bodypix_masks[1].shape[1]) - - mask_image_only_face = mask_image.copy() - mask_image_only_face[face_mask == 0] = 0 - - face_region = cv2_img.extract_mask_region( - img, mask_image_only_face) - if face_region is None: - raise Exception( - "Failed to extract mask region") - - msg = cv2_img.cv2_img_to_msg(face_region) - features.extend(self.torch_face_features( - msg).detected_features) - except Exception as e: - rospy.logerr(f"Failed to process extraction: {e}") + torso_mask = part_mask + elif part == 'head': + head_mask = part_mask + + torso_mask_data, torso_mask_shape, torso_mask_dtype = numpy2message(torso_mask) + head_mask_data, head_mask_shape, head_mask_dtype = numpy2message(head_mask) + + full_frame = cv2_img.cv2_img_to_msg(img) + + rst = self.torch_face_features( + full_frame, + head_mask_data, head_mask_shape, head_mask_dtype, + torso_mask_data, torso_mask_shape, torso_mask_dtype, + ).description people.append({ 'detection': person, - 'features': features + 'features': rst }) # Userdata: @@ -199,6 +177,5 @@ def execute(self, userdata): # - parts # - - part # - mask - userdata['people'] = people return 'succeeded' diff --git a/skills/src/lasr_skills/vision/__init__.py b/skills/src/lasr_skills/vision/__init__.py index 53bf0eb7b..426b1bddf 100644 --- a/skills/src/lasr_skills/vision/__init__.py +++ b/skills/src/lasr_skills/vision/__init__.py @@ -1,3 +1,3 @@ -from .get_image import GetImage +from .get_image import GetImage, GetPointCloud, GetImageAndPointCloud from .image_cv2_to_msg import ImageCv2ToMsg -from .image_msg_to_cv2 import ImageMsgToCv2 +from .image_msg_to_cv2 import ImageMsgToCv2, PclMsgToCv2 diff --git a/skills/src/lasr_skills/vision/get_image.py b/skills/src/lasr_skills/vision/get_image.py index bb4b894c3..6d67ecbc5 100644 --- a/skills/src/lasr_skills/vision/get_image.py +++ b/skills/src/lasr_skills/vision/get_image.py @@ -1,8 +1,7 @@ import os import smach import rospy -from sensor_msgs.msg import Image - +from sensor_msgs.msg import Image, PointCloud2 class GetImage(smach.State): """ @@ -27,3 +26,54 @@ def execute(self, userdata): return 'failed' return 'succeeded' + + +class GetPointCloud(smach.State): + """ + State for acquiring a PointCloud2 message. + """ + + def __init__(self, topic: str = None): + smach.State.__init__( + self, outcomes=['succeeded', 'failed'], output_keys=['pcl_msg']) + + if topic is None: + self.topic = '/xtion/depth_registered/points' + else: + self.topic = topic + + def execute(self, userdata): + try: + userdata.pcl_msg = rospy.wait_for_message( + self.topic, PointCloud2) + except Exception as e: + print(e) + return 'failed' + + return 'succeeded' + + +class GetImageAndPointCloud(smach.State): + """ + State for acquiring Image and PointCloud2 messages simultaneously. + """ + + def __init__(self, topic: str = None): + smach.State.__init__( + self, outcomes=['succeeded', 'failed'], output_keys=['img_msg', 'pcl_msg']) + + self.topic1 = '/xtion/rgb/image_raw' + self.topic2 = '/xtion/depth_registered/points' + + def execute(self, userdata): + try: + userdata.img_msg = rospy.wait_for_message( + self.topic1, Image) + userdata.pcl_msg = rospy.wait_for_message( + self.topic2, PointCloud2) + except Exception as e: + print(e) + return 'failed' + + return 'succeeded' + diff --git a/skills/src/lasr_skills/vision/image_msg_to_cv2.py b/skills/src/lasr_skills/vision/image_msg_to_cv2.py index e079b2e62..3021aebac 100644 --- a/skills/src/lasr_skills/vision/image_msg_to_cv2.py +++ b/skills/src/lasr_skills/vision/image_msg_to_cv2.py @@ -1,6 +1,7 @@ import os import smach import cv2_img +import rospy class ImageMsgToCv2(smach.State): @@ -15,3 +16,18 @@ def __init__(self): def execute(self, userdata): userdata.img = cv2_img.msg_to_cv2_img(userdata.img_msg) return 'succeeded' + + +class PclMsgToCv2(smach.State): + """ + State for converting a sensor Image message to cv2 format + """ + + def __init__(self): + smach.State.__init__( + self, outcomes=['succeeded'], input_keys=['img_msg_3d'], output_keys=['img', 'xyz']) + + def execute(self, userdata): + userdata.img = cv2_img.pcl_msg_to_cv2(userdata.img_msg_3d) + userdata.xyz = cv2_img.pcl_msg_to_xyz(userdata.img_msg_3d) + return 'succeeded' diff --git a/tasks/coffee_shop/config/config:=full.yaml b/tasks/coffee_shop/config/config:=full.yaml deleted file mode 100644 index 2de30b413..000000000 --- a/tasks/coffee_shop/config/config:=full.yaml +++ /dev/null @@ -1,108 +0,0 @@ -counter: - cuboid: - - [0.11478881255836193, 2.8703450451171575] - - [0.044620891454333886, 3.7672813608081324] - - [-0.722736944640509, 3.707870872696769] - - [-0.6525690235364809, 2.810934557005794] - last_updated: '2023-09-19 14:20:13.331419' - location: - orientation: {w: 0.05378161245420122, x: 0.0, y: 0.0, z: 0.998552721773781} - position: {x: 0.7923260692997367, y: 3.1698751043324664, z: 0.0} -tables: - table0: - last_updated: '2023-09-19 14:12:11.785287' - location: - orientation: {w: 0.6981442535161398, x: 0.0, y: 0.0, z: -0.7159571225166993} - position: {x: 3.4185893265341467, y: 1.5007719904983003, z: 0.0} - semantic: end - num_persons: 0 - objects_cuboid: - - [3.872702193443944, 0.6559161243738907] - - [2.97325624499618, 0.6392973444633933] - - [2.9822439064193755, 0.13944712694630035] - - [3.8816898548671395, 0.15606590685679772] - order: [] - persons_cuboid: - - [4.6578294448981765, 1.4704487212105706] - - [2.159368476987721, 1.424285443681411] - - [2.1971166549651433, -0.6750854698903794] - - [4.695577622875598, -0.6289221923612198] - pre_location: - orientation: {w: 0.6981442535161398, x: 0.0, y: 0.0, z: -0.7159571225166993} - position: {x: 3.4185893265341467, y: 1.5007719904983003, z: 0.0} - status: unvisited - table1: - last_updated: '2023-09-19 14:14:23.654079' - location: - orientation: {w: 0.679755237535674, x: 0.0, y: 0.0, z: -0.7334390343053875} - position: {x: 6.294876273276469, y: 2.3564053436919483, z: 0.0} - objects_cuboid: - - [5.975099899520781, 1.6171292380625146] - - [5.916117157155604, 0.04866821248209674] - - [6.682256235074643, 0.021546325976471215] - - [6.74123897743982, 1.590007351556889] - persons_cuboid: - - [5.496391304636297, 2.134250733434652] - - [5.3998399365608165, -0.433230053661956] - - [7.160964829959127, -0.49557516939566626] - - [7.257516198034608, 2.071905617700941] - pre_location: - orientation: {w: 0.679755237535674, x: 0.0, y: 0.0, z: -0.7334390343053875} - position: {x: 6.294876273276469, y: 2.3564053436919483, z: 0.0} - table2: - last_updated: '2023-09-19 14:16:18.722673' - location: - orientation: {w: 0.6988937950671122, x: 0.0, y: 0.0, z: 0.7152254632049179} - position: {x: 6.4837847765967505, y: 3.3319861177542167, z: 0.0} - objects_cuboid: - - [6.182153357080189, 4.241042314587973] - - [6.248667753720983, 4.460477068504972] - - [6.4505652887727765, 4.568103825639009] - - [6.669577124411927, 4.5008762913352] - - [6.777409097641248, 4.298175443423813] - - [6.710894701000455, 4.078740689506813] - - [6.508997165948661, 3.9711139323727758] - - [6.28998533030951, 4.038341466676585] - persons_cuboid: - - [5.487688326425621, 4.174386997612826] - - [5.486032199694549, 5.0903099603404325] - - [6.382394765400911, 5.264592034449616] - - [7.295871631694631, 5.2640198171209525] - - [7.471874128295816, 4.364830760398959] - - [7.473530255026889, 3.4489077976713536] - - [6.577167689320526, 3.2746257235621696] - - [5.663690823026807, 3.2751979408908327] - pre_location: - orientation: {w: 0.6988937950671122, x: 0.0, y: 0.0, z: 0.7152254632049179} - position: {x: 6.4837847765967505, y: 3.3319861177542167, z: 0.0} - table3: - last_updated: '2023-09-19 14:18:50.691359' - location: - orientation: {w: 0.632821856784876, x: 0.0, y: 0.0, z: 0.7742974219092699} - position: {x: 3.7567571172300678, y: 2.983492576363372, z: 0.0} - objects_cuboid: - - [2.9502728886424308, 3.682447992569198] - - [4.519891622610968, 3.715626804870335] - - [4.5034995842988685, 4.485192995360613] - - [2.933880850330331, 4.452014183059477] - persons_cuboid: - - [2.461038491667587, 3.1721631863231843] - - [5.0304143810300985, 3.226475000090013] - - [4.992733981273712, 4.9954778016066275] - - [2.423358091911201, 4.941165987839798] - pre_location: - orientation: {w: 0.632821856784876, x: 0.0, y: 0.0, z: 0.7742974219092699} - position: {x: 3.7567571172300678, y: 2.983492576363372, z: 0.0} -wait: - cuboid: - - [1.9877075290272157, 0.5417149995170536] - - [0.38914681856697353, 0.47853785625496126] - - [0.44870023065244924, -1.020133288151187] - - [2.0472609411126914, -0.9569561448890949] - last_updated: '2023-09-19 14:21:53.764799' - location: - orientation: {w: 0.733502658301428, x: 0.0, y: 0.0, z: -0.6796865823780388} - position: {x: 1.3003716926945177, y: 4.293212965356256, z: 0.0} - pose: - orientation: {w: 0.992020738813, x: 0.0, y: 0.0, z: 0.126074794327} - position: {x: 10.3656408799, y: 25.7369664143, z: 0.0} diff --git a/tasks/receptionist/launch/setup.launch b/tasks/receptionist/launch/setup.launch index 1669854f4..a8fd79ede 100644 --- a/tasks/receptionist/launch/setup.launch +++ b/tasks/receptionist/launch/setup.launch @@ -16,5 +16,8 @@ + + +