-
Notifications
You must be signed in to change notification settings - Fork 772
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix for multiple video plugins (#898) #937
Conversation
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. |
Fixed the crashing of gazebo on shutdown |
@@ -186,6 +186,9 @@ GazeboRosVideo::GazeboRosVideo() | |||
// Destructor | |||
GazeboRosVideo::~GazeboRosVideo() | |||
{ | |||
if (rclcpp::is_initialized()) { | |||
rclcpp::shutdown(); | |||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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."); | |
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 VisualPlugin
s.
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?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
if you ever need it, Scene::EnableVisualizations returns true if it's the client and false if it's the server
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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.
* [ros2] Port elevator to ROS2 * [ros2] Fix test for diff drive (ros-simulation#951) * use c_str() (ros-simulation#950) (ros-simulation#954) Signed-off-by: Karsten Knese <[email protected]> * [ros2] Port projector to ROS2 (ros-simulation#956) * [ros2] Port projector to ROS2 * fix small typo * [ros2] Port planar move to ROS2 (ros-simulation#958) * [ros2] Port planar move to ROS2 * Add test for pose conversion * [ros2] Add ackermann drive plugin (ros-simulation#947) * [ros2] Add ackermann drive plugin * Minor fixes Use gazebo database model * Update example usage * Fix TF for demo * changelog * 3.3.2 * [ros2] Port harness to ROS2 (ros-simulation#944) * [ros2] Port hand of god to ROS2 (ros-simulation#957) * [ros2] Port hand of god to ROS2 * Minor fixes * [ros2] Port model states to ROS2 (ros-simulation#968) * [ros2] Port model states to ROS2 * remove unported code Signed-off-by: Louise Poubel <[email protected]> * Add Gazebo builtin plugins to LD_LIBRARY_PATH (ros-simulation#974) * Add Gazebo builtin plugins to LD_LIBRARY_PATH Signed-off-by: Louise Poubel <[email protected]> * cross-platform Signed-off-by: Louise Poubel <[email protected]> * [ros2] Fix tests on Dashing (ros-simulation#953) * [ros2] Fix camera triggered test on Dashing backport remove noe fix and re-enable distortion tests * improve robustness of joint state pub test Signed-off-by: Louise Poubel <[email protected]> * Fix for multiple video plugins (ros-simulation#898) (ros-simulation#937) * Fix for multiple video plugins (ros-simulation#898) * Fix crash on shutdown * Fix gazebo node destructor * [ros2] Port bumper sensor to ROS2 (ros-simulation#943) * [ros2] Port bumper sensor to ROS2 * Add author name * Minor fixes and add contact msg conversion * Remove unused header includes * [ros2] Port gazebo_ros_path plugin to ROS2 (ros-simulation#925) * [ros2] Port gazebo_ros_path plugin * Minor fixes * Change plugin launch file to python script * Fix for flake8 test * set gazebo library dirs (ros-simulation#963) Signed-off-by: Karsten Knese <[email protected]> * [ros2] Port Link states to ROS2 (ros-simulation#969) * [ros2] Port model states to ROS2 * [ros2] Port link states to ROS2 * Change usage of body -> link * Remove link_states from .ros1_unported * fix video test Signed-off-by: chapulina <[email protected]> * [ros2] Port joint pose trajectory to ROS2 (ros-simulation#955) * [ros2] Port joint pose trajectory to ROS2 * Add conversion tests Minor fixes * [ros2] Port gazebo launch scripts to ROS2 (ros-simulation#962) * [ros2] Port gazebo launch scripts to ROS2 * Add gdb and valgrind option * Use shell command for extra gazebo args * [ros2] Port vacuum gripper to ROS2 (ros-simulation#960) * [ros2] Port vacuum gripper to ROS2 * Fix gripper forces * Add option to set max_distance Change SetForce -> Add Force * [ros2] Port spawn model to ROS2 (ros-simulation#948) * [ros2] Port spawn model to ROS2 * Delete .ros1_unported files * Fixes and add demo Change spawn_model to spawn_entity * Rename demo launch and add checks for service * Fix reading xml file from param and model states * remove diplicate Signed-off-by: Louise Poubel <[email protected]> * Use gazebo launch file * Change topic behaviour * [ros2] Spawn <plugin> without <ros> (ros-simulation#983) Signed-off-by: Louise Poubel <[email protected]> * [ros2] Backport depth camera to dashing (ros-simulation#967) * [ros2] Backport depth camera to dashing * don't install header that will be removed Signed-off-by: Louise Poubel <[email protected]> * fix linting error Signed-off-by: Louise Poubel <[email protected]> * Address reviews on ros-simulation#868 (ros-simulation#972) * [ros2] World plugin to get/set entity state services (ros-simulation#839) remove status_message * [ros2] Port time commands (pause / reset) (ros-simulation#866) * [ros2] Migration of get/set world, model, joint, link, light properties * Trying to pass CI test, try n1. * clean up some linter warnings * Requested changes in review, unfinished * Fix uncrustify * Address reviews * more tests, joint types Signed-off-by: Louise Poubel <[email protected]> * Revert changes to GetModelProperties message Document gazebo_ros_properties header * Convert msgs pose to math pose and use it on SetCoG Signed-off-by: Louise Poubel <[email protected]> * [ros2] Adding GPS plugin (ros-simulation#982) * Adding gps plugin sensor * Adding test for the gps plugin * Adding GPS world demo and other small text corrections * [ros2] Adding option to select the frame where the force will be applied (ros-simulation#978) * Adding option to select the frame where the force will be applied A new parameter was added on the plugin with the options 'world' and 'link' frame. The default value is 'world'. Internally the AddRelativeForce() and torque functions are used instead of the AddForce() when the body option is selected. * Modifying force test for the 'world' frame, and adding test for the force on the 'link' frame The new world file starts with the box rotated. * Fix cpplint and uncrustify on force plugin files * Removing OnUpdateRelative() from the force plugin This function could potentially break the ABI, therefore is been removed. * body -> link, warn -> info, more examples Signed-off-by: Louise Poubel <[email protected]> * [backport] Backport multicamera to dashing (ros-simulation#984) * [backport] Backport multicamera to dashing * fix test - use correct world Signed-off-by: Louise Poubel <[email protected]> * Add maintainer (ros-simulation#985) Signed-off-by: Louise Poubel <[email protected]> * [ros2] Conditional launch includes (ros-simulation#979) * [ros2] Conditional launch includes Signed-off-by: Louise Poubel <[email protected]> * remove unused import Signed-off-by: Louise Poubel <[email protected]> * changelog Signed-off-by: Louise Poubel <[email protected]> * 3.3.3 * [ros2] Uncommenting bond option on spawn_entity (wait Ctrl+C then remove entity) (ros-simulation#986) * Uncommenting bond option on spawn_entity (wait Ctrl+C then remove entity) Instead of waiting for a shutdown callback to be created in rclpy, we can use the try/except to get the SIGINT signal, then delete the entity. * Message formatting Signed-off-by: Louise Poubel <[email protected]> * linter 😅 Signed-off-by: Louise Poubel <[email protected]> * [ros2] Remove ported / deprecated (ros-simulation#989) Signed-off-by: Louise Poubel <[email protected]> * Remove ROS-specific arguments before passing to argparse (ros-simulation#994) This resolves argparse errors when trying to launch the spawn_entity script as a ROS node. For example, a launch file like the following wouldn't work without this change: <launch> <arg name="model_urdf" default="$(find-pkg-share mymodels)/urdf/ball.urdf" /> <node pkg="gazebo_ros" exec="spawn_entity.py" name="spawner" args="-entity foo -file /path/to/my/model/foo.urdf" /> </launch> Signed-off-by: Jacob Perron <[email protected]> * fix multi_camera_plugin on windows (ros-simulation#999) * changelog Signed-off-by: Louise Poubel <[email protected]> * 3.3.4 * Update changelogs * 3.4.0 * generate a .dsv file for the environment hook * Update changelogs * 3.4.1 * changelog Signed-off-by: Louise Poubel <[email protected]> * 3.4.2 * fix pathsep for windows (ros-simulation#1028) * Image publishers use SensorDataQoSProfile (ros-simulation#1031) All other sensor publishers were updated previously to use the same profile (ros-simulation#926). I'm not sure if the image publishers were overlooked or the image_transport API didn't support setting the QoS profile at the time. Signed-off-by: Jacob Perron <[email protected]> * [backport][ros2] make transient local reliable (ros-simulation#1033) (ros-simulation#1036) * [ros2] make transient local reliable (ros-simulation#1033) * make transient local reliable Signed-off-by: Karsten Knese <[email protected]> * fix master Signed-off-by: Karsten Knese <[email protected]> * add launch test Signed-off-by: Karsten Knese <[email protected]> * make it actual latched Signed-off-by: Karsten Knese <[email protected]> * alpha sort Signed-off-by: Karsten Knese <[email protected]> * add launch_test dependency Signed-off-by: Karsten Knese <[email protected]> * more dependencies Signed-off-by: Karsten Knese <[email protected]> * remove debug print Signed-off-by: Karsten Knese <[email protected]> * is_initialized -> ok Signed-off-by: Karsten Knese <[email protected]> * Update gazebo_ros/test/entity_spawner.test.py Co-Authored-By: chapulina <[email protected]> * use erase-remove idiom Signed-off-by: Karsten Knese <[email protected]> * use ReadyToTest() Signed-off-by: Karsten Knese <[email protected]> Co-authored-by: chapulina <[email protected]> * Set timeout and call gzserver directly Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: chapulina <[email protected]> * changelog Signed-off-by: Louise Poubel <[email protected]> * 3.4.3 * Measure IMU orientation with respect to world (ros-simulation#1058) Report the IMU orientation from the sensor plugin with respect to the world frame. This complies with convention documented in REP 145: https://www.ros.org/reps/rep-0145.html In order to not break existing behavior, users should opt-in by adding a new SDF tag. Co-authored-by: Jacob Perron <[email protected]> * Measure IMU orientation with respect to world (dashing) (ros-simulation#1065) Report the IMU orientation from the sensor plugin with respect to the world frame. This complies with convention documented in REP 145: https://www.ros.org/reps/rep-0145.html In order to not break existing behavior, users should opt-in by adding a new SDF tag. Co-authored-by: Jacob Perron <[email protected]> * wait for service with variable timeout (ros-simulation#1090) * wait for service with variable timeout Signed-off-by: Karsten Knese <[email protected]> Co-authored-by: chapulina <[email protected]> * Backport Gazebo11/Bionic fix for boost variant (ros-simulation#1102) * Backport Gazebo11/Bionic fix for boost variant (ros-simulation#1103) * Prepare changelogs * 3.3.5 * Prepare changelogs * 3.4.4 * [eloquent] Fix Windows build. (ros-simulation#1077) * Adding Windows bringup. * Remove unported gazebo_ros_control (ros-simulation#1108) Signed-off-by: Louise Poubel <[email protected]> * use target include directories (ros-simulation#1040) Signed-off-by: Karsten Knese <[email protected]> Co-authored-by: Louise Poubel <[email protected]> * Apply acceleration until both left and right reach targetspeed (ros-simulation#1009) Co-authored-by: Louise Poubel <[email protected]> * Fix all Foxy tests Signed-off-by: Louise Poubel <[email protected]> * uncrustifyyyyyy Signed-off-by: Louise Poubel <[email protected]> * changelog Signed-off-by: Louise Poubel <[email protected]> * 3.5.0 Co-authored-by: Shivesh Khaitan <[email protected]> Co-authored-by: chapulina <[email protected]> Co-authored-by: Karsten Knese <[email protected]> Co-authored-by: alexfneves <[email protected]> Co-authored-by: Jacob Perron <[email protected]> Co-authored-by: Jonathan Noyola <[email protected]> Co-authored-by: Jose Luis Rivero <[email protected]> Co-authored-by: Dirk Thomas <[email protected]> Co-authored-by: Steven Peters <[email protected]> Co-authored-by: Sean Yen <[email protected]> Co-authored-by: scgroot <[email protected]>
Append the name of the plugin specified in the sdf to avoid crashing when multiple video plugins are used. This connects to #898