LoopNode : std::vector<T> | std::deque<T>

[viprob-ai]

Hi @facontidavide

snippet

<LoopWaypointNode queue="{waypoints}" value="{wp}" if_empty="SKIPPED">
....
</>

Error

1746000290.487891654                bt_action_server   ERROR| Behavior Tree exception:The creation of the tree failed because the port [waypoints] was initially created with type [std::vector<geometry_msgs::msg::PoseStamped_<std::allocator<void> >, std::allocator<geometry_msgs::msg::PoseStamped_<std::allocator<void> > > >] and, later type [std::shared_ptr<std::deque<geometry_msgs::msg::PoseStamped_<std::allocator<void> >, std::allocator<geometry_msgs::msg::PoseStamped_<std::allocator<void> > > > >] was used somewhere else.

As my upstream node return it in std::vector<T>
I cant Loop through it using LoopNode<T>

Do i have to always write a overhead wrapper ?

class VectorToQueueNode : public BT::SyncActionNode
{
public:
  VectorToQueueNode(const std::string & name, const BT::NodeConfig & config)
  : BT::SyncActionNode(name, config) {}

  static BT::PortsList providedPorts()
  {
    return {
      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>("vector_in"),
      BT::OutputPort<std::shared_ptr<std::deque<geometry_msgs::msg::PoseStamped>>>("queue_out")
    };
  }

  BT::NodeStatus tick() override
  {
    std::vector<geometry_msgs::msg::PoseStamped> input_vec;
    if (!getInput("vector_in", input_vec)) {
      return BT::NodeStatus::FAILURE;
    }

    auto q = std::make_shared<std::deque<geometry_msgs::msg::PoseStamped>>(input_vec.begin(), input_vec.end());
    setOutput("queue_out", q);
    return BT::NodeStatus::SUCCESS;
  }
};

Any Suggest to handle it in better way ?

[sven-hoek]

Just ran into the same issue and was tempted to write a similar vector-to-queue converter node but came here to see if there are better solutions. Anything so far?

[sven-hoek]

Btw, if this is needed for multiple types, a template can be pretty handy:

#include <behaviortree_cpp/action_node.h>
#include <behaviortree_cpp/bt_factory.h>

template <typename T>
class VectorToQueueNode : public BT::SyncActionNode {
   public:
    VectorToQueueNode(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config) {}

    static BT::PortsList providedPorts() {
        return {
            BT::InputPort<std::vector<T>>("vector"),
            BT::OutputPort<std::shared_ptr<std::deque<T>>>("queue"),
        };
    }

    BT::NodeStatus tick() override {
        std::vector<T> vec;
        if (!getInput("vector", vec)) {
            return BT::NodeStatus::FAILURE;
        }

        auto queue = std::make_shared<std::deque<T>>(vec.cbegin(), vec.cend());
        setOutput("queue", queue);
        return BT::NodeStatus::SUCCESS;
    }
};

BT_REGISTER_NODES(factory) {
    factory.registerNodeType<VectorToQueueNode<std::string>>("VectorToQueueString");
    factory.registerNodeType<VectorToQueueNode<bool>>("VectorToQueueBool");
    factory.registerNodeType<VectorToQueueNode<int>>("VectorToQueueInt");
    factory.registerNodeType<VectorToQueueNode<double>>("VectorToQueueDouble");
}

[viprob-ai]

Yeah , i also landed up with the template class as it seemed the more logical thing given in cases where you might also need to refill or initiate the LoopNode queue=? over and over again for repetitive actions

wp_vector persist in blackboard ,whereas you consume the wp_queue by in_place poping

<Repeat num_attempts="4">
  <Sequence>
      <WpVectorToQueue vector="wp_vector" queue="{wp_queue}" />
      <LoopNode queue="wp_queue" >
            <>...........
      </LoopNode>