Merge branch 'humble-devel' of https://github.com/CoreSenseEU/CoreSense4Home into humble-devel

Author: igonzf

bt_nodes/arm/CMakeLists.txt

@@ -16,7 +16,13 @@ find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(play_motion2_msgs REQUIRED)
-# uncomment the following section in order to fill in
+find_package(Eigen3 REQUIRED)
+find_package(srdfdom REQUIRED)
+find_package(eigen_stl_containers REQUIRED)
+find_package(geometric_shapes REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIRS})
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
@@ -32,8 +38,15 @@ set(dependencies
   geometry_msgs
   sensor_msgs
   play_motion2_msgs
+  srdfdom
+  eigen_stl_containers
+  geometric_shapes
+  moveit_core
+  tf2_geometry_msgs
+  Eigen3
 )
 
+
 include_directories(include)
 
 add_library(pick_bt_node SHARED src/manipulation/pick_object.cpp)

bt_nodes/hri/src/hri/dialog/get_guest_info.cpp

@@ -117,8 +117,10 @@ void GetGuestInfo::on_result()
   std_msgs::msg::String fact_msg;
 
   // Delete attending fact
-  fact_msg.data = "robot1 oro:attends " + guest_id_;
-  kb_publisher_->publish(fact_msg);
+  if (!guest_id_.empty()) {
+    fact_msg.data = "robot1 oro:attends " + guest_id_;
+    kb_publisher_->publish(fact_msg);
+  }
 
   setStatus(BT::NodeStatus::SUCCESS);
 }

bt_nodes/hri/src/hri/dialog/store_guest_info.cpp

@@ -75,6 +75,26 @@ std::string obtain_guest_id(const std::string & json)
   return "guest" + std::to_string(max_id + 1);
 }
 
+static std::string escape_turtle_literal(std::string s)
+{
+  // 1) Escape backslashes
+  for (size_t pos = 0; (pos = s.find('\\', pos)) != std::string::npos; pos += 2) {
+    s.replace(pos, 1, "\\\\");
+  }
+
+  // 2) Escape double quotes
+  for (size_t pos = 0; (pos = s.find('"', pos)) != std::string::npos; pos += 2) {
+    s.replace(pos, 1, "\\\"");
+  }
+
+  // 3) Optional: normalize newlines (depends on parser strictness)
+  for (size_t pos = 0; (pos = s.find('\n', pos)) != std::string::npos; pos += 2) {
+    s.replace(pos, 1, "\\n");
+  }
+
+  return s;
+}
+
 void StoreGuestInfo::on_result()
 {
   RCLCPP_DEBUG(node_->get_logger(), "[StoreGuestInfo] result received");
@@ -101,6 +121,7 @@ void StoreGuestInfo::on_result()
   kb_publisher_->publish(fact_msg);
 
   if (!guest_description_.empty()) {
+    guest_description_ = escape_turtle_literal(guest_description_);
     fact_msg.data = guest_id + " oro:description \"" + guest_description_ + "\"";
     kb_publisher_->publish(fact_msg);
   }

robocup_bringup/launch/dialog.launch.py

@@ -27,20 +27,13 @@ def generate_launch_description():
     # package_dir = get_package_share_directory('robocup_bringup')
     # llama_dir = get_package_share_directory('llama_bringup')
     whisper_dir = get_package_share_directory('whisper_bringup')
+    kb_dir = get_package_share_directory('knowledge_core')
     # audio_common_dir = get_package_share_directory('audio_common')
 
     # Configuration Variables
     model_repo = LaunchConfiguration('model_repo')
     model_filename = LaunchConfiguration('model_filename')
 
-    declare_model_repo_cmd = DeclareLaunchArgument(
-        'model_repo', default_value='ggerganov/whisper.cpp',
-        description='Hugging Face model repo')
-
-    declare_model_filename_cmd = DeclareLaunchArgument(
-        'model_filename', default_value='ggml-large-v3-q5_0.bin',
-        description='Hugging Face model filename')
-
     # Actions
     llama_cmd = create_llama_launch(
             n_ctx=2048,
@@ -87,6 +80,12 @@ def generate_launch_description():
         )
     )
 
+    kb_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(kb_dir, 'launch', 'knowledge_core.launch.py')
+        )
+    )
+
     audio_common_tts_node = Node(
         package='tts_ros',
         executable='tts_node',
@@ -112,13 +111,13 @@ def generate_launch_description():
     )
 
     ld = LaunchDescription()
-    ld.add_action(declare_model_repo_cmd)
-    ld.add_action(declare_model_filename_cmd)
-    #ld.add_action(whisper_cmd)
-    #ld.add_action(llama_cmd)
+
+    ld.add_action(whisper_cmd)
     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(music_player_node)
 
     return ld

robocup_bringup/launch/receptionist_dependencies.launch.py

@@ -41,7 +41,7 @@ def generate_launch_description():
 
     manipulation_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(manipulation_dir, 'launch', 'server.launch.py')
+            os.path.join(manipulation_dir, 'launch', 'simple_server.launch.py')
         )
     )
 
@@ -58,12 +58,13 @@ def generate_launch_description():
         ),
         launch_arguments={
             # 'namespace': 'perception_system',
+            'use_3d': 'True',
             'model': 'yolov8n.pt',
             'input_image_topic': '/head_front_camera/rgb/image_raw',
             'input_depth_topic': '/head_front_camera/depth/image_raw',
             'input_depth_info_topic': '/head_front_camera/depth/camera_info',
             'depth_image_units_divisor': '1000',  # 1 for simulation, 1000 real
-            'target_frame': 'camera_color_optical_frame',
+            'target_frame': 'head_front_camera_optical_frame',
             'threshold': '0.5'
             }.items()
     )
@@ -81,7 +82,7 @@ def generate_launch_description():
         launch_arguments={
             'rviz': 'True',
             # 'map': package_dir + '/maps/robocup_arena_1.yaml', # ARENA C
-            'map': package_dir + '/maps/robocup_arena_2.yaml', # ARENA B
+            'map': package_dir + '/maps/apartamento_leon_gimp_con_mesa_tv.yaml', # ARENA B
             'params_file': package_dir +
                     '/config/receptionist/tiago_nav_params.yaml',
             'slam_params_file': package_dir +
@@ -95,7 +96,7 @@ def generate_launch_description():
     ld.add_action(dialog)
     ld.add_action(yolo3d)
     ld.add_action(real_time)
-    ld.add_action(move_group)
+    #ld.add_action(move_group)
     ld.add_action(manipulation_server)
 
     return ld