Merge branch 'humble-devel' into humble-devel-backup

Author: igonzf

.docker/Dockerfile

@@ -23,7 +23,7 @@ RUN mkdir -p /robocup/src/
 WORKDIR /robocup
 
 ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements1.txt
-ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements2.txt
+ADD https://raw.githubusercontent.com/jmguerreroh/yolo_ros/refs/heads/main/requirements.txt /robocup/requirements2.txt
 ADD https://raw.githubusercontent.com/mgonzs13/tts_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt
 
 # Install external dependencies.

bt_nodes/configuration/CMakeLists.txt

@@ -88,6 +88,9 @@ list(APPEND plugin_libs sleep_bt_node)
 add_library(set_blackboard_int_bt_node SHARED src/configuration/set_blackboard_int.cpp)
 list(APPEND plugin_libs set_blackboard_int_bt_node)
 
+add_library(activation_control_bt_node SHARED src/configuration/ActivationControl.cpp)
+list(APPEND plugin_libs activation_control_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   ament_target_dependencies(${bt_plugin} ${dependencies})
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

bt_nodes/configuration/include/configuration/ActivationControl.hpp

@@ -0,0 +1,54 @@
+// 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__ACTIVATION_CONTROL_HPP_
+#define CONFIGURATION__ACTIVATION_CONTROL_HPP_
+
+#include <memory>
+#include <string>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace configuration
+{
+
+class ActivationControl : public BT::ActionNodeBase
+{
+public:
+  explicit ActivationControl(const std::string & xml_tag_name, const BT::NodeConfiguration & conf);
+
+  void halt();
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList({
+      BT::InputPort<bool>("deactivate", "If true, remove activation, otherwise add"),
+      BT::InputPort<std::string>("node_name", "Name of the node to (de)activate")
+    });
+  }
+
+private:
+  std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
+  bool deactivate_ {false};
+  std::string node_name_;
+};
+
+}  // namespace configuration
+
+#endif  // CONFIGURATION__ACTIVATION_CONTROL_HPP_

bt_nodes/configuration/src/configuration/ActivationControl.cpp

@@ -0,0 +1,60 @@
+// 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/ActivationControl.hpp"
+
+namespace configuration
+{
+
+ActivationControl::ActivationControl(const std::string & xml_tag_name, const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+  config().blackboard->get("node", node_);
+}
+
+BT::NodeStatus ActivationControl::tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "ActivationControl ticked");
+
+  // Read inputs
+  if (!getInput("deactivate", deactivate_)) {
+    RCLCPP_WARN(node_->get_logger(), "ActivationControl: missing 'deactivate' input, defaulting to false");
+    deactivate_ = false;
+  }
+  if (!getInput("node_name", node_name_)) {
+    RCLCPP_ERROR(node_->get_logger(), "ActivationControl: missing 'node_name' input");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (deactivate_) {
+    RCLCPP_INFO(node_->get_logger(), "ActivationControl: removing activation for %s", node_name_.c_str());
+    node_->remove_activation(node_name_);
+  } else {
+    RCLCPP_INFO(node_->get_logger(), "ActivationControl: adding activation for %s", node_name_.c_str());
+    node_->add_activation(node_name_);
+  }
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void ActivationControl::halt()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "ActivationControl halted");
+}
+
+}  // namespace configuration
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<configuration::ActivationControl>("ActivationControl");
+}

bt_nodes/configuration/src/configuration/Deferred.cpp

@@ -82,7 +82,9 @@ BT::NodeStatus Deferred::tick()
     // publisher_zmq_ = std::make_unique<BT::PublisherZMQ>(subtree_, 10, 2666, 2667);
   }
 
+  std::cerr << "[DeferredBT] Executing tick to the subtree!!" << std::endl;
   auto state = subtree_.rootNode()->executeTick();
+  std::cerr << "[DeferredBT] Executed tick " << std::endl;
 
 //  if (state == BT::NodeStatus::FAILURE || state == BT::NodeStatus::SUCCESS) {
 //     publisher_zmq_.reset();

bt_nodes/configuration/src/configuration/init_carry.cpp

@@ -21,7 +21,7 @@ InitCarry::InitCarry(const std::string & xml_tag_name, const BT::NodeConfigurati
 : BT::ActionNodeBase(xml_tag_name, conf)
 {
   config().blackboard->get("node", node_);
-  node_->declare_parameter("cam_frame", "head_front_camera_rgb_optical_frame");
+  node_->declare_parameter("cam_frame", "camera_color_optical_frame");
   node_->declare_parameter("home_position", std::vector<double>{0.0, 0.0, 0.0});
   node_->declare_parameter("home_pose", "home");
   node_->declare_parameter("offer_pose", "offer");

bt_nodes/configuration/src/configuration/init_groceries.cpp

@@ -22,7 +22,7 @@ InitGroceries::InitGroceries(const std::string & xml_tag_name, const BT::NodeCon
 {
   config().blackboard->get("node", node_);
 
-  node_->declare_parameter("cam_frame", "head_front_camera_rgb_optical_frame");
+  node_->declare_parameter("cam_frame", "camera_color_optical_frame");
   node_->declare_parameter("manipulation_frame", "base_link");
   // node_->declare_parameter("party_wp",  std::vector<double>{0.0, 0.0, 0.0});
   // node_->declare_parameter("entrance_wp",  std::vector<double>{0.0, 0.0, 0.0});

bt_nodes/configuration/src/configuration/init_restaurant.cpp

@@ -23,7 +23,7 @@ InitRestaurant::InitRestaurant(
 {
   config().blackboard->get("node", node_);
 
-  node_->declare_parameter("cam_frame", "head_front_camera_rgb_optical_frame");
+  node_->declare_parameter("cam_frame", "camera_color_optical_frame");
 
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

bt_nodes/configuration/src/configuration/init_stickler.cpp

@@ -23,7 +23,7 @@ InitStickler::InitStickler(
 {
   config().blackboard->get("node", node_);
 
-  node_->declare_parameter("cam_frame", "head_front_camera_rgb_optical_frame");
+  node_->declare_parameter("cam_frame", "camera_color_optical_frame");
 
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

bt_nodes/configuration/src/configuration/set_perception_model.cpp

@@ -48,7 +48,7 @@ inline std::string joinPaths(const std::string & path1, const std::string & path
 
 void SetPerceptionModel::on_tick()
 {
-  RCLCPP_DEBUG(node_->get_logger(), "SetPerceptionModelNode ticked");
+  RCLCPP_INFO(node_->get_logger(), "SetPerceptionModelNode ticked");
   getInput("model_name", model_name_);
   getInput("model_type", model_type_);
   getInput("model_path", model_path_);