Issue with multi_robot simulation

[moutalibbadr]

I've been working on Launching multiple create3 robots in simulation with ignition edifice and ROS2. This feature is not supported out of the box with the create3_sim package and so I had to do some modifications to enable it. However I ran into a segfault error

[INFO] [1650890806.787380428] [create3_0.controller_manager]: Loading controller 'joint_state_broadcaster'
Stack trace (most recent call last):
#31   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af66adaa, in 
#30   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af65e025, in 
#29   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af5bc1be, in 
#28   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af4fc5d2, in rb_protect
#27   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af6759f0, in rb_yield
#26   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af66886f, in rb_vm_exec
#25   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af662130, in 
#24   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af651405, in 
#23   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af66adaa, in 
#22   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af65e025, in 
#21   Object "/usr/lib/x86_64-linux-gnu/ruby/2.7.0/fiddle.so", at 0x7f93ab4fe714, in 
#20   Object "/lib/x86_64-linux-gnu/libruby-2.7.so.2.7", at 0x7f93af62f6d9, in rb_nogvl
#19   Object "/usr/lib/x86_64-linux-gnu/ruby/2.7.0/fiddle.so", at 0x7f93ab4fe8fb, in 
#18   Object "/lib/x86_64-linux-gnu/libffi.so.7", at 0x7f93ab4ad409, in 
#17   Object "/lib/x86_64-linux-gnu/libffi.so.7", at 0x7f93ab4adff4, in 
#16   Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo5-ign.so.5.4.0", at 0x7f93aaa4a74e, in runServer
#15   Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5d041b, in ignition::gazebo::v5::Server::Run(bool, unsigned long, bool)
#14   Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5da793, in 
#13   Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e815a, in ignition::gazebo::v5::SimulationRunner::Run(unsigned long)
#12   Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e794a, in ignition::gazebo::v5::SimulationRunner::Step(ignition::gazebo::v5::UpdateInfo const&)
#11   Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e0561, in ignition::gazebo::v5::SimulationRunner::UpdateSystems()
#10   Object "/usr/lib/x86_64-linux-gnu/ign-gazebo-5/plugins/libignition-gazebo-user-commands-system.so", at 0x7f9385749178, in ignition::gazebo::v5::systems::UserCommands::PreUpdate(ignition::gazebo::v5::UpdateInfo const&, ignition::gazebo::v5::EntityComponentManager&)
#9    Object "/usr/lib/x86_64-linux-gnu/ign-gazebo-5/plugins/libignition-gazebo-user-commands-system.so", at 0x7f938574b7bd, in ignition::gazebo::v5::systems::CreateCommand::Execute()
#8    Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa59bc90, in ignition::gazebo::v5::SdfEntityCreator::CreateEntities(sdf::v11::Model const*)
#7    Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5c5fb6, in void ignition::gazebo::v5::EventManager::Emit<ignition::common::EventT<void (unsigned long, std::shared_ptr<sdf::v11::Element>), ignition::gazebo::v5::events::LoadPluginsTag>, unsigned long const&, std::shared_ptr<sdf::v11::Element> const&>(unsigned long const&, std::shared_ptr<sdf::v11::Element> const&)
#6    Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e3b86, in ignition::gazebo::v5::SimulationRunner::LoadPlugins(unsigned long, std::shared_ptr<sdf::v11::Element> const&)
#5    Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e36b4, in ignition::gazebo::v5::SimulationRunner::LoadPlugin(unsigned long, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<sdf::v11::Element> const&)
#4    Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e1038, in ignition::gazebo::v5::SimulationRunner::AddSystem(ignition::plugin::TemplatePluginPtr<ignition::plugin::SpecializedPlugin<ignition::gazebo::v5::System, ignition::gazebo::v5::ISystemConfigure, ignition::gazebo::v5::ISystemPreUpdate, ignition::gazebo::v5::ISystemUpdate, ignition::gazebo::v5::ISystemPostUpdate> > const&, std::optional<unsigned long>, std::optional<std::shared_ptr<sdf::v11::Element const> >)
#3    Object "/lib/x86_64-linux-gnu/libignition-gazebo5.so.5", at 0x7f93aa5e0c1a, in ignition::gazebo::v5::SimulationRunner::AddSystemImpl(ignition::gazebo::v5::SystemInternal, std::optional<unsigned long>, std::optional<std::shared_ptr<sdf::v11::Element const> >)
#2    Object "/home/Workspace/shroom/simulation_ws/install/ign_ros2_control/lib/libign_ros2_control-system.so", at 0x7f9384ce3cfb, in ign_ros2_control::IgnitionROS2ControlPlugin::Configure(unsigned long const&, std::shared_ptr<sdf::v11::Element const> const&, ignition::gazebo::v5::EntityComponentManager&, ignition::gazebo::v5::EventManager&)
#1    Object "/opt/ros/galactic/lib/librclcpp.so", at 0x7f938496b247, in rclcpp::Executor::add_node(std::shared_ptr<rclcpp::Node>, bool)
#0    Object "/opt/ros/galactic/lib/librclcpp.so", at 0x7f93849920e4, in rclcpp::Node::get_node_base_interface()

This error seems to be related to ign_ros2_control_plugin.cpp. It does not appear when spawning one robot.

To recreate this error run this after building this repo :

ros2 launch irobot_create_ignition_bringup create3_multi.launch.py

Any help or lead will be much appreciated. Thank you!

[moutalibbadr]

Hello, This error is still persisting and i just want to add some clarifications. The namespace modifications break the "namespaced single robot" launch if we do not declare namespacing inside the ign_ros2_control::IgnitionROS2ControlPlugin urdf element of the create3

<xacro:if value="${'$(arg gazebo)' == 'ignition'}">
    <gazebo>
      <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
        <parameters> $(find irobot_create_control)/config/control.yaml </parameters>
        <ros>
          <namespace>$(arg namespace)</namespace>
          <!-- <remapping>/tf:=/$(arg namespace)/tf</remapping> -->
        </ros>
      </plugin>
    </gazebo>
  </xacro:if>

When it is done we can spawn and move our create3 with a namespace but the above segfault error will occur if we spawn more than one create3. If we remove the plugin all together from the urdf we get no such error but our create3 robots can not be controlled.

NB : the create3_multi.launch.py is only available up to this commit 7141668b885496ebb49739371701216608a4c97a in our repo and has been removed temporarily for the purpose of validating our pull request. Therefore to reproduce the error you can get back to the old commit.

[moutalibbadr]

Hello again,

I've seen that this issue has been pulled up from the To do and I just wanted to know if you think this is not relevant to gz_ros2_control. I know you guys are busy but this is a stumbling block for us and it would be very helpful if you could share any insight on this.

Thank you!

[bmagyar]

We indeed have discussed wanting to solve this however we are currently a little swamped rolling out chaining controllers and CI fixes. @destogl do you have any useful pointers for this?

In any case @moutalibbadr please feel free to tag along to our working group meeting and add this to the agenda for discussion...

[moutalibbadr]

Hello @bmagyar thank you for the feedback. How can I join in on the group meeting ?

[bmagyar]

The next one will be on the 14th, announcing tomorrow. You can follow this rag on Ros discourse: https://discourse.ros.org/tag/wg-ros2-control

On Thu, 7 Jul 2022, 12:44 moutalibbadr, @.***> wrote:

Hello @bmagyar https://github.com/bmagyar thank you for the feedback. How can I join in on the group meeting ?

— Reply to this email directly, view it on GitHub https://github.com/ros-controls/gz_ros2_control/issues/49#issuecomment-1177477419, or unsubscribe https://github.com/notifications/unsubscribe-auth/AA24PYNNQAGYJPPL5IQHEW3VS27KTANCNFSM5UIUKVWA . You are receiving this because you were mentioned.Message ID: @.***>

[hugo-tardiou]

Hi, https://github.com/ros-controls/gz_ros2_control/blob/main/ign_ros2_control/src/ign_ros2_control_plugin.cpp#L319-L328

I believe there might be an issue here unless I am misunderstanding the code.

this->dataPtr->node_ = rclcpp::Node::make_shared(node_name); is only called on the first plugin call but after that the node_ is never set therefore add_node calls an empty pointer.

[destogl]

It seems to me that you are trying to run this with multiple controller managers, i.e., each robot has it's controller manager. In that case, you should use controller_manager_prefix_node_name in the .xacro file where Gazebo (Ignition) plugin is defined.

I got his from this line

Try this configuration:

<xacro:if value="${'$(arg gazebo)' == 'ignition'}">
  <gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
      <controller_manager_prefix_node_name>$(arg namespace)</controller_manager_prefix_node_name>
      <parameters> $(find irobot_create_control)/config/control.yaml </parameters>
      <ros>
        <namespace>$(arg namespace)</namespace>
        <!-- <remapping>/tf:=/$(arg namespace)/tf</remapping> -->
      </ros>
    </plugin>
  </gazebo>
</xacro:if>

And yes! This needs to be enhanced. We should actually use the namespace parameter to put the controller manager under a specific namespace. For this to work, you should check this setup of the controller manager under a specific namespace so you also get all controllers under the namespace and there is no collision between them. (I would also appreciate review on the demos PR if this works for you :) )

[Ronoman]

I'm running into this issue as well. With a single controller manager, I can use the controller_manager_prefix_node_name to properly set the prefix for the controller. However, adding in a second <plugin filename="libign_ros2_control-system.so" ...>...</plugin> tag to the same URDF causes the same segfault as in the original post, even with the prefix node name tag set.

I agree with @hugo-tardiou 's diagnosis about the segfault happening around those lines. I have experimented with moving around those lines of code into different scope to avoid the segfault, but I can't quite get everything to work properly (probably because I don't fully understand how the ign_ros_control node plays into the whole system).

Are multiple controller_managers for a single model currently supported in gz_ros2_control?