[ROS2 foxy] Interactive marker server inside a Rviz plugin

edit

Hello,

I have a combination a tool and panel in Rviz that should allow me to manage/create a waypoint list. When the tool is loaded it should also load the panel and the interactive marker server that will manage the waypoint.

The issue is the interactive marker server constructor will take a rclcpp::Node::SharedPtr in its constructor which is not immediately available in a Rviz2::plugin.

Rightn now i have tried two different things:

I tried to get the rviz node instance:

WaypointTool::WaypointTool() : rviz_common::Tool(){
   auto nodeAbstraction = context_->getRosNodeAbstraction().lock();
   rclcpp::Node::SharedPtr node = nodeAbstraction->get_raw_node();
   server_ = new interactive_markers::InteractiveMarkerServer("waypoint", node);
   // Stuff
}

However this gives me a segfault on this line:

auto nodeAbstraction = context_->getRosNodeAbstraction().lock();

I guess the node weak_ptr is not initialized in this context.

I tried another way where I directly create a node instance in my tool class and give it to the server:

    WaypointTool::WaypointTool()
    : rviz_common::Tool(), nh_(std::make_shared<rclcpp::Node>("w_tool"), server_("waypoint", nh_) {
    // Stuff
}

However if I use this directly rviz interactive marker display can't connect to the server and gives me an error like this:

[INFO] [1613640458.153922653] [rviz]: Connected on namespace: waypoint
[INFO] [1613640458.169085509] [rviz]: Sending request for interactive markers
[WARN] [1613640459.188331428] [rviz]: Did not receive response with interactive markers, resending request...

I assume it is because the node is not spinning so nothing happens when the server is called by the client. To solve this I added a thread inside my class where I make the node spin:

void WaypointTool::onInitialize() {
   // Stuff
   thread_ = std::make_shared<std::thread>(std::bind(&WaypointTool::spin, this));
 }

 void WaypointTool::spin()
 {
    exec_.add_node(nh_);
    while(rclcpp::ok()){
       exec_.spin_some();
    }
 }

When doing this, the console will outputs these lines continuously:

[INFO] [1613653584.000676745] [rviz]: Sending request for interactive markers
[INFO] [1613653583.936624364] [w_tool]: Responding to request to get interactive markers
[INFO] [1613653583.968256112] [rviz]: Service response received for initialization

However no interactive markers are ever displayed even if they seem to be created correctly as the topic /waypoint/output correctly outputs something.

Has anyone already managed to make interactive server work with rviz2 plugins ?