Behavior Tree Callback Not Running

edit

Hi,

Currently, I am modifying one of the condition behavior tree inside nav2 which is the initial_pose_received condition. The code is running fine before I send any callback to update the blackboard inside the behaviour tree. Every tick is running good.

But, for the first time, I sent the cmd of "ros2 topic pub -1 /is_recovered_bypass xxxx", the callback inside the behavior tree will be triggered, and gave me the correct result. Yet, when I sent the same cmd afterwards, none of the cmd can reach the callback sequence. Yet, the tick info logs are still showing, but with old data.

Anyone has any idea on this?

#include <string>

#include "amr_behaviour_tree_plugins/condition/is_recovered_condition.hpp"

namespace nav2_behavior_tree
{

IsRecoveredCondition::IsRecoveredCondition(
  const std::string & condition_name,
  const BT::NodeConfiguration & conf)
: BT::ConditionNode(condition_name, conf),
bypass_topic_("/is_recovered_bypass")
{
   getInput("bypass_topic", bypass_topic_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   recovered_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
      bypass_topic_,
      rclcpp::SystemDefaultsQoS(),
      std::bind(&IsRecoveredCondition::isRecoveredCallback, this, std::placeholders::_1));
} 

BT::NodeStatus IsRecoveredCondition::tick()
{
   auto initPoseReceived = config().blackboard->get<bool>("initial_pose_received");
   RCLCPP_INFO(node_->get_logger(), "Received pose; %s", initPoseReceived? "true": "false");
   if (initPoseReceived) {
       return BT::NodeStatus::SUCCESS;
   }
   return BT::NodeStatus::FAILURE;
}

void IsRecoveredCondition::isRecoveredCallback(std_msgs::msg::Bool::SharedPtr msg)
{
     RCLCPP_INFO(node_->get_logger(), "Recover Callback: '%s'", msg->data? "true": "false");
     config().blackboard->template set<bool>("initial_pose_received", true);
}

}  // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
      factory.registerNodeType<nav2_behavior_tree::IsRecoveredCondition>("InitialPoseReceived");
}