Fix for multiple video plugins (#898)

[shiveshkhaitan]

Append the name of the plugin specified in the sdf to avoid crashing when multiple video plugins are used. This connects to #898

[chapulina]

With this PR, I'm able to add another display to the demo world like this, and both displays work:

diff --git a/gazebo_plugins/worlds/gazebo_ros_video_demo.world b/gazebo_plugins/worlds/gazebo_ros_video_$emo.world
index 46766d2..74e05b3 100644
--- a/gazebo_plugins/worlds/gazebo_ros_video_demo.world
+++ b/gazebo_plugins/worlds/gazebo_ros_video_demo.world
@@ -159,5 +159,34 @@
       </link>
     </model>

+    <model name="box_display_2">
+      <pose>3.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <link name="base_link">
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.5</size>
+            </box>
+          </geometry>
+          <plugin name="display_video_controller_2" filename="libgazebo_ros_video.so">
+            <ros>
+              <!-- Remap for image topic to be subscribed -->
+              <argument>~/image_raw:=/camera1/image_raw</argument>
+            </ros>
+            <height>120</height>
+            <width>160</width>
+          </plugin>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.5</size>
+            </box>
+          </geometry>
+          <laser_retro>100.0</laser_retro>
+        </collision>
+      </link>
+    </model>
+
   </world>

But Gazebo seems to hang on shutdown and needs to be force-closed. Can you reproduce the issue? It would be good to look into it.

[shiveshkhaitan]

Fixed the crashing of gazebo on shutdown

[chapulina]

This solution is a bit too drastic, because we still want all other plugins to keep working after one plugin is destroyed. You can try for example adding the 2nd video box as on my other comment and then deleting one of the boxes. This line will be called and the other box's video will stop.

Is there another way of fixing the shutdown issue?

[shiveshkhaitan]

For me the other box's video is stopping even without rclcpp::shutdown(). Also I show that this

gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/node.hpp

Lines 139 to 142 in 1200972

	if (!rclcpp::is_initialized()) {
	rclcpp::init(0, nullptr);
	RCLCPP_INFO(internal_logger(), "ROS was initialized without arguments.");
	}

goes into if multiple times when we have multiple video plugins. Is is supposed to happen?

[shiveshkhaitan]

Not only multiple video plugins, even if I have just one video plugin and a diff drive plugin, diff drive stops working on deleting the box

[chapulina]

goes into if multiple times when we have multiple video plugins. Is is supposed to happen?

Ah that got me digging for a while. What's happening is that the video plugins are being loaded twice, once in gzclient, for the GUI, and once in gzserver, for the camera sensor. That happens for all VisualPlugins.

If you start only gzserver gazebo_ros_video_demo.world, you can see that there are only 2 subscribers to camera images:

$ ros2 topic info /camera1/image_raw
Topic: /camera1/image_raw
Publisher count: 1
Subscriber count: 2

So I think the root of the shutdown problems we're having come from these duplicate nodes being created on both client and server. There are a few ways to tackle this. I think we should load that plugin only on the server scene and not on the client. I can't remember if we have an easy way to tell if we're on the client or server from the plugin though. Since this PR the Scene keeps a member variable, but that's not exposed. Maybe @iche033 remembers some nice trick to check it?

[chapulina]

Actually come to think of it, we probably want the plugin on both client and server... So maybe we need to give the nodes unique names.

[iche033]

if you ever need it, Scene::EnableVisualizations returns true if it's the client and false if it's the server

[shiveshkhaitan]

Setting different names for gzserver and gzclient did not help. But I think I figured out the problem. Adding this line executor_->remove_node(get_node_base_interface()); in gazebo_ros::Node destructor

gazebo_ros_pkgs/gazebo_ros/src/node.cpp

Lines 29 to 31 in 1200972

	Node::~Node()
	{
	}

fixes it. What I think is that when the plugin is destroyed and the node is deleted, the executor does not remove it and tries to spin but gets stuck as the node is already deleted. The above line explicitly removes the node. Does it make sense @chapulina?

[chapulina]

Argh it sounds like #860 is back to haunt us.

If I remember correctly, when debugging that with @wjwwood , the underlying issue seemed to be coming from the RMW layer. I don't remember if there was any problem adding the remove_node call, I think it just didn't fix the issue completely. So if it solves this specific issue without breaking anything else, I think we should get it in. Let's give it a try!

[wjwwood]

Isn't this essentially ros2/rclcpp#272? I believe it should have been fixed with ros2/rclcpp#741.

If not, then we should make sure there is an open issue on ros2/rclcpp which tracks the bug described in #860.

Unless I misunderstood and it's actually a bug in how rclcpp is being used here, I can't remember :x.

[chapulina]

@wjwwood , I just tried uncommenting the test that was failing:

gazebo_ros_pkgs/gazebo_ros/test/test_node.cpp

Lines 71 to 108 in 5e4a989

	// TODO(anyone) Fix this test, see
	// https://github.com/ros-simulation/gazebo_ros_pkgs/issues/855
	// Create another node
	// auto sdf_str_2 =
	// "<?xml version='1.0' ?>"
	// "<sdf version='1.6'>"
	// "<world name='default'>"
	// "<plugin name='node_2' filename='libnode_name.so'/>"
	// "</world>"
	// "</sdf>";
	// sdf::SDF sdf_2;
	// sdf_2.SetFromString(sdf_str_2);
	// auto plugin_sdf_2 = sdf_2.Root()->GetElement("world")->GetElement("plugin");
	// auto node_2 = gazebo_ros::Node::Get(plugin_sdf_2);
	// ASSERT_NE(nullptr, node_2);
	// EXPECT_STREQ("node_2", node_2->get_name());
	// EXPECT_NE(node_1, node_2);
	// Reset both
	// node_1.reset();
	// node_2.reset();
	// // Create another node
	// auto sdf_str_3 =
	// "<?xml version='1.0' ?>"
	// "<sdf version='1.6'>"
	// "<world name='default'>"
	// "<plugin name='node_3' filename='libnode_name.so'/>"
	// "</world>"
	// "</sdf>";
	// sdf::SDF sdf_3;
	// sdf_3.SetFromString(sdf_str_3);
	// auto plugin_sdf_3 = sdf_3.Root()->GetElement("world")->GetElement("plugin");
	// auto node_3 = gazebo_ros::Node::Get(plugin_sdf_3);
	// ASSERT_NE(nullptr, node_3);
	// EXPECT_STREQ("node_3", node_3->get_name());

And the situation is still the same:

• Without the remove_node call, the test fails every time
• With the remove_node call, the test never passes 10 times in a row

Unfortunately, I don't have time right now to create a failing test case using just rclcpp 😕 If I remember correctly, we tried doing that together and it was complicated to reproduce what's happening here.

I'll merge the remove_node fix because it solves the issue for the video plugin, but I'll leave #855 open.