[Bug]: Building plugins might be incompatible with ros2_control #125
Comments
yaymalaga
changed the title
|
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? |
|
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> |
|
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 |
|
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. |
|
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. Now, both 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 :) |
|
Thanks for the debugging! This is very interesting.
I assume since you say that only the rmf plugin is initialized at world
creation time that the model containing the ros2_control system is not in
the world but for example spawned through a service call? If that's the
case I agree this would be tough.
However! Can you give a try adding argv parsing to the rclpp initialization
in the rmf door plugin and see if that helps? There is a TODO (
https://github.com/open-rmf/rmf_simulation/blob/be887b441992caff0518903abf27dd887819b30e/rmf_building_sim_gz_plugins/src/door.cpp#L140)
about parsing command line arguments, it's just that since we don't use
them I never got around to it.
If parsing command line arguments there helps (so when the context is
initialized in the door plugin it can also set ros parameters) and that
doesn't bring any side effect, which unless I'm missing something I don't
believe it should, I would be happy to merge a PR with this fix!
…On Fri, Jun 7, 2024, 5:46 AM yaymalaga ***@***.***> wrote:
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 they are set as 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 :)
—
Reply to this email directly, view it on GitHub
<#125 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/ACFQQBW76GLHQDQBMN2IZALZGDKB3AVCNFSM6AAAAABIYNERX6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDCNJTGQ2TOMJSHE>
.
You are receiving this because you commented.Message ID:
***@***.***>
|
|
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. |
|
Interesting, fair enough, I'll close this but let me know if you plan to explore the argv parsing in |
Before proceeding, is there an existing issue or discussion for this?
OS and version
Ubuntu 22.04.3
Open-RMF installation type
Binaries
Other Open-RMF installation methods
No response
Open-RMF version or commit hash
humble-release
ROS distribution
Humble
ROS installation type
Docker
Other ROS installation methods
No response
Package or library, if applicable
No response
Description of the bug
I'm trying to integrate the building plugins into a rosbot simulation, specifically the door and lift ones, however the simulation just panics on start even though the plugin itself seems to load just fine as shown below. Given the logs, it seems the plugins are colliding somehow with ros2_control.
Steps to reproduce the bug
For the bug report, I'm using the following docker image provided by Husarion:
husarion/rosbot-xl-gazebo:humble.I started a container with the root user so I could install rmf:
apt-get update && apt-get install ros-humble-rmf-dev. Then, I modified the default world (/ros2_ws/install/husarion_office_gz/share/husarion_office_gz/worlds/husarion_world.sdf) to include a door model together with its plugin as shown below (extracted from the rmf hote_ign demo):Finally, I exported the gazebo plugin dir and launched the simulation using:
If you just comment out the plugin part of the sdf, the simulation loads just fine, so the model is not the issue.
Just to reiterate, I'm not planning to integrate rmf into the simulation, but just to make use of the door and lift plugins as they are pretty handy to do some multi-floor testing.
Expected behavior
The simulation runs just fine and I can control the door and the robot without issues.
Actual behavior
The simulation simply panics.
Additional information or screenshots
No response
The text was updated successfully, but these errors were encountered: