Humble update compilation

bt_nodes/configuration/package.xml

@@ -16,6 +16,7 @@
   <depend>behaviortree_cpp_v3</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>yolov8_msgs</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/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);

bt_nodes/perception/src/perception/filter_object.cpp

@@ -36,27 +36,43 @@ FilterObject::FilterObject(
   objects_({{"person", {"human", 170.0f, 70.0f}},
       {"bag", {"accessory", 30.0f, 0.3f}},
       {"soccer_ball", {"toy", 28.0f, 0.4f}},
+      {"washcloth", {"cleaning_supply", 28.0f, 0.01f}},
       {"bottle", {"drink", 27.0f, 1.0f}},
+      {"big_coke", {"drink", 27.0f, 2.0f}},
       {"red_wine", {"drink", 26.0f, 1.0f}},
       {"pringles", {"snack", 25.0f, 0.5f}},
       {"orange_juice", {"drink", 24.0f, 0.5f}},
       {"tropical_juice", {"drink", 24.0f, 0.5f}},
       {"cleanser", {"cleaning_supply", 23.0f, 0.75f}},
-      {"cornflakes", {"snack", 22.0f, 0.4f}},
+      {"soap", {"cleaning_supply", 23.0f, 0.75f}},
+      {"cornflakes", {"food", 22.0f, 0.4f}},
+      {"pancake_mix", {"food", 20.0f, 0.3f}},
+      {"hagelslag", {"food", 15.0f, 0.3f}},
       {"plate", {"dish", 21.0f, 0.5f}},
       {"bowl", {"dish", 20.0f, 0.5f}},
       {"banana", {"fruit", 19.0f, 0.25f}},
       {"tomato_soup", {"food", 18.0f, 0.5f}},
-      {"iced_tea", {"drink", 17.0f, 0.5f}},
+      {"iced_tea", {"drink", 12.0f, 0.5f}},
       {"juice_pack", {"drink", 12.0f, 0.4f}},
-      {"cola", {"drink", 11.0f, 0.5f}},
-      {"sponge", {"cleaning_supply", 11.0f, 0.1f}},
+      {"cola", {"drink", 12.0f, 0.5f}},
+      {"fanta", {"drink", 12.0f, 0.5f}},
+      {"dubbelfris", {"drink", 11.0f, 0.6f}},
+      {"sponge", {"cleaning_supply", 11.0f, 0.01f}},
+      {"tictac", {"snack", 10.0f, 0.4f}},
       {"7up", {"drink", 11.0f, 0.5f}},
       {"mustard", {"food", 11.0f, 0.3f}},
-      {"milk", {"drink", 10.0f, 1.0f}},
-      {"water", {"drink", 10.0f, 1.0f}},
+      {"curry", {"food", 12.0f, 0.7f}},
+      {"mayonaise", {"food", 12.0f, 0.7f}},
+      {"pea_soup", {"food", 14.0f, 1.0f}},
+      {"sausages", {"food", 14.0f, 1.0f}},
+      {"milk", {"drink", 14.0f, 0.7f}},
+      {"stroopwafel", {"snack", 15.0f, 0.8f}},
+      {"candy", {"snack", 14.0f, 0.5f}},
+      {"water", {"drink", 13.0f, 0.6f}},
       {"apple", {"fruit", 9.0f, 0.2f}},
       {"cheezit", {"snack", 9.0f, 0.1f}},
+      {"liquorice", {"snack", 9.0f, 0.1f}},
+      {"crisps", {"snack", 9.0f, 0.1f}},
       {"orange", {"fruit", 9.0f, 0.25f}},
       {"pear", {"fruit", 9.0f, 0.25f}},
       {"peach", {"fruit", 9.0f, 0.2f}},
@@ -78,6 +94,7 @@ FilterObject::FilterObject(
       {"spoon", {"dish", 4.0f, 0.1f}},
       {"strawberry", {"fruit", 3.0f, 0.05f}},
       {"dice", {"toy", 2.0f, 0.05f}},
+      {"candle", {"decoration", 1.0f, 0.02f}},
       {"dishwasher_tab", {"cleaning_supply", 1.0f, 0.02f}}})
 {
   config().blackboard->get("node", node_);

robocup_bringup/bt_xml/carry_my_luggage.xml

@@ -4,7 +4,7 @@
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
             <Action ID="InitCarry" x_axis_max="{max_x}" x_axis_min="{min_x}" y_axis_max="{max_y}" y_axis_min="{min_y}"/>
-            <Action ID="Speak" param="" say_text="Hi, This is gentlebots. Im ready to carry your luggage, please point to the correct bag"/>
+            <Action ID="Speak" param="" say_text="Hi, This is gentlebots. Im ready to carry your luggage,please point to the correct bag and dont be too close"/>
             <Action ID="SetStartPosition" 
                                           reference_frame="map"
                                           frame_name="initial"
@@ -65,15 +65,15 @@
                                                            interest="person"
                                                            max_depth="3.0" 
                                                            max_entities="2"
-                                                           order="color"/>
+                                                           order="depth"/>
                             </RetryUntilSuccessful>
                             <Action ID="FilterEntity" frame="person_0" lambda="0.1"/>
                             <Action ID="LookAt" tf_frame="person_0_filtered"/>
 
                             <Condition ID="IsEntityMoving" distance_tolerance="0.6"
-                                                           robot_distance_to_person="1.0"
+                                                           robot_distance_to_person="1.5"
                                                            frame="person_0_filtered"
-                                                           check_time="9.0"/>
+                                                           check_time="8.0"/>
                             <Action ID="FollowEntity" camera_frame="head_front_camera_rgb_optical_frame"
                                                       distance_tolerance="0.2"
                                                       frame_to_follow="person_0_filtered"
@@ -100,6 +100,7 @@
                     </ReactiveSequence>
                 </Sequence>
             </RetryUntilSuccessful>
+            <Action ID="Speak" param="" say_text="I will give you the bag now, please be carefull"/>
             <Action ID="MoveToPredefined" pose="offer" group_name="arm_torso"/>
             <ReactiveSequence>
                 <Action ID="LookAt" tf_frame="person_0"/>

robocup_bringup/bt_xml/receptionist.xml

@@ -43,7 +43,13 @@
                     </RetryUntilSuccessful>
                     <Action ID="FilterEntity" frame="person_0" lambda="0.05"/>
                     <Action ID="LookAt" tf_frame="person_0_filtered"/> 
-                    <Action ID="ExtractEntityColor" confidence="0.6" interest="person" person_id="{guest_color_id}"/>                                      
+                    <Action ID="ExtractEntityColor" confidence="0.6" interest="person" person_id="{guest_color_id}"/>
+                    <RetryUntilSuccessful num_attempts="5">
+
+                        <Action ID="CheckPolicy" question="Describe the person in the picture, Please extract 3-5 features. Start the description with 'the guest is...'"
+                                            output_text="{description}"
+                                            image_topic="/camera/color/image_raw"/>
+                    </RetryUntilSuccessful>
                     <RetryUntilSuccessful num_attempts="-1">
                             <SequenceStar>
                                 <Sequence>
@@ -135,6 +141,7 @@
                             </SequenceStar>
                     </RetryUntilSuccessful>
                     <Action ID="StoreDetection" current_name="{name_value}"
+                                                guest_description="{description}"
                                                 drink="{drink_value}"
                                                 guest_id="{n}"
                                                 guest_color_id="{guest_color_id}"
@@ -143,7 +150,9 @@
                                                 drink_1="{drink_1}"
                                                 drink_2="{drink_2}"
                                                 guest_color_id_1="{guest_color_id_1}"
-                                                guest_color_id_2="{guest_color_id_2}"/>
+                                                guest_color_id_2="{guest_color_id_2}"
+                                                guest_description_id_1="{guest_description_id_1}"
+                                                guest_description_id_2="{guest_description_id_2}"/>
                     <Action ID="Speak" say_text="Please follow me, i will get you to the  party"/>
                     <Action ID="LookAt" tf_frame="{attention_home}"/>
                     <Action ID="MoveTo" is_truncated="false" distance_tolerance="0" tf_frame="party"/>
@@ -208,6 +217,7 @@
                             <Action ID="LookAt" tf_frame="{best_detection}"/>
                             <Action ID="Speak" say_text="Hi" param="{h_name}"/>
                             <Action ID="Speak" say_text=" this is" param="{name_1}"/>
+                            <Action ID="Speak" say_text=" " param="{guest_description_id_1}"/>
                             <Action ID="Speak" say_text=" and their favorite drink is" param="{drink_1}"/>
                             <Action ID="MoveTo" is_truncated="false" distance_tolerance="0" tf_frame="guest_confirmation"/>                                
                             <ReactiveFallback>
@@ -245,7 +255,8 @@
                             <Action ID="LookAt" tf_frame="{best_detection}"/>
 
                             <Action ID="Speak" say_text="Hi" param="{h_name}"/>
-                            <Action ID="Speak" say_text=" this is" param="{name_2}"/>                                
+                            <Action ID="Speak" say_text=" this is" param="{name_2}"/>
+                            <Action ID="Speak" say_text=" " param="{guest_description_id_2}"/>                                
                             <Action ID="Speak" say_text=" and their favorite drink is" param="{drink_2}"/>
 
                             <ReactiveFallback>