restore manipulation server launch

Author: juandpenan

bt_nodes/hri/src/hri/check_policy.cpp

@@ -17,6 +17,9 @@
 #include <nlohmann/json.hpp>
 #include <string>
 #include <utility>
+#include <regex>
+#include <algorithm>
+#include <cctype>
 
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "hri/check_policy.hpp"
@@ -49,9 +52,8 @@ void CheckPolicy::on_tick()
   RCLCPP_DEBUG(node_->get_logger(), "CheckPolicy ticked");
   RCLCPP_INFO(node_->get_logger(), "CheckPolicy ticked");
   if (!image_) {
-    RCLCPP_ERROR(node_->get_logger(), "No image received");
-    RCLCPP_INFO(node_->get_logger(), "No image received, setting to IDLE");
-    setStatus(BT::NodeStatus::IDLE);
+    RCLCPP_ERROR(node_->get_logger(), "No image received yet");
+    goal_.prompt.clear();
     return;
   }
   RCLCPP_INFO(node_->get_logger(), "Image received, proceeding with CheckPolicy");
@@ -61,6 +63,7 @@ void CheckPolicy::on_tick()
 
   std::string prompt_ = text_;
   goal_.prompt = prompt_;
+  goal_.images.clear();
   goal_.images.push_back(*image_);
   goal_.reset = true;
   goal_.sampling_config.temp = 0.0;
@@ -99,6 +102,61 @@ void CheckPolicy::image_callback(
   RCLCPP_INFO_ONCE(node_->get_logger(), "Image received in CheckPolicy");
 }
 
+std::string trim_copy(const std::string & s)
+{
+  auto start = std::find_if_not(s.begin(), s.end(),
+    [](unsigned char c) { return std::isspace(c); });
+  auto end = std::find_if_not(s.rbegin(), s.rend(),
+    [](unsigned char c) { return std::isspace(c); }).base();
+
+  if (start >= end) {
+    return "";
+  }
+  return std::string(start, end);
+}
+
+std::string sanitize_llm_output(std::string text)
+{
+  // 1) quitar bloques <think>...</think>
+  text = std::regex_replace(text, std::regex(R"(<think>.*?</think>)"), "");
+
+  // 2) quitar tags sueltos <think>, </think> y cualquier otro <...>
+  text = std::regex_replace(text, std::regex(R"(</?think>)"), "");
+  text = std::regex_replace(text, std::regex(R"(<[^>]+>)"), "");
+
+  // 3) si existe 'the guest is', quedarse desde ahí
+  // std::string anchor = "the guest is";
+  // auto pos = text.find(anchor);
+  // if (pos != std::string::npos) {
+  //   text = text.substr(pos);
+  // }
+
+  // 4) quitar saltos de línea
+  text = std::regex_replace(text, std::regex(R"([\r\n\t]+)"), " ");
+
+  // 5) quitar comillas y paréntesis
+  text = std::regex_replace(text, std::regex(R"(["])"), "");
+  text = std::regex_replace(text, std::regex(R"([()])"), "");
+  text = std::regex_replace(text, std::regex(R"([!])"), "");
+
+  // 6) cambiar guiones por espacio
+  text = std::regex_replace(text, std::regex(R"(-)"), " ");
+
+  // 7) colapsar espacios múltiples
+  text = std::regex_replace(text, std::regex(R"(\s{2,})"), " ");
+
+  // 8) trim
+  text = trim_copy(text);
+
+  // 9) quedarnos con una sola frase si hay varias
+  // auto dot_pos = text.find('.');
+  // if (dot_pos != std::string::npos) {
+  //   text = text.substr(0, dot_pos + 1);
+  // }
+
+  return text;
+}
+
 BT::NodeStatus CheckPolicy::on_success()
 {
   fprintf(stderr, "%s\n", result_.result->response.text.c_str());
@@ -110,24 +168,18 @@ BT::NodeStatus CheckPolicy::on_success()
   if (result_.result->response.text.empty() || result_.result->response.text == "{}") {
     return BT::NodeStatus::FAILURE;
   }
-  std::string answer = result_.result->response.text;
-  setOutput("output_text", answer);
+
+  std::string answer = sanitize_llm_output(result_.result->response.text);
+
   RCLCPP_INFO(
     node_->get_logger(), "CheckPolicy extracted answer: %s",
     answer.c_str());
 
-  answer.erase(
-    std::remove_if(
-      answer.begin(), answer.end(),
-      [](unsigned char c) {return !std::isalnum(c);}), answer.end());
-  std::transform(
-    answer.begin(), answer.end(), answer.begin(),
-    [](unsigned char c) {return std::tolower(c);});
-
   if (answer.empty()) {
     return BT::NodeStatus::FAILURE;
   }
 
+  setOutput("output_text", answer);
   return BT::NodeStatus::SUCCESS;
 }
 

bt_nodes/perception/src/perception/set_persistent_id.cpp

@@ -38,9 +38,10 @@ void SetPersistentId::on_tick()
   int id;
   if (!getInput("id", id)) {
     RCLCPP_ERROR(node_->get_logger(), "Missing ID in SetPersistentId");
+    setStatus(BT::NodeStatus::FAILURE);
+  }else{
+    request_->id = id;
   }
-
-  request_->id = id;
 }
 
 void SetPersistentId::on_result()

robocup_bringup/config/gpsr/gpsr.yaml

@@ -2,7 +2,7 @@ behaviors_main:
   ros__parameters:
     use_sim_time: False
     cam_frame: "head_front_camera_color_optical_frame"
-    home_position: [54.701, 1.360, 0.001]
+    home_position: [0.0, 0.0, 0.0]
     home_pose: "home"
     offer_pose: "offer"
     person_id: 001122334455
@@ -29,12 +29,13 @@ behaviors_main:
         "kitchen",
         "office",
         "living_room",
-        "instruction_point"
+        "instruction_point",
+        "bedroom"
       ]
     waypoints:
       #ARENA B
-      tv_table: [-14.936, -0.490, 1.543]
-      hallway: [-12.272, 0.691, 2.076]
+      tv_table: [1.42, 3.89, 1.543]
+      hallway: [1.01, 9.05, 2.076]
       hallway_cabinet: [-14.214, 2.942, 1.258]
       desk: [-9.134, 2.014, 2.901]
       shelf: [-8.354, 0.573, 1.306]
@@ -47,13 +48,14 @@ behaviors_main:
       dinner_table: [-9.216, -1.555, -1.775]
       kitchen_counter: [-9.312, -4.550, -1.747]
       dishwasher: [-10.371, -4.185, -1.762]
-      entrance: [-11.886, 2.915, 1.952]
+      entrance: [1.16, 6.19, 1.952]
       coffee_table: [-14.142, -2.027, -2.056]
       exit: [-8.242, -0.344, -0.441]
-      kitchen: [-11.626, -0.85, -0.689]
+      kitchen: [3.79, 6.57, -0.689]
       office: [-8.551, 0.033, 2.090]
-      living_room: [-13.452, -1.870, -2.397]
-      instruction_point: [-12.933, -0.495, -2.132]
+      living_room: [1.27, 4.56, -2.397]
+      instruction_point: [1.16, 6.19, -2.132]
+      bedroom: [6.31, 1.54, 1.258]
     plugins:
       - dialogConfirmation_bt_node
       - listen_bt_node

robocup_bringup/config/llm.yaml

@@ -0,0 +1,24 @@
+/**:
+  ros__parameters:
+    # Model parameters
+    model:
+      repo: bartowski/Qwen_Qwen3.5-4B-GGUF
+      filename: Qwen_Qwen3.5-4B-Q4_K_M.gguf
+
+    mmproj:
+      repo: bartowski/Qwen_Qwen3.5-4B-GGUF
+      filename: mmproj-Qwen_Qwen3.5-4B-f16.gguf
+
+    # Context / inference parameters
+    context:
+      n_ctx: 16384
+      n_batch: 1024
+      n_predict: 2000
+
+    # GPU / backend parameters
+    gpu:
+      n_gpu_layers: -1
+
+    # CPU parameters
+    cpu:
+      n_threads: -1

robocup_bringup/launch/dialog.launch.py

@@ -20,7 +20,6 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-# from llama_bringup.utils import create_llama_launch
 
 
 def generate_launch_description():
@@ -66,29 +65,18 @@ def generate_launch_description():
     # )
 
     llava_cmd = Node(
-        package="llama_ros",
-        executable="llava_node",
-        name="llava_node",
-        namespace="llama",
-        parameters=[{
-            "context.n_ctx": 2048,
-            "context.n_batch": 256,
-            "gpu.n_gpu_layers": 23,
-            "cpu.n_threads": 4,
-            "context.n_predict": -1,
-
-            # uncomment this for GPSR:
-            # "model.repo": "cstr/Spaetzle-v60-7b-Q4_0-GGUF",
-            # "model.filename": "Spaetzle-v60-7b_Q4_0.gguf",
-
-            # comment this for GPSR:
-            "model.repo": 'bartowski/Qwen2-VL-2B-Instruct-GGUF',
-            "model.filename": 'Qwen2-VL-2B-Instruct-Q4_K_M.gguf',
-            "mmproj.repo": "bartowski/Qwen2-VL-2B-Instruct-GGUF",
-            "mmproj.filename": "mmproj-Qwen2-VL-2B-Instruct-f16.gguf",
-            "prompt.system_prompt_type": "ChatML"
-        }]
-    )
+                package="llama_ros",
+                executable="llava_node",
+                name="llava_node",
+                namespace="llama",
+                parameters=[
+                    os.path.join(
+                        get_package_share_directory("robocup_bringup"),
+                        "config",
+                        "llm.yaml",
+                    )
+                ],
+            )
 
     whisper_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -134,8 +122,8 @@ def generate_launch_description():
     ld.add_action(llava_cmd)
     ld.add_action(audio_common_tts_node)
     ld.add_action(audio_common_player_node)
-    ld.add_action(kb_cmd)
+    # ld.add_action(kb_cmd)
 
-    #ld.add_action(music_player_node)
+    ld.add_action(music_player_node)
 
     return ld