ROSbot 2R, XL webots_ros2 support

[delihus]

Description
ROSbot 2R and ROSbot XL webots_ros2 support

Related Topics
cyberbotics/webots#5973

Affected Packages
List of affected packages:

• webots_ros2_husarion

[delihus]

I really don't want to copy the robots descriptions to the webots_ros2.
https://github.com/cyberbotics/webots_ros2/actions/runs/5220510624/jobs/9423668157?pr=770
I found an error with ROSbots.protos. I will try to fix this.

[robot_state_publisher-3] Error:   link 'solid' is not unique.
[robot_state_publisher-3]          at line 178 in ./urdf_parser/src/model.cpp
[robot_state_publisher-3] Failed to parse robot description using: urdf_xml_parser/URDFXMLParser
[robot_state_publisher-3] [WARN] [1686302208.945937669] [robot_state_publisher]: Unable to initialize urdf::model from robot description

[delihus]

I really don't want to copy the robots descriptions to the webots_ros2. https://github.com/cyberbotics/webots_ros2/actions/runs/5220510624/jobs/9423668157?pr=770 I found an error with ROSbots.protos. I will try to fix this.

[robot_state_publisher-3] Error:   link 'solid' is not unique.
[robot_state_publisher-3]          at line 178 in ./urdf_parser/src/model.cpp
[robot_state_publisher-3] Failed to parse robot description using: urdf_xml_parser/URDFXMLParser
[robot_state_publisher-3] [WARN] [1686302208.945937669] [robot_state_publisher]: Unable to initialize urdf::model from robot description

I found the problem. When I changed the name of Solid in RpLidarA2.proto in Lidar.rotatingHead to rotating_head I got the same error with another name:

[robot_state_publisher-3] Error:   link 'rotating_head' is not unique.
[robot_state_publisher-3]          at line 178 in ./urdf_parser/src/model.cpp
[robot_state_publisher-3] Failed to parse robot description using: urdf_xml_parser/URDFXMLParser
[robot_state_publisher-3] [WARN] [1686305352.530253249] [robot_state_publisher]: Unable to initialize urdf::model from robot description

When I delete whole Lidar.rotatingHead the error does not appear. It looks like the Lidar.rotatingHead doubles the Solid tag.

If this work I could remove rosbot_description and publish description from the .proto file. The submodules would not be necessary then.

[ygoumaz]

You are right, I think including the submodules is not useful and it would be cleaner to use the URDF exported by Webots from the PROTO. There is indeed a problem with the export of the RPLidarA2 rotating head. I am checking the code of Webots to see if I can fix the bug before releasing the R2023b version. In the end, the cleanest solution would be to have a webots_ros2_husarion package with launch files and resources that you already added but without additional submodule components. In addition, a simple URDF file in the resource directory would allow to specify the ROS2 control joints and sensor topics (in the same way as https://github.com/cyberbotics/webots_ros2/blob/develop/webots_ros2_tiago/resource/tiago_webots.urdf).

[ygoumaz]

We are preparing the repository for the release of the next Webots version, so I rebased this PR to master, as we just merged develop into master (#779).

[ygoumaz]

The fix should be merged tomorrow, so you should be able to update the branch and remove the submodules.

[ygoumaz]

The fix is now merged, so you can already test with the master branch of Webots. The CI tests will use the fix tomorrow with the new nightly build.

What remains to be done in this PR:

• Synchronize with base branch.
• Remove submodules and replace them with a URDF file in the resource directory.
• Add a CHANGELOG.rst file in the package.
• Add a test in webots_ros2_tests to make sure the package works correctly.

[delihus]

As I see in https://github.com/cyberbotics/webots_ros2/actions/runs/5344139662/jobs/9688152381?pr=770 the webots_ros2_husarion is built but test cannot find the sources.

[ygoumaz]

So the RPLidarA2 is giving nan values in output in the CI? Or is this a problem introduced by the Lidar plugin in the webots_ros2_driver package?

[delihus]

These are logs from github action and there are inf and nans. I think there should be no nans:

print(message.ranges)
array('f', [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 0.217191681265831, 0.22657224535942078, 0.22516748309135437, 0.22383493185043335, 0.23317134380340576, 0.23192067444324493, 0.2412281483411789, 0.2505028247833252, 0.24936223030090332, 0.25863850116729736, 0.2575974464416504, 0.2668929398059845, 0.26595744490623474, 0.2752895951271057, 0.2846313416957855, 0.28385210037231445, 0.2932591438293457, 0.30269402265548706, 0.312163770198822, 0.32167527079582214, 0.3312353789806366, 0.34085118770599365, 0.34051448106765747, 0.3502700626850128, 0.37010273337364197, 0.3800116777420044, 0.3900119662284851, 0.4101138412952423, 0.42032408714294434, 0.4406657814979553, 0.4511265754699707, 0.47175952792167664, 0.49256524443626404, 0.5236295461654663, 0.5448492169380188, 0.576405942440033, 0.6082544326782227, 0.6404205560684204, 0.6831260323524475, 0.726268470287323, 0.7801486253738403, 0.8346157670021057, 0.9000619649887085, 0.9766687750816345, 1.0750747919082642, 1.3110227584838867, 1.4758588075637817, 1.695791482925415, 1.9825382232666016, 2.380342483520508, 2.967017889022827, nan, nan, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 5.3751726150512695, 5.265248775482178, 5.180513381958008, 5.120338439941406, 5.073215961456299, 5.03884744644165, 5.0169572830200195, 4.9965739250183105, 4.988321304321289, 4.981387615203857, 4.975752830505371, 4.98188591003418, 5.02049446105957, 5.028804779052734, 5.038277626037598, 5.048910140991211, 5.0607008934021, 5.073650360107422, 5.087759971618652, 5.103033542633057, 5.1194748878479, 5.14720344543457, 5.16597843170166, 5.185946941375732, 5.207118034362793, 5.239541530609131, 5.263143539428711, 5.298004627227783, 5.3241047859191895, 5.361487865447998, 5.400166034698486, 5.430166721343994, 5.471518516540527, 5.514251708984375, 5.548382759094238, 5.593969345092773, 5.6410393714904785, 5.68963098526001, 5.739785194396973, 5.791545867919922, 5.8550705909729, 5.910205841064453, 5.967092990875244, 6.025783538818359, 6.096563339233398, 6.1590681076049805, 6.233858108520508, 6.300433158874512, 6.379517078399658, 6.460886478424072, 6.597066402435303, 6.683532238006592, 6.7725677490234375, 6.864272117614746, 6.9587483406066895, 6.915848731994629, 6.830679893493652, 6.6367292404174805, 6.573853492736816, 6.500601291656494, 6.439030170440674, 6.3893561363220215, 6.238186836242676, 6.1891398429870605, 6.152326583862305, 6.0229878425598145, 5.986962795257568, 5.868393421173096, 5.845071315765381, 5.822712421417236, 5.727569580078125, 5.718719959259033, 5.635613918304443, 5.56463098526001, 5.55773401260376, 5.486730575561523, 5.428438186645508, 5.437015056610107, 5.392726421356201, 5.348482131958008, nan, 5.262134552001953, 5.237918853759766, 5.20207405090332, 5.181944847106934, 5.137155532836914, 5.120948314666748, 5.106411457061768, 5.068017959594727, 5.056958198547363, 5.047393798828125, 5.014697551727295, 5.008263111114502, 4.979135990142822, 4.963733196258545, 4.961638450622559, 4.937450885772705, 4.9496169090271, 4.928389549255371, 4.920101165771484, 4.935692310333252, 4.918704032897949, 4.914277076721191, 4.900108814239502, 4.887343883514404, 4.919391632080078, 4.9090657234191895, 3.7243943214416504, 2.782052516937256, 2.217264413833618, 1.8412439823150635, 1.5803662538528442, 1.3723297119140625, 1.2165088653564453, 1.0921374559402466, 0.9888005256652832, 0.9062818884849548, 0.8343452215194702, 0.7728825807571411, 0.7218024730682373, 0.6810289621353149, 0.6404938101768494, 0.6001665592193604, 0.5700175166130066, 0.5400165915489197, 0.5201443433761597, 0.49037808179855347, 0.4707111716270447, 0.4511265754699707, 0.43160974979400635, 0.4221987724304199, 0.40279194712638855, 0.39350223541259766, 0.37415823340415955, 0.3649526834487915, 0.3557891845703125, 0.3466610014438629, 0.33756139874458313, 0.32848361134529114, 0.31942084431648254, 0.3103661835193634, 0.3013126850128174, 0.30269095301628113, 0.2831808924674988, 0.2740880846977234, 0.27556613087654114, 0.26647019386291504, 0.26805657148361206, 0.2589397430419922, 0.2606300711631775, 0.2514740824699402, 0.24225257337093353, 0.24404987692832947, 0.23475632071495056, 0.23664280772209167, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan])

Locally webots_ros2 echo:

header:
  stamp:
    sec: 80
    nanosec: 896000000
  frame_id: laser
angle_min: 3.141592025756836
angle_max: -3.141592025756836
angle_increment: -0.015747329220175743
time_increment: 0.0
scan_time: 0.0
range_min: 0.20000000298023224
range_max: 12.0
ranges:
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- 0.2780417501926422
- 0.28191909193992615
- 0.2995957136154175
- 0.3178681433200836
- 0.3508087396621704
- 0.3788739740848541
- 0.4146113395690918
- 0.4493940770626068
- 0.5369895100593567
- 6.35524845123291
- 6.361996173858643
- 6.357226371765137
- 6.315924644470215
- 6.327488899230957
- 6.340324401855469
- 6.305247783660889
- 6.32111120223999
- 6.302095890045166
- 6.273114204406738
- 6.305171489715576
- 6.291457653045654
- 6.3257975578308105
- 6.303756237030029
- 6.30636739730835
- 6.344282627105713
- 6.338422775268555
- 6.3342061042785645
- 6.34261417388916
- 6.341522693634033
- 6.396297931671143
- 6.408761024475098
- 6.422635078430176
- 6.441125869750977
- 6.472313404083252
- 6.483658313751221
- 6.507784366607666
- 6.544974327087402
- 6.58435583114624
- 6.614729881286621
- 6.635887145996094
- 6.681978702545166
- 6.718989372253418
- 6.758354187011719
- 6.800154209136963
- 6.832573413848877
- 6.891412258148193
- 6.953218936920166
- 6.993536949157715

The real RPLidar:

header:
  stamp:
    sec: 1689083986
    nanosec: 600664010
  frame_id: laser
angle_min: -3.1241390705108643
angle_max: 3.1415927410125732
angle_increment: 0.01745329238474369
time_increment: 0.00031720675178803504
scan_time: 0.11387722194194794
range_min: 0.15000000596046448
range_max: 12.0
ranges:
- .inf
- 0.4612500071525574
- .inf
- 0.45625001192092896
- 0.45524999499320984
- .inf
- 0.4582499861717224
- .inf
- .inf
- 0.4702500104904175
- .inf
- 0.46274998784065247
- .inf
- 0.4404999911785126
- .inf
- .inf
- 0.5214999914169312
- .inf
- 0.49125000834465027
- .inf
- 0.5087500214576721
- 0.30649998784065247
- .inf
- 0.28824999928474426
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- 0.7724999785423279
- .inf
- 0.7582499980926514
- .inf
- 0.8159999847412109
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- 0.14875000715255737

It looks like somehow the remote action build changes infs to nans?

[ygoumaz]

The Iron tests fail because the /odometry/filtered topic outputs no linear movement (published coordinates are always (0.0, 0.0, 0.0)). This is probably related to the diffdrive_controller or the ekf_node of the robot_localization package. I tried multiple configurations but was not able to make it work. It works well on other distributions.

I see two possibilities about this issue:

• There exists a nice and simple fix and we should apply it. We may ask for help and open an issue on the robot_localization repository.
• As we are in the webots_ros2 package and try to make some simple examples we may consider removing the robot_localization part and only rely on the default /odom topic. Reducing the number of dependencies is never a bad idea in ROS 2 package management I think...

@delihus, any thoughts?

[ygoumaz]

I just have a few minor suggestions on the code. Once cyberbotics/webots#6324 is merged, it should be tested to confirm that Lidar messages are now correctly displayed.

[ygoumaz]

Two last details:

• With the rebase, the version of the changelog becomes 2024.0.0
• The main changelog file of the package should have a new section 2024.0.0 and a new entry to explain the addition of the husarion sub-package: https://github.com/husarion/webots_ros2/blob/develop-husarion/webots_ros2/CHANGELOG.rst

[ygoumaz]

Everything is alright to me! Once the branch is updated with upstream branch (using the Update branch button) and tests pass, you can merge it!