Demo with Dualshock 4

.gitignore

@@ -11,4 +11,6 @@
 .prev-vscode
 **.pt
 **.pth
-**.gz
+**.gz
+hri/packages/nlp/assets/**
+!hri/packages/nlp/assets/download-model.bash

.gitmodules

@@ -10,4 +10,9 @@
 	branch = humble
 [submodule "manipulation/packages/pymoveit2"]
 	path = manipulation/packages/pymoveit2
-	url = https://github.com/AndrejOrsula/pymoveit2
+	url = https://github.com/AndrejOrsula/pymoveit2
+
+[submodule "navigation/packages/PlayStation-JoyInterface-ROS2"]
+	path = navigation/packages/PlayStation-JoyInterface-ROS2
+	url = https://github.com/HarvestX/PlayStation-JoyInterface-ROS2.git
+	branch = humble

docker/hri/Dockerfile.stt

@@ -6,11 +6,7 @@ COPY ../../hri/packages/speech/scripts/stt/requirements.txt .
 
 # Install dependencies
 RUN apt-get update && apt-get install -y \
-    libsndfile1 \
-    ffmpeg \
     portaudio19-dev \
     && rm -rf /var/lib/apt/lists/*
 
-RUN pip install --no-cache-dir -r requirements.txt
-
-RUN apt-get update && apt-get install -y ffmpeg
+RUN pip install --no-cache-dir -r requirements.txt

docker/hri/Dockerfile.stt-l4t

@@ -0,0 +1,40 @@
+FROM dustynv/l4t-pytorch:r36.4.0
+
+# Install dependencies
+RUN apt-get update && apt-get install -y \
+libsndfile1 \
+libxml2-dev \
+portaudio19-dev \
+git \
+cmake \
+g++ \
+&& rm -rf /var/lib/apt/lists/*
+
+# Install CTranslate2 from source
+WORKDIR /tmp
+RUN git clone --recursive https://github.com/OpenNMT/CTranslate2.git
+WORKDIR /tmp/CTranslate2
+RUN mkdir build
+WORKDIR /tmp/CTranslate2/build
+RUN cmake .. -DOPENMP_RUNTIME=NONE -DWITH_CUDA=ON -DWITH_MKL=OFF -DWITH_CUDNN=ON
+RUN make -j4
+RUN make install
+RUN ldconfig
+WORKDIR /tmp/CTranslate2/python
+RUN pip install -r install_requirements.txt
+RUN python3 setup.py bdist_wheel
+RUN pip install dist/*.whl
+
+# Install PyAV from source
+WORKDIR /tmp
+RUN git clone https://github.com/PyAV-Org/PyAV.git
+WORKDIR /tmp/PyAV
+RUN pip install virtualenv
+RUN bash -c "source ./scripts/activate.sh && ./scripts/build-deps && make && deactivate && pip install ."
+
+# Install additional Python dependencies
+WORKDIR /app
+COPY ../../hri/packages/speech/scripts/stt/requirements.txt .
+RUN python3 -m pip install --upgrade pip
+RUN pip install --no-cache-dir -r /app/requirements.txt
+RUN pip install --upgrade protobuf

docker/hri/docker-compose.yml

@@ -1,7 +1,8 @@
 include:
   # Jetson Orin
   - path: l4t-devices.yaml
-  - path: stt.yaml
+  - path: stt-l4t.yaml
+  - path: ollama-jetson.yaml
   # - path: chromadb.yaml
 
   # Laptop

docker/hri/ollama-jetson.yaml

@@ -0,0 +1,13 @@
+services:
+  ollama:
+    container_name: home2-hri-ollama
+    image: dustynv/ollama:r36.4.0
+    runtime: nvidia
+    network_mode: host
+    volumes:
+      - ../../hri/packages/nlp/assets:/ollama
+    environment:
+      - OLLAMA_MODELS=/ollama
+    stdin_open: true
+    tty: true
+    restart: unless-stopped

docker/hri/ollama.yaml

@@ -0,0 +1,13 @@
+services:
+  ollama:
+    container_name: home2-hri-ollama
+    image: ollama/ollama
+    runtime: nvidia
+    network_mode: host
+    volumes:
+      - ../../hri/packages/nlp/assets:/ollama
+    environment:
+      - OLLAMA_MODELS=/ollama
+    stdin_open: true
+    tty: true
+    restart: unless-stopped

docker/hri/stt-l4t.yaml

@@ -0,0 +1,15 @@
+services:
+  stt-l4t:
+    container_name: home2-hri-stt-l4t
+    image: roborregos/home2:hri-stt-l4t
+    runtime: nvidia
+    build:
+      context: ../..
+      dockerfile: docker/hri/Dockerfile.stt-l4t
+    ports:
+      - 50051:50051
+    volumes:
+      - ../../hri/packages/speech/scripts/stt/:/app
+      - ../../hri/packages/speech/speech/:/app/speech
+    command: >
+      bash -c "source /tmp/PyAV/scripts/activate.sh && deactivate && python3 -u faster-whisper-l4t.py --port 50051 --model_size base.en"

docker/hri/stt.yaml

@@ -10,5 +10,4 @@ services:
     volumes:
       - ../../hri/packages/speech/scripts/stt/:/app
       - ../../hri/packages/speech/speech/:/app/speech
-
-    command: ["bash", "-c", "python3 -u Faster-whisper.py --port 50051 --model_size base.en"]
+    command: ["bash", "-c", "python3 -u faster-whisper.py --port 50051 --model_size base.en"]

docker/navigation/Dockerfile.cpu

@@ -0,0 +1,2 @@
+ARG BASE_IMAGE=ubuntu:22.04
+FROM ${BASE_IMAGE}

docker/navigation/Dockerfile.cuda

@@ -0,0 +1,3 @@
+ARG BASE_IMAGE=nvidia/cuda:11.4.1-cudnn8-devel-ubuntu20.04
+FROM ${BASE_IMAGE}
+

docker/navigation/Dockerfile.jetson

@@ -0,0 +1,7 @@
+ARG BASE_IMAGE=dustynv/l4t-pytorch:r36.4.0
+FROM ${BASE_IMAGE}
+
+USER ros
+RUN sudo usermod -a -G video ros
+RUN newgrp video
+WORKDIR /workspace

docker/navigation/docker-compose.yaml

@@ -0,0 +1,29 @@
+services:
+  navigation:
+    container_name: home2-navigation
+    build:
+      context: ../..
+      dockerfile: ${DOCKERFILE:-docker/navigation/Dockerfile.cpu}
+      args:
+        BASE_IMAGE: ${BASE_IMAGE:-roborregos/home2:cpu_base} 
+    image: ${IMAGE_NAME:-roborregos/home2:vision-cpu}
+    volumes:
+      - ../../:/workspace
+      - /dev:/dev
+      - /dev/video0:/dev/video0
+      - /tmp/.X11-unix:/tmp/.X11-unix
+
+    network_mode: host
+    user: 1000:1000
+    privileged: true
+
+    environment:
+      DISPLAY: ${DISPLAY}
+      TERM: xterm-256color
+      QT_X11_NO_MITSHM: 1
+      QT_DEBUG_PLUGINS: 1
+
+    tty: true
+    stdin_open: true    
+    working_dir: /workspace
+    command: ["/bin/bash", "-i", "-l"]

docker/navigation/run.sh

@@ -0,0 +1,122 @@
+#!/bin/bash
+
+
+#_________________________BUILD_________________________
+
+# Image names
+CPU_IMAGE="roborregos/home2:cpu_base"
+CUDA_IMAGE="roborregos/home2:gpu_base"
+
+# Function to check if an image exists
+check_image_exists() {
+    local image_name=$1
+    if ! docker images --format "{{.Repository}}:{{.Tag}}" | grep -q "^${image_name}$"; then
+        echo "Image $image_name does not exist. Building it..."
+        return 1  # Image doesn't exist
+    else
+        echo "Image $image_name already exists. Skipping build."
+        return 0  # Image exists
+    fi
+}
+
+# Check type of environment (CPU, GPU, or Jetson), default CPU
+ENV_TYPE="cpu"
+
+# Check device type
+if [[ -f /etc/nv_tegra_release ]]; then
+    ENV_TYPE="jetson"
+else
+    # Check if NVIDIA GPUs are available
+    if command -v nvidia-smi > /dev/null 2>&1; then
+        if nvidia-smi > /dev/null 2>&1; then
+            ENV_TYPE="gpu"
+        fi
+    fi
+fi
+echo "Detected environment: $ENV_TYPE"
+> .env
+# Write environment variables to .env file for Docker Compose and build base images
+case $ENV_TYPE in
+  "cpu")
+    #_____CPU_____
+    echo "DOCKERFILE=docker/navigation/Dockerfile.cpu" >> .env
+    echo "BASE_IMAGE=roborregos/home2:cpu_base" >> .env
+    echo "IMAGE_NAME=roborregos/home2:navigation-cpu" >> .env
+
+    # Build the base image if it doesn't exist
+    check_image_exists "$CPU_IMAGE"
+    if [ $? -eq 1 ]; then
+        docker compose -f ../cpu.yaml build
+    fi
+    ;;
+  "gpu")
+    #_____GPU_____
+    echo "DOCKERFILE=docker/navigation/Dockerfile.gpu" >> .env
+    echo "BASE_IMAGE=roborregos/home2:gpu_base" >> .env
+    echo "IMAGE_NAME=roborregos/home2:navigation-gpu" >> .env
+
+    # Build the base image if it doesn't exist
+    check_image_exists "$CUDA_IMAGE"
+    if [ $? -eq 1 ]; then
+        docker compose -f ../cuda.yaml build
+    fi
+
+    ;;
+  "jetson")
+    #_____Jetson_____
+    echo "DOCKERFILE=docker/navigation/Dockerfile.jetson" >> .env
+    echo "BASE_IMAGE=roborregos/home2:l4t_base" >> .env
+    echo "IMAGE_NAME=roborregos/home2:navigation-jetson" >> .env
+    ;;
+  *)
+    echo "Unknown environment type!"
+    exit 1
+    ;;
+esac
+
+
+#_________________________SETUP_________________________
+
+# Export user
+export LOCAL_USER_ID=$(id -u)
+export LOCAL_GROUP_ID=$(id -g)
+
+# Remove current install, build and log directories if they exist 
+# if [ -d "../../install" ] || [ -d "../../log" ] || [ -d "../../build" ]; then
+#   read -p "Do you want to delete 'install', 'log', and 'build' directories (Recommended for first build)? (y/n): " confirmation
+#   if [[ "$confirmation" == "y" ]]; then
+#     rm -rf ../../install/ ../../log/ ../../build/
+#     echo "Directories deleted."
+#   else
+#     echo "Operation cancelled."
+#   fi
+# fi
+
+# Setup camera permissions
+if [ -e /dev/video0 ]; then
+    echo "Setting permissions for /dev/video0..."
+    sudo chmod 666 /dev/video0  # Allow the container to access the camera device
+fi
+
+#_________________________RUN_________________________
+
+SERVICE_NAME="navigation"  # Change this to your service name in docker-compose.yml
+
+# Check if the container exists
+EXISTING_CONTAINER=$(docker ps -a -q -f name=$SERVICE_NAME)
+if [ -z "$EXISTING_CONTAINER" ]; then
+    echo "No container with the name $SERVICE_NAME exists. Building and starting the container now..."
+    docker compose up --build -d
+fi
+
+# Check if the container is running
+RUNNING_CONTAINER=$(docker ps -q -f name=$SERVICE_NAME)
+
+if [ -n "$RUNNING_CONTAINER" ]; then
+    echo "Container $SERVICE_NAME is already running. Executing bash..."
+    docker compose exec -it $SERVICE_NAME /bin/bash
+else
+    echo "Container $SERVICE_NAME is stopped. Starting it now..."
+    docker compose up --build -d
+    docker compose exec $SERVICE_NAME /bin/bash
+fi

frida_interfaces/CMakeLists.txt

@@ -8,7 +8,7 @@ find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(action_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
+find_package(shape_msgs REQUIRED)
 find_package(unique_identifier_msgs REQUIRED)
 
 file(GLOB_RECURSE action_files RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "*.action")
@@ -20,7 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   ${action_files}
   ${srv_files}
   ${msg_files}
-  DEPENDENCIES std_msgs unique_identifier_msgs action_msgs geometry_msgs sensor_msgs
+  DEPENDENCIES std_msgs unique_identifier_msgs action_msgs geometry_msgs sensor_msgs shape_msgs
 )
 
 # Export dependencies for other packages

frida_interfaces/hri/srv/AddItem.srv

@@ -1,7 +1,7 @@
 # AddItem.srv
-string[] document 
-string[] id  
+string document 
 string collection
+string metadata
 ---
 bool success  # Whether the operation was successful
 string message  # A message describing the result

frida_interfaces/hri/srv/QueryItem.srv

@@ -2,7 +2,9 @@
 string query  # The search query
 string collection
 int16 topk #number of results 
+string where
 ---
 string[] results  # List of results (documents) that match the query, top k
+string[] metadata 
 bool success  # Whether the operation was successful
 string message  # A message describing the result

frida_interfaces/manipulation/srv/AddCollisionObject.srv

@@ -0,0 +1,8 @@
+string id
+string type
+geometry_msgs/PoseStamped pose
+geometry_msgs/Point dimensions # length, width, height
+shape_msgs/Mesh mesh
+string path_to_mesh
+---
+bool success

frida_interfaces/manipulation/srv/RemoveCollisionObject.srv

@@ -0,0 +1,3 @@
+string id
+---
+bool success

frida_interfaces/manipulation/srv/ToggleServo.srv

@@ -0,0 +1,3 @@
+bool state
+---
+bool success

frida_interfaces/package.xml

@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>action_msgs</depend>
+  <depend>shape_msgs</depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>