added new node to add string suffix

Author: juandpenan

bt_nodes/configuration/CMakeLists.txt

@@ -61,6 +61,9 @@ list(APPEND plugin_libs init_restaurant_bt_node)
 add_library(remove_string_suffix_bt_node SHARED src/configuration/remove_string_suffix.cpp)
 list(APPEND plugin_libs remove_string_suffix_bt_node)
 
+add_library(add_string_suffix_bt_node SHARED src/configuration/add_string_suffix.cpp)
+list(APPEND plugin_libs add_string_suffix_bt_node)
+
 add_library(remove_string_prefix_bt_node SHARED src/configuration/remove_string_prefix.cpp)
 list(APPEND plugin_libs remove_string_prefix_bt_node)
 

bt_nodes/configuration/include/configuration/add_string_suffix.hpp

@@ -0,0 +1,50 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONFIGURATION__ADD_STRING_SUFFIX_HPP_
+#define CONFIGURATION__ADD_STRING_SUFFIX_HPP_
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace configuration
+{
+
+class AddStringSuffix : public BT::ActionNodeBase
+{
+public:
+  explicit AddStringSuffix(const std::string & xml_tag_name, const BT::NodeConfiguration & conf);
+
+  void halt();
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("string_to_modify"),
+        BT::InputPort<std::string>("suffix"),
+        BT::OutputPort<std::string>("result")
+      });
+  }
+
+private:
+  std::string string_to_modify_;
+  std::string suffix_;
+  std::string result_;
+};
+
+}  // namespace configuration
+
+#endif  // CONFIGURATION__ADD_STRING_SUFFIX_HPP_

bt_nodes/configuration/src/configuration/add_string_suffix.cpp

@@ -0,0 +1,45 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "configuration/add_string_suffix.hpp"
+
+namespace configuration
+{
+
+AddStringSuffix::AddStringSuffix(
+  const std::string & xml_tag_name, const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+}
+
+BT::NodeStatus AddStringSuffix::tick()
+{
+  getInput("string_to_modify", string_to_modify_);
+  getInput("suffix", suffix_);
+  if (string_to_modify_.empty()) {
+    return BT::NodeStatus::FAILURE;
+  }
+  result_ = string_to_modify_ + suffix_;
+  setOutput("result", result_);
+  return BT::NodeStatus::SUCCESS;
+}
+
+void AddStringSuffix::halt() {}
+
+}  // namespace configuration
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<configuration::AddStringSuffix>("AddStringSuffix");
+}

bt_nodes/perception/src/perception/ExtractHandoverAlignment.cpp

@@ -101,20 +101,14 @@ BT::NodeStatus ExtractHandoverAlignment::tick()
         detected_object.bbox3d.center.position.z));
     camera_2_object.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
 
-    // Get transform from arm_4_link to camera
+    // Get transform from arm_4_link to object frame
     geometry_msgs::msg::TransformStamped arm_2_camera_msg;
     try {
       arm_2_camera_msg = tf_buffer_->lookupTransform(
-        "arm_4_link", "head_front_camera_rgb_optical_frame", tf2::TimePointZero);
+        "arm_4_link", detected_object.bbox3d.frame_id, tf2::TimePointZero);
     } catch (const tf2::TransformException & ex) {
-      // Try regular frame if optical fails
-      try {
-        arm_2_camera_msg = tf_buffer_->lookupTransform(
-          "arm_4_link", "head_front_camera_rgb_frame", tf2::TimePointZero);
-      } catch (const tf2::TransformException & ex2) {
-        RCLCPP_ERROR(node_->get_logger(), "TF Error: %s", ex2.what());
-        return BT::NodeStatus::FAILURE;
-      }
+      RCLCPP_ERROR(node_->get_logger(), "TF Error looking up %s: %s", detected_object.bbox3d.frame_id.c_str(), ex.what());
+      return BT::NodeStatus::FAILURE;
     }
 
     tf2::Transform arm_2_camera;
@@ -147,7 +141,7 @@ BT::NodeStatus ExtractHandoverAlignment::tick()
     double new_torso_height = current_torso_height_ + torso_z_error;
 
     // Limits
-    double min_height = 0.0;
+    double min_height = 0.11;
     double max_height = 0.35;
     if (new_torso_height < min_height) {
       new_torso_height = min_height;

robocup_bringup/launch/receptionist_dependencies.launch.py

@@ -85,7 +85,7 @@ def generate_launch_description():
         launch_arguments={
             'rviz': 'True',
             # 'map': package_dir + '/maps/robocup_arena_1.yaml', # ARENA C
-            'map': package_dir + '/maps/ir_lab.yaml', # ARENA B
+            'map': package_dir + '/maps/lab_marzo.yaml', # ARENA B
             'params_file': package_dir +
                     '/config/receptionist/tiago_nav_params.yaml',
             'slam_params_file': package_dir +
@@ -114,7 +114,15 @@ def generate_launch_description():
     person_tracker = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(person_tracker_dir, 'launch', 'person_tracker.launch.py')
-        )
+        ),
+        launch_arguments={
+            'target_frame': 'map',
+            'only_yolo': 'true',
+            'ema_alpha': '0.1',
+            'yolo_only_gate': '2.0',
+            'yolo_only_delete_threshold': '1.5',
+            'yolo_only_confirm_threshold': '2'
+        }.items()
     )
 
     ld = LaunchDescription()
@@ -123,7 +131,7 @@ def generate_launch_description():
     ld.add_action(yolo3d)
     ld.add_action(real_time)
     ld.add_action(knowledge_core)
-    ld.add_action(laser_people_detector)
+    # ld.add_action(laser_people_detector) Not working so far, only yolo for the moment
     ld.add_action(person_tracker)
     # ld.add_action(move_group)
     ld.add_action(manipulation_server)

robocup_bringup/maps/lab_marzo.pgm

@@ -0,0 +1,7 @@
+image: lab_marzo.pgm
+mode: trinary
+resolution: 0.050
+origin: [-4.997, -5.181, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

robocup_bringup/maps/lab_marzo.yaml

@@ -73,7 +73,7 @@ repositories:
     version: main
   ThirdParty/upo_laser_people_detector:
     type: git
-    url: https://github.com/robotics-upo/upo_laser_people_detector.git
+    url: https://github.com/juandpenan/upo_laser_people_detector.git
     version: ros2
   ThirdParty/cs4home_person_tracker:
     type: git