updated bt_nodes

Author: juandpenan

bt_nodes/configuration/CMakeLists.txt

@@ -14,7 +14,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(yolo_msgs REQUIRED)
+find_package(yolov8_msgs REQUIRED)
 
 set(CMAKE_CXX_STANDARD 17)
 
@@ -27,7 +27,7 @@ set(dependencies
   tf2_geometry_msgs
   tf2
   tf2_ros
-  yolo_msgs
+  yolov8_msgs
 )
 
 include_directories(include)
@@ -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/include/configuration/set_perception_model.hpp

@@ -23,14 +23,14 @@
 #include "configuration/bt_service_node.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
-#include "yolo_msgs/srv/change_model.hpp"
+#include "yolov8_msgs/srv/change_model.hpp"
 
 
 namespace configuration
 {
 
 class SetPerceptionModel
-  : public configuration::BtServiceNode<yolo_msgs::srv::ChangeModel,
+  : public configuration::BtServiceNode<yolov8_msgs::srv::ChangeModel,
     rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 {
 public:

bt_nodes/configuration/package.xml

@@ -16,7 +16,7 @@
   <depend>behaviortree_cpp_v3</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>yolo_msgs</depend>
+  <depend>yolov8_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

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_);