Merge pull request #50 from juandpenan/rolling

Added Jazzy compilation

.docker/Dockerfile

@@ -0,0 +1,89 @@
+# Dockerfile for ROSCon 2024 Deliberation Workshop
+
+FROM ros:jazzy AS robocup2024
+SHELL ["/bin/bash", "-o", "pipefail", "-c"]
+
+# Install apt dependencies.
+ARG DEBIAN_FRONTEND=noninteractive
+
+
+RUN apt-get update \
+    && apt-get -y --quiet --no-install-recommends install \
+    gcc \
+    git \
+    wget \
+    portaudio19-dev \
+    python3 \
+    python3-pip \
+    ros-jazzy-moveit \
+    nlohmann-json3-dev
+
+
+# Create a ROS 2 workspace.
+RUN mkdir -p /robocup/src/
+WORKDIR /robocup
+
+ADD https://raw.githubusercontent.com/mgonzs13/audio_common/refs/heads/main/requirements.txt /robocup/requirements1.txt
+ADD https://raw.githubusercontent.com/mgonzs13/whisper_ros/refs/heads/main/requirements.txt /robocup/requirements2.txt
+ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt
+ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements4.txt
+ADD https://raw.githubusercontent.com/mgonzs13/tts_ros/refs/heads/main/requirements.txt /robocup/requirements5.txt
+
+# Install external dependencies.
+RUN pip install -r requirements1.txt --break-system-packages
+RUN pip install -r requirements2.txt --break-system-packages
+RUN pip install -r requirements3.txt --break-system-packages
+RUN pip install -r requirements4.txt --break-system-packages
+RUN pip install -r requirements5.txt --break-system-packages
+
+
+WORKDIR /robocup/src
+RUN git clone https://github.com/CoreSenseEU/CoreSense4Home.git -b jazzy
+RUN vcs import --recursive < ./CoreSense4Home/robocup_bringup/thirdparty.repos
+
+WORKDIR /robocup
+RUN rosdep install --from-paths src --ignore-src -r -y
+
+# Set up the entrypoint.
+# Install CUDA nvcc
+ARG USE_CUDA
+ARG CUDA_VERSION=12-6
+
+RUN if [ "$USE_CUDA" = "1" ]; then \
+    wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/cuda-keyring_1.1-1_all.deb && \
+    dpkg -i cuda-keyring_1.1-1_all.deb && \
+    rm cuda-keyring_1.1-1_all.deb; \
+    apt-get update && apt-get install -y cuda-toolkit-$CUDA_VERSION; \
+    echo "export PATH=/usr/local/cuda/bin${PATH:+:${PATH}}" >> ~/.bashrc; \
+    echo "export LD_LIBRARY_PATH=/usr/local/cuda/lib64${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}" >> ~/.bashrc; \
+    fi
+
+# Install gdp grasp generator library
+WORKDIR /robocup/src/ThirdParty/gpd
+RUN mkdir build
+WORKDIR /robocup/src/ThirdParty/gpd/build
+RUN cmake ..
+RUN make -j
+RUN make install
+
+# Colcon the ws
+WORKDIR /robocup
+FROM robocup2024 AS builder
+ARG CMAKE_BUILD_TYPE=Release
+
+ENV PATH=/usr/local/cuda/bin${PATH:+:${PATH}}
+ENV LD_LIBRARY_PATH=/usr/local/cuda/lib64
+
+RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \
+    if [ "$USE_CUDA" = "1" ]; then \
+    source ~/.bashrc && \
+    colcon build --packages-skip tts_bringup --cmake-args -DGGML_CUDA=ON; \
+    else \
+    colcon build --packages-skip tts_bringup; \
+    fi
+
+# Source the ROS 2 setup file
+RUN echo "source /root/robocup/install/setup.bash" >> ~/.bashrc
+
+# Run a default command, e.g., starting a bash shell
+CMD ["bash"]

.github/workflows/jazzy.yaml

@@ -0,0 +1,26 @@
+name: jazzy
+
+on:
+  pull_request:
+    branches:
+      - jazzy
+  push:
+    branches:
+      - jazzy
+  schedule:
+      - cron: '0 0 * * 6'     
+jobs:
+  docker_build:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v4
+
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+
+      - name: Build Docker
+        uses: docker/build-push-action@v6
+        with:
+          file: .docker/Dockerfile
+          push: false

.github/workflows/main.yaml

@@ -20,18 +20,15 @@ jobs:
       - name: Setup ROS 2
         uses: ros-tooling/[email protected]
         with:
-          required-ros-distributions: humble
+          required-ros-distributions: jazzy
       - name: Install nlohmann
         run: sudo apt -y install nlohmann-json3-dev
-      - name: Install ceres dependencies
-        run: sudo apt -y install libunwind-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libgflags-dev
-      - name: Install ceres
-        run: sudo apt -y install libceres-dev
+      - name 
       - name: build and test
         uses: ros-tooling/[email protected]
         with:
           package-name: arm bt_test hri motion perception robocup_bringup cs4home_core
-          target-ros2-distro: humble
+          target-ros2-distro: jazzy
           colcon-defaults: |
             {
               "test": {

bt_nodes/bt_test/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(arm REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(hri REQUIRED)
 find_package(perception_system_interfaces REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 # find_package(backward_ros REQUIRED)
 
 set(CMAKE_CXX_STANDARD 17)
@@ -45,6 +46,7 @@ set(dependencies
   arm
   geometry_msgs
   perception_system_interfaces
+  ament_index_cpp
   # backward_ros
 )
 

bt_nodes/bt_test/package.xml

@@ -25,6 +25,7 @@
   <depend>tf2_ros</depend>
   <depend>hri</depend>
   <depend>perception_system_interfaces</depend>
+  <depend>ament_index_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

bt_nodes/configuration/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(yolov8_msgs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 
 set(CMAKE_CXX_STANDARD 17)
 
@@ -28,6 +29,7 @@ set(dependencies
   tf2
   tf2_ros
   yolov8_msgs
+  ament_index_cpp
 )
 
 include_directories(include)

bt_nodes/configuration/package.xml

@@ -16,6 +16,8 @@
   <depend>behaviortree_cpp_v3</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>yolov8_msgs</depend>
+  <depend>ament_index_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

bt_nodes/hri/include/hri/check_policy.hpp

@@ -47,7 +47,8 @@ class CheckPolicy
     return BT::PortsList(
       {BT::InputPort<std::string>("question"),
         BT::InputPort<std::string>("image_topic"),
-        BT::OutputPort<bool>("output")});
+        BT::OutputPort<bool>("output"),
+        BT::OutputPort<std::string>("output_text")});
   }
 
 private:

bt_nodes/hri/include/hri/dialog/store_detection.hpp

@@ -43,19 +43,22 @@ class StoreDetection : public BT::ActionNodeBase
         BT::InputPort<std::string>("drink"),
         BT::InputPort<std::string>("guest_id"),
         BT::InputPort<std::int64_t>("guest_color_id"),
+        BT::InputPort<std::string>("guest_description"),
 
         BT::OutputPort<std::string>("name_1"),
         BT::OutputPort<std::string>("name_2"),
         BT::OutputPort<std::string>("drink_1"),
         BT::OutputPort<std::string>("drink_2"),
         BT::OutputPort<std::int64_t>("guest_color_id_1"),
-        BT::OutputPort<std::int64_t>("guest_color_id_2")});
+        BT::OutputPort<std::int64_t>("guest_color_id_2"),
+        BT::OutputPort<std::string>("guest_description_id_1"),
+        BT::OutputPort<std::string>("guest_description_id_2")});
   }
 
 private:
   std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
 
-  std::string current_name_, current_drink_, name_1_, name_2_, drink_1_, drink_2_;
+  std::string current_name_, current_drink_, name_1_, name_2_, drink_1_, drink_2_, current_description_;
   std::string current_id_ = "0";
   std::int64_t current_color_id_ = 0;
 };

bt_nodes/hri/src/hri/check_policy.cpp

@@ -101,6 +101,7 @@ BT::NodeStatus CheckPolicy::on_success()
     return BT::NodeStatus::FAILURE;
   }
   std::string answer = result_.result->response.text;
+  setOutput("output_text", answer);
 
   answer.erase(
     std::remove_if(
@@ -121,7 +122,6 @@ BT::NodeStatus CheckPolicy::on_success()
     RCLCPP_ERROR(node_->get_logger(), "Not a valid answer: %s", answer.c_str());
     return BT::NodeStatus::FAILURE;
   }
-
   setOutput("output", value_);
   return BT::NodeStatus::SUCCESS;
 }

bt_nodes/hri/src/hri/dialog/DialogConfirmation.cpp

@@ -54,16 +54,16 @@ void DialogConfirmation::on_tick()
 
 BT::NodeStatus DialogConfirmation::on_success()
 {
-  fprintf(stderr, "%s\n", result_.result->text.c_str());
+  fprintf(stderr, "%s\n", result_.result->transcription.text.c_str());
 
-  if (result_.result->text.size() == 0) {
+  if (result_.result->transcription.text.size() == 0) {
     return BT::NodeStatus::FAILURE;
   }
 
   std::transform(
-    result_.result->text.begin(), result_.result->text.end(), result_.result->text.begin(),
+    result_.result->transcription.text.begin(), result_.result->transcription.text.end(), result_.result->transcription.text.begin(),
     [](unsigned char c) {return std::tolower(c);});
-  if (result_.result->text.find("yes") != std::string::npos) {
+  if (result_.result->transcription.text.find("yes") != std::string::npos) {
     return BT::NodeStatus::SUCCESS;
   } else {
     return BT::NodeStatus::FAILURE;

bt_nodes/hri/src/hri/dialog/Listen.cpp

@@ -51,13 +51,13 @@ void Listen::on_tick()
 
 BT::NodeStatus Listen::on_success()
 {
-  fprintf(stderr, "%s\n", result_.result->text.c_str());
+  fprintf(stderr, "%s\n", result_.result->transcription.text.c_str());
 
-  if (result_.result->text.size() == 0) {
+  if (result_.result->transcription.text.size() == 0) {
     return BT::NodeStatus::FAILURE;
   }
 
-  setOutput("listen_text", result_.result->text);
+  setOutput("listen_text", result_.result->transcription.text);
   return BT::NodeStatus::SUCCESS;
 }
 

bt_nodes/hri/src/hri/dialog/store_detection.cpp

@@ -37,6 +37,7 @@ BT::NodeStatus StoreDetection::tick()
   getInput("drink", current_drink_);
   getInput("guest_id", current_id_);
   getInput("guest_color_id", current_color_id_);
+  getInput("guest_description", current_description_);
 
   if (current_name_.empty() || current_drink_.empty()) {
     return BT::NodeStatus::FAILURE;
@@ -48,13 +49,15 @@ BT::NodeStatus StoreDetection::tick()
     setOutput("name_1", name_1_);
     setOutput("drink_1", drink_1_);
     setOutput("guest_color_id_1", current_color_id_);
+    setOutput("guest_description_id_1", current_description_);
     return BT::NodeStatus::SUCCESS;
   } else if (current_id_ == "2") {
     name_2_ = current_name_;
     drink_2_ = current_drink_;
     setOutput("name_2", name_2_);
     setOutput("drink_2", drink_2_);
     setOutput("guest_color_id_2", current_color_id_);
+    setOutput("guest_description_id_2", current_description_);
     return BT::NodeStatus::SUCCESS;
   }
   return BT::NodeStatus::SUCCESS;

bt_nodes/motion/package.xml

@@ -19,7 +19,7 @@
   <depend>nav2_behavior_tree</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>navigation_system_interfaces</depend>
-  <depend>manipultaion_interfaces</depend>
+  <depend>manipulation_interfaces</depend>
   <depend>slam_toolbox</depend>
 
   <depend>attention_system</depend>

bt_nodes/motion/src/motion/navigation/clear_map_at_goal.cpp

@@ -14,7 +14,7 @@
 
 #include "motion/navigation/clear_map_at_goal.hpp"
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <string>
 #include <utility>

bt_nodes/motion/src/motion/navigation/follow_entity.cpp

@@ -14,7 +14,7 @@
 
 #include "motion/navigation/follow_entity.hpp"
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <string>
 #include <utility>

bt_nodes/motion/src/motion/navigation/goal_publisher.cpp

@@ -14,7 +14,7 @@
 
 #include "motion/navigation/goal_publisher.hpp"
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <string>
 #include <utility>

bt_nodes/perception/include/perception/IsDetected.hpp

@@ -35,7 +35,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 #include "sensor_msgs/msg/image.hpp"
-#include "cv_bridge/cv_bridge.h"
+#include "cv_bridge/cv_bridge.hpp"
 
 namespace perception
 {

bt_nodes/perception/src/perception/IsDetected.cpp

@@ -72,7 +72,7 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura
     bb_img_pub_ = node_->create_publisher<sensor_msgs::msg::Image>(
       "/bb_img_best_detection", 10);
     img_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
-      "/camera/color/image_raw", 10,
+      "/head_front_camera/rgb/image_raw", 10,
       std::bind(&IsDetected::image_callback, this, _1));
   } else {
     bb_img_pub_ = nullptr;
@@ -243,8 +243,8 @@ BT::NodeStatus IsDetected::tick()
     // cv::circle(last_image_, center2d, 5, cv::Scalar(0, 0, 255), -1);
 
     cv::putText(
-      last_image_, "X", center2d, cv::FONT_HERSHEY_SIMPLEX, 1,
-      cv::Scalar(0, 0, 255), 2);
+      last_image_, "[X]", center2d, cv::FONT_HERSHEY_SIMPLEX, 1.0,
+      cv::Scalar(0, 0, 0), 1);
 
     auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", last_image_).toImageMsg();
     bb_img_pub_->publish(*msg);