ROS2 how to create custom waitable

edit

Hi,

My goal is the following: next to the usual pub/sub mechanisms, I would like to have an additional method for passing data to nodes. This could be a queue that triggers a callback when a new element is available.

I would like to have this new mechanism managed by the rclcpp::spin method. I was thinking at creating a Waitable object and adding it to the callback group.

I came up with the following implementation, using a rcl_guard_condition. However, I'm not sure if this is the best solution.

#include <chrono>
#include <mutex>
#include "rclcpp/rclcpp.hpp"

rcl_guard_condition_t gc_ = rcl_get_zero_initialized_guard_condition();
// this is the current size of a fake queue
int size_ = 0;

class MyWaitable : public rclcpp::Waitable
{
public:

  std::recursive_mutex reentrant_mutex_;

  MyWaitable(){

  std::shared_ptr<rclcpp::Context> context_ptr = 
      rclcpp::contexts::default_context::get_global_default_context();

  rcl_guard_condition_options_t guard_condition_options = 
      rcl_guard_condition_get_default_options();
  rcl_ret_t ret = rcl_guard_condition_init(
      &gc_, context_ptr->get_rcl_context().get(), guard_condition_options);
   }

size_t
get_number_of_ready_guard_conditions() { return 1;}

bool
add_to_wait_set(rcl_wait_set_t * wait_set)
{
  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);

  rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
  return RCL_RET_OK == ret;
}

bool
is_ready(rcl_wait_set_t * wait_set) { return size_ > 0; }

void
execute()
{
  std::cout<<"Executing waitable function!"<<std::endl;
  size_ --;
}
};

class MyNode : public rclcpp::Node
{
public:

MyNode()
: Node("my_node")
{
   auto node_waitables_interface = this->get_node_waitables_interface();
   waitable_ptr = std::make_shared<MyWaitable>();
   node_waitables_interface->add_waitable(waitable_ptr, nullptr);
}

std::shared_ptr<MyWaitable> waitable_ptr;
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);

  // create a node with a custom waitable and let it spin in a separate thread
  std::shared_ptr<MyNode> node = std::make_shared<MyNode>();
  std::thread t([=](){
     std::cout<<"before spinning"<<std::endl;   
     rclcpp::spin(node);
  });
  t.detach();

  // after some time, simulate that a new item is ready to be consumed and notify the guard condition
  std::this_thread::sleep_for(std::chrono::seconds(1));
  size_ ++;
  rcl_trigger_guard_condition(&gc_);

  std::this_thread::sleep_for(std::chrono::seconds(1));
  rclcpp::shutdown();
  return 0;
}

Thanks