[Bug]: Building plugins might be incompatible with ros2_control about rmf_simulation HOT 8 CLOSED

Hello!

I admit not having tried anything like this before, on a first glance though, the door that you inserted is a lift shaft door, did you try to insert just a normal door as well?

from rmf_simulation.

I have also tried with the SwingDoor model below and still having the exactly same crash as before. Without the plugin part, the door renders just fine as expected.

<model name="L1_door4">
  <pose>19.549120677562087 -11.10939551168469 0.0 0 0 -0.009497018345977264</pose>
  <link name="right">
    <pose>0 0 1.11 0 0 0</pose>
    <visual name="right_visual">
      <geometry>
        <box>
          <size>1.5905993122484277 0.03 2.2</size>
        </box>
      </geometry>
      <material>
        <ambient>0.5 0.25 0 0.6</ambient>
        <diffuse>0.5 0.25 0 0.6</diffuse>
        <pbr>
          <metal>
            <metalness>0.0</metalness>
          </metal>
        </pbr>
      </material>
    </visual>
    <collision name="right_collision">
      <geometry>
        <box>
          <size>1.5905993122484277 0.03 2.2</size>
        </box>
      </geometry>
      <surface>
        <contact>
          <collide_bitmask>0x02</collide_bitmask>
        </contact>
      </surface>
    </collision>
    <inertial>
      <mass>50.0</mass>
      <inertia>
        <ixx>20.17041666666667</ixx>
        <iyy>30.70835905052155</iyy>
        <izz>10.545442383854882</izz>
      </inertia>
    </inertial>
  </link>
  <joint name="right_joint" type="revolute">
    <parent>world</parent>
    <child>right</child>
    <axis>
      <xyz>0 0 1</xyz>
      <limit>
        <lower>0</lower>
        <upper>1.57</upper>
      </limit>
    </axis>
    <pose>0.8002996561242138 0 0 0 0 0</pose>
  </joint>
  <plugin name="door" filename="libdoor.so">
    <v_max_door>0.5</v_max_door>
    <a_max_door>0.3</a_max_door>
    <a_nom_door>0.15</a_nom_door>
    <dx_min_door>0.01</dx_min_door>
    <f_max_door>500.0</f_max_door>
    <door name="L1_door4" type="SwingDoor" left_joint_name="empty_joint" right_joint_name="right_joint" />
  </plugin>
</model>

from rmf_simulation.

I looked at the husarion world (I'm assuming this?) Something that strikes me is that there is no physics system, we do need physics to simulate doors. You can look at the empty world template that we use to add on all the RMF models here. I'd suggest at least adding the world systems such as Physics and SceneBroadcaster and see if that helps

from rmf_simulation.

I have taken a look to the template and added to my sdf the following below, however it still crashes with the same log.

<plugin
    filename="libgz-sim-physics-system.so"
    name="gz::sim::systems::Physics">
</plugin>
<plugin
    filename="libgz-sim-user-commands-system.so"
    name="gz::sim::systems::UserCommands">
</plugin>
<plugin
    filename="libgz-sim-scene-broadcaster-system.so"
    name="gz::sim::systems::SceneBroadcaster">
</plugin>

I have also tested spawning the robot directly into the hotel_ign map, but it only works when the door/lift plugins are not enabled, otherwise it crashes too.

from rmf_simulation.

I continued debugging this stuff and have some interesting insights!

First, instead of using the provided docker image, I started from a humble-desktop-full one and did the manual installation of the rosbot-xl workspace (https://github.com/husarion/rosbot_xl_ros), which seems to be more up-to-date. Now the simulation doesn't crash, but libign_ros2_control still fails to configure the robot.

After some testing, it turned out that when the door plugin is added, the controller manager is not setting up the ROS parameters from the config file. Thus, I checked out the libign_ros2_control code and the interesting bit is that it is setting the arguments when initializing the ROS context.

// ...... some parameters parsing .....
if (!rclcpp::ok()) {
  rclcpp::init(static_cast<int>(argv.size()), argv.data());
}

Now, both rmf_building_plugins and libign_ros2_control are defined as ignition::gazebo::System plugins and therefore they seems to share the same context. As the door plugin is part of the world definition, it gets to load first and create the rclcpp context, so when the robot is spawned the arguments can't be set or you would get a rclcpp::ContextAlreadyInitialized error.

It seems this is a known "edge-case" as I found the following discussion regarding a similar issue while developing a new plugin: https://robotics.stackexchange.com/questions/110448/multiple-plugins-with-ros2-node-in-gazebo-simulation.

TLDR: This issue is simply not directly related to open-rmf, so feel free to close the bug report, but thanks for your quick reponse anyway :)

from rmf_simulation.

from rmf_simulation.

Right now the gazebo world is loaded first, then the robot urdf is created on runtime using xacro with the launch file parameters, and spawned using ros_gz_sim's create node together with the ros_gz_bridge's parameter_bridge node.

So in this case, the easiest workaround was to print the generated urdf, take all the ros2_control parameters from there, and hard-code it into the floor plugin code:

    int argv_count = 7;
    const char* argv[argv_count] = {
        "--ros-args",
        "--params-file",
        "/home/yaymalaga/rosbot_ws/install/rosbot_xl_controller/share/rosbot_xl_controller/config/diff_drive_controller.yaml",
        "--remap",
        "rosbot_xl_base_controller/cmd_vel_unstamped:=cmd_vel",
        "--remap",
        "/tf:=tf"};
    std::string name;
    auto door_ele = sdf->GetElementImpl("door");
    get_sdf_attribute_required<std::string>(door_ele, "name", name);
    if (!rclcpp::ok())
      rclcpp::init(argv_count, argv);

Of course, this is not an ideal solution as we need to know these arguments in advance. I agree adding the argv parsing to these plugins could improve the situation a bit, so just modifying the world would be enough instead of recompiling.

That aside, I think the real issue lies in the ign_ros2_control, which has several issues opened talking about this matter, specially ros-controls/gz_ros2_control#166.

from rmf_simulation.

Interesting, fair enough, I'll close this but let me know if you plan to explore the argv parsing in rmf_simulation stragety!

from rmf_simulation.