Ros2 receptionist get states

common/helpers/tf_pcl/CMakeLists.txt

@@ -7,7 +7,16 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See https://index.ros.org/doc/ros2/Tutorials/Creating-Your-First-ROS2-Package/#create-setup-py
+# ament_python_install_package(${PROJECT_NAME})
+ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

common/helpers/tf_pcl/package.xml

@@ -9,12 +9,17 @@
 
     <buildtool_depend>ament_cmake</buildtool_depend>
 
+    <depend>rclpy</depend>
+    <depend>sensor_msgs</depend>
     <depend>std_msgs</depend>
+    <depend>geometry_msgs</depend>
 
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_lint_common</test_depend>
 
+    <build_depend>ament_virtualenv</build_depend>
     <export>
         <build_type>ament_cmake</build_type>
+        <pip_requirements>requirements.txt</pip_requirements>
     </export>
 </package>

common/helpers/tf_pcl/requirements.txt

@@ -0,0 +1,2 @@
+numpy
+scipy

common/helpers/tf_pcl/setup.py

@@ -1,23 +1,35 @@
 from setuptools import setup
+import setuptools.command.install
+import ament_virtualenv.install
 
 package_name = "tf_pcl"
 
+
+class InstallCommand(setuptools.command.install.install):
+    def run(self):
+        super().run()
+        ament_virtualenv.install.install_venv(
+            install_base=self.install_base,
+            scripts_base=self.install_scripts,
+            package_name=package_name,
+            python_version="3",
+        )
+
+        return
+
+
 setup(
     name=package_name,
     version="0.0.1",
     packages=[package_name],
     package_dir={"": "src"},
-    data_files=[
-        # Install package.xml in the appropriate location
-        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        ("share/" + package_name, ["package.xml"]),
-    ],
     install_requires=["setuptools"],  # Add any Python dependencies here
     zip_safe=True,
     maintainer="Your Name",
     maintainer_email="your_email@example.com",
     description="A ROS 2 package for point cloud transformations",
     license="Apache License 2.0",  # Update license as per your project
     tests_require=["pytest"],  # Add any test dependencies here
+    cmdclass={"install": InstallCommand},
     entry_points={"console_scripts": ["pointcloud_transformer = tf_pcl.__init__:main"]},
 )

common/language/lasr_llm_interfaces/package.xml

@@ -23,6 +23,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
-    <pip_requirements>requirements.txt</pip_requirements>
+    <!--pip_requirements>requirements.txt</pip_requirements-->
   </export>
 </package>

skills/src/lasr_skills/__init__.py

@@ -15,7 +15,7 @@
 from .say import Say
 
 # from .wait_for_person_in_area import WaitForPersonInArea
-# from .describe_people import DescribePeople
+from .describe_people import DescribePeople
 from .look_to_point import LookToPoint
 
 from .play_motion import PlayMotion

tasks/receptionist/config/receptionist_params.yaml

@@ -0,0 +1,22 @@
+receptionist_node: # Must match node (parameters are related to nodes)
+  ros__parameters:    
+    priors:
+      names:
+        - Adel
+        - Angel
+        - Axel
+        - Charlie
+        - Jane
+        - Jules
+        - Morgan
+        - Paris
+        - Robin
+        - Simone
+      drinks:
+        - cola
+        - iced tea
+        - juice pack
+        - milk
+        - orange juice
+        - red wine
+        - tropical juice

tasks/receptionist/launch/setup_launch.xml

@@ -0,0 +1,38 @@
+<launch>
+
+    <!--     PARAMS     -->
+    <!-- <arg name="config" default="lab"/> -->
+    <arg name="config" default="lab" />
+
+    <!--     INTERACTION     -->
+    <!-- <arg name="whisper_device_param" default="9" /> -->
+    <!-- <node pkg="lasr_speech_recognition_whisper" type="transcribe_microphone_server" name="transcribe_speech" output="screen" args="/-/-mic_device $(arg whisper_device_param)"/> -->
+
+    <!-- LLM -->
+    <!-- <node pkg="lasr_llm" exec="llm" name="llm_server" output="screen" /> -->
+
+    <!--     STATIC POINTS     -->
+    <!-- <rosparam command="load" file="$(find receptionist)/config/$(arg config).yaml" ns="receptionist" /> -->
+
+    <!--     MOTIONS     -->
+    <!-- <rosparam command="load" file="$(find lasr_skills)/config/motions.yaml"/> -->
+    <!-- <rosparam command="load" file="$(find receptionist)/config/motions.yaml"/> -->
+
+    <!--      PERCEPTION     -->
+    <!-- <node pkg="lasr_vision_clip" name="clip_service" type="vqa" output="screen"/> -->
+    <!-- <node pkg="lasr_vision_reid" type="service.py" name="reid_service" output="screen"/> -->
+    <!-- <node pkg ="lasr_vision_eye_tracker" type="eye_tracker_server.py" name="eye_tracker_service" output="screen"/> -->
+    <!-- <node name="yolo" pkg="lasr_vision_yolo" type="service.py" output="screen"/> -->
+    <!-- <node pkg="lasr_sentence_embedding" exec="lasr_sentence_embedding_service.py" name="sentence_embedding_service" output="screen"/> -->
+
+    <!-- WEB VIEW -->
+    <!-- <node pkg="web_video_server" type="web_video_server"> -->
+        <!-- <param name="port" value="8081"/> -->
+        <!-- <param name="address" value="localhost"/> -->
+    <!-- </node> -->
+
+    <node pkg="receptionist" exec="string_test" name="receptionist_node" output="screen">
+        <param from="$(find-pkg-share receptionist)/config/$(var config).yaml"/>
+    </node>
+
+</launch>   

tasks/receptionist/package.xml

@@ -1,18 +1,31 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-    <name>tasks</name>
+    <name>receptionist</name>
     <version>0.0.0</version>
     <description>Receptionist task</description>
     <maintainer email="maayan.armony@gmail.com">maayan</maintainer>
     <license>MIT</license>
 
+    <depend>smach</depend>
+    <depend>smach_ros</depend>
+
+    <depend>skills</depend>
+    <depend>cv2_img</depend>
+    <depend>cv2_pcl</depend>
+    <depend>tf_pcl</depend>
+
+    <depend>lasr_llm_interfaces</depend>
+    <depend>lasr_sentence_embedding</depend>
+
     <test_depend>ament_copyright</test_depend>
     <test_depend>ament_flake8</test_depend>
     <test_depend>ament_pep257</test_depend>
     <test_depend>python3-pytest</test_depend>
-
+
+    <build_depend>ament_virtualenv</build_depend>
     <export>
         <build_type>ament_python</build_type>
+        <pip_requirements>requirements.txt</pip_requirements>
     </export>
 </package>

tasks/receptionist/setup.py

@@ -1,21 +1,48 @@
 from setuptools import find_packages, setup
+import setuptools.command.install
+import ament_virtualenv.install
+import os
+from glob import glob
 
 package_name = "receptionist"
 
+
+class InstallCommand(setuptools.command.install.install):
+    def run(self):
+        super().run()
+        ament_virtualenv.install.install_venv(
+            install_base=self.install_base,
+            scripts_base=self.install_scripts,
+            package_name=package_name,
+            python_version="3",
+        )
+        return
+
+
 setup(
     name=package_name,
     version="0.0.0",
-    packages=find_packages(exclude=["test"]),
+    package_dir={"": "src"},
+    packages=find_packages(where="src", exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*")),
+        (os.path.join("share", package_name, "config"), glob("config/*")),
     ],
     install_requires=["setuptools"],
     zip_safe=True,
     maintainer="maayan",
     maintainer_email="maayan.armony@gmail.com",
-    description="REceptionist task",
+    description="Receptionist task",
     license="MIT",
     tests_require=["pytest"],
-    entry_points={"console_scripts": []},
+    cmdclass={"install": InstallCommand},
+    entry_points={
+        "console_scripts": [
+            "llm_test = receptionist.states.test_llm:main",
+            "string_test = receptionist.states.test_string:main",
+            "llm_and_sentence_test = receptionist.states.test_llm_and_sentence:main",
+        ]
+    },
 )

tasks/receptionist/src/receptionist/states/__init__.py

@@ -9,11 +9,11 @@
 from .get_name_or_drink import GetNameOrDrink
 
 # from .handle_guest import HandleGuest
-from .pointcloud_sweep import PointCloudSweep
+# from .pointcloud_sweep import PointCloudSweep
 
 # from .run_and_process_detections import RunAndProcessDetections
 # from .recognise_people import RecognisePeople
 # from .seat_guest import SeatGuest
-from .check_sofa import CheckSofa
+# from .check_sofa import CheckSofa
 
 # from .introduce_and_seat_guest import IntroduceAndSeatGuest

tasks/receptionist/src/receptionist/states/get_attributes.py

@@ -0,0 +1,80 @@
+import smach
+from smach import UserData, StateMachine
+from smach_ros import RosState
+
+from skills import DescribePeople
+import json
+
+
+class GetGuestAttributes(StateMachine):
+    class InitialiseDetectionFlag(RosState):
+        def __init__(self, node, guest_id: str):
+            RosState.__init__(
+                self,
+                node,
+                outcomes=["succeeded", "failed"],
+                input_keys=["guest_data", "clip_detection_dict"],
+                output_keys=["guest_data"],
+            )
+
+            self._guest_id: str = guest_id
+
+        def execute(self, userdata: UserData) -> str:
+            try:
+                userdata.guest_data[self._guest_id]["detection"] = False
+                return "succeeded"
+            except Exception as e:
+                print(e)
+                return "failed"
+
+    class HandleGuestAttributes(RosState):
+        def __init__(self, node, guest_id: str):
+            RosState.__init__(
+                self,
+                node,
+                outcomes=["succeeded", "failed"],
+                input_keys=["guest_data", "clip_detection_dict"],
+                output_keys=["guest_data"],
+            )
+
+            self._guest_id: str = guest_id
+
+        def execute(self, userdata: UserData) -> str:
+            userdata.guest_data[self._guest_id][
+                "attributes"
+            ] = userdata.clip_detection_dict
+            userdata.guest_data[self._guest_id]["detection"] = True
+            return "succeeded"
+
+    def __init__(self, node, guest_id: str):
+        StateMachine.__init__(
+            self,
+            outcomes=["succeeded", "failed"],
+            input_keys=["guest_data"],
+            output_keys=["guest_data"],
+        )
+        self._guest_id: str = guest_id
+        self.__node = node
+
+        with self:
+            StateMachine.add(
+                "INITIALISE_DETECTION_FLAG",
+                self.InitialiseDetectionFlag(self.__node, self._guest_id),
+                transitions={
+                    "succeeded": "GET_GUEST_ATTRIBUTES",
+                    "failed": "GET_GUEST_ATTRIBUTES",
+                },
+            )
+            StateMachine.add(
+                "GET_GUEST_ATTRIBUTES",
+                DescribePeople(self.__node),
+                transitions={
+                    "succeeded": "HANDLE_GUEST_ATTRIBUTES",
+                    "failed": "failed",
+                },
+            )
+            StateMachine.add(
+                "HANDLE_GUEST_ATTRIBUTES",
+                self.HandleGuestAttributes(self.__node, self._guest_id),
+                transitions={"succeeded": "succeeded", "failed": "failed"},
+            )