fix merge

Author: juandpenan

bt_nodes/configuration/src/configuration/Setup_gpsr.cpp

@@ -76,6 +76,11 @@ BT::NodeStatus SetupGPSR::tick()
   plugins.push_back("sleep_bt_node");
   plugins.push_back("count_people_bt_node");
   plugins.push_back("set_blackboard_int_bt_node");
+  plugins.push_back("check_policy_bt_node");
+  plugins.push_back("move_along_axis_bt_node");
+  plugins.push_back("switch_yolo_model_bt_node");
+  plugins.push_back("remove_string_prefix_bt_node");
+  plugins.push_back("set_persistent_id_bt_node");
 
   setOutput("plugins", plugins);
 

bt_nodes/hri/src/hri/dialog/Query.cpp

@@ -47,10 +47,17 @@ void Query::on_tick()
   std::string text_;
   getInput("text", text_);
   getInput("intention", intention_);
-  std::string prompt_ = "Given the sentence \"" + text_ + "\", extract the " + intention_ +
+
+  std::string prompt_ = "";
+
+  if(intention_.empty()){
+    prompt_= "You are a robot named Tiago who is participating in the Robocup with the Gentlebots team from Spain, made up of the Rey Juan Carlos University of Madrid and the University of León. Answer the following question: \"" + text_ + "\"";
+  }else{
+    prompt_ = "Given the sentence \"" + text_ + "\", extract the " + intention_ +
     " from the sentence and return "
     "it with the following JSON format:\n" +
     "{\n\t\"intention\": \"word extracted in the sentence\"\n}";
+  }
   goal_.prompt = prompt_;
   goal_.reset = true;
   goal_.sampling_config.temp = 0.0;

bt_nodes/perception/include/perception/count_people.hpp

@@ -61,7 +61,7 @@ class CountPeople : public BT::ActionNodeBase
         BT::InputPort<int>("input_num_person"),
 
         BT::OutputPort<std::vector<std::string>>("frames"),
-        BT::OutputPort<int>("num_person")});
+        BT::OutputPort<std::string>("num_person")});
   }
 
 private:

bt_nodes/perception/src/perception/count_people.cpp

@@ -78,6 +78,9 @@ BT::NodeStatus CountPeople::tick()
     RCLCPP_INFO(node_->get_logger(), "CountPeople idle");
   }
 
+  RCLCPP_INFO(node_->get_logger(), "[CountPeople] color %s", color_);
+  RCLCPP_INFO(node_->get_logger(), "[CountPeople] pose %s", pose_);
+  RCLCPP_INFO(node_->get_logger(), "[CountPeople] color %s", gesture_);
   RCLCPP_INFO(node_->get_logger(), "CountPeople ticked");
   pl::getInstance(node_)->set_interest("person", true);
   pl::getInstance(node_)->update(35);
@@ -96,8 +99,11 @@ BT::NodeStatus CountPeople::tick()
     bool removed = false;
 
     if (detection.score > threshold_) {
+      RCLCPP_INFO(node_->get_logger(), "[CountPeople] threshold okey");
+      
       // Color filtering
       if (color_ != "none") {
+        RCLCPP_INFO(node_->get_logger(), "[CountPeople] Count by color");
         auto const detection_id_colors = perception_system::getHSVFromUniqueID(
           detection.color_person);
         std::string lower_color = "lower_" + color_;
@@ -135,6 +141,7 @@ BT::NodeStatus CountPeople::tick()
 
       // gesture filtering
       if (gesture_ != "unknown" && !removed) {
+        RCLCPP_INFO(node_->get_logger(), "[CountPeople] Count by gesture");
         if (std::find(
             gestures_[gesture_].begin(), gestures_[gesture_].end(),
             detection.pointing_direction) != gestures_[gesture_].end())
@@ -153,6 +160,7 @@ BT::NodeStatus CountPeople::tick()
 
       // pose filtering
       if (pose_ != "unknown" && !removed) {
+        RCLCPP_INFO(node_->get_logger(), "[CountPeople] Count by pose");
         if (pose_names_[detection.body_pose] == pose_) {
           RCLCPP_DEBUG(
             node_->get_logger(), "[IsDetected] Detection %s is %s",
@@ -182,7 +190,9 @@ BT::NodeStatus CountPeople::tick()
     // return BT::NodeStatus::SUCCESS;
   }
 
-  setOutput("num_person", prev_num_person + num_entities);
+  auto result = std::to_string(prev_num_person + num_entities);
+
+  setOutput("num_person", result);
 
   RCLCPP_INFO(node_->get_logger(), "[CountPeople] %d people detected", num_entities);
 

robocup_bringup/launch/gpsr_dependencies.launch.py

@@ -49,7 +49,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')
         )
     )
 
@@ -62,16 +62,16 @@ def generate_launch_description():
 
     yolo3d = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(yolo3d_dir, 'launch', 'yolo_3d.launch.py')
+            os.path.join(yolo3d_dir, 'launch', 'yolo.launch.py')
         ),
         launch_arguments={
             # 'namespace': 'perception_system',
-            'model': 'yolov8n-pose.pt',
+            'model': 'yolo11n.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 in real robot
-            'target_frame': 'head_front_camera_rgb_optical_frame',
+            'input_depth_info_topic': '/head_front_camera/rgb/camera_info',
+            'depth_image_units_divisor': '1000',  # 1 for simulation, 1000 real
+            'target_frame': 'head_front_camera_color_optical_frame',
             'threshold': '0.5'
             }.items()
     )
@@ -82,13 +82,13 @@ def generate_launch_description():
         ),
         launch_arguments={
             'rviz': 'True',
-            'mode': 'amcl',
-            'params_file': package_dir + '/config/gpsr/tiago_nav_params.yaml',
-            'slam_params_file': package_dir + '/config/gpsr/tiago_nav_follow_params.yaml',
-            'map': os.path.join(
-                                package_dir,
-                                'maps',
-                                'robocup_arena_1.yaml'),
+            # 'map': package_dir + '/maps/robocup_arena_1.yaml', # ARENA C
+            '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 +
+                    '/config/receptionist/tiago_nav_follow_params.yaml',
+            'nav_mode': 'amcl'
         }.items()
     )
 
@@ -103,7 +103,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)
     ld.add_action(gpsr)