`reference_coordinates` setting in config.yaml does not work

[royito55]

Bug report

Required information:

• Operating system and version:

  • Ubuntu 22

• OpenRMF installation type:

  • Binary except for rmf_demos (not available in Humble)

• ROS distribution and version:

  • ROS2 Humble

• ROS installation type:

  • from binaries

• Package or library, if applicable:

  • N/A

Description of the bug

So I've been trying to figure out how the transform configuration works but it simply doesn't. Perhaps I am missing something. This happens with both slotcar and using nav2.

Basically, I have a map like this:

From what I see, the origin of this map is at the top left corner, x pointing to the right and y pointing up.

And a simulation like this (note where the origin is): Since I am using slotcar in this example, then the pose of the robot in the simulation will be the pose advertised by /robot_state

The room with the furniture corresponds to the RMF map.

Now, I am following the instructions to adapt the coordinates and this is my process (using Slotcar):

1. The actual initial position of the robot I'm testing with with respect to the gazebo simulation origin is [15, -17]
2. I have an RMF node at that position, so I open the traffic editor and check the coordinates that show when I click on that node: [8, -4]
3. So, I have my first relationship. This means that when the robot says it is at [15, -17], it should be at [8, -4] in the RMF coordinate system, so the first set of points would be:

reference_coordinates:
  rmf: [8, -4]
  robot: [15, -17]

4. I keep this logic for the next 3 points. This leads me to the following in my config.yaml:

reference_coordinates:
  rmf: [[8.0, -4.0],
        [0.81, -1.25],
        [5.1870, -4.0791 ],
        [5.1870, -0.86]]
  robot: [[15.0, -17.0],
        [7.9, -14.1],
        [12.08, -17.0],
        [12.08, -14.0]]

However, this does nothing. I launch RMF and still see it at the same spot.

I already checked that I have nudged installed.

What am I doing wrong here? Does this work? Here's a video that shows what I mean (too big to paste here): https://drive.google.com/file/d/1k76fFCez7KlGElAhn2GuGOgw5_BL9NC7/view?usp=sharing

[LKSeng]

In the original fleet_adapter template, the logs print out the transforms produced by nudged (see link).

Are you able to log what the transforms are and are they (hand wavingly) what you expect?

[MatteoCarlone]

@royito55 Did you solve the problem with reference coordinates?

[Yadunund]

@royito55 as @LKSeng mentioned, it may be helpful if you could share the temrinal printout when launching the adapter (in plain text form) here so that we can see the output of nudged.

if you're running with slotcar in simulation, the list of rmf and robot coordinates will be identical cos the position reported by the slot car plugin will be same as the coordiantes on the map in traffic editor (ie, the gazebo world and the traffic editor map are aligned).

[Vortex-TH]

I have this problem also with the latest version of the fleet adapter. Changes to reference_coordinates don't seem to have any effect at all. I tested it for example with the demo office simulation.

If I change it to, let's say:

reference_coordinates:
    L1:
        rmf: [[0.0, 0.0 ],
              [1.0, 1.0],
              [2.0, 2.0],
              [3.0, 3.0]]
        robot: [[2.0, 2.0 ],
              [3.0, 3.0],
              [4.0, 4.0],
              [5.0, 5.0]]

Shouldn't the robots position be changed 2m in each direction, or am I missing something?

[Yadunund]

I have this problem also with the latest version of the fleet adapter. Changes to reference_coordinates don't seem to have any effect at all. I tested it for example with the demo office simulation.

If I change it to, let's say:

reference_coordinates:
    L1:
        rmf: [[0.0, 0.0 ],
              [1.0, 1.0],
              [2.0, 2.0],
              [3.0, 3.0]]
        robot: [[2.0, 2.0 ],
              [3.0, 3.0],
              [4.0, 4.0],
              [5.0, 5.0]]

Shouldn't the robots position be changed 2m in each direction, or am I missing something?

@arjo129 @xiyuoh could you take a look?

[xiyuoh]

Hi @Vortex-TH!

May I check, you're using the latest fleet adapter template? The transformation is performed in this compute_transforms function, which is called here when you parse in the transformation coordinates for each map. Could you share the printouts for the transformation error estimate? The latest template is still using nudged to estimate transformation values.

[Vortex-TH]

I used the implementation from rmf_demos which should be up to date with the template. I only added the compute_transforms function since it is missing in the demo.

In the tinyRobot_config.yaml for the office demo, I added the reference_coordinates mentioned above and made sure they are read correctly by the fleet adapter. The error estimate for those coordinates is 0, which is expected with those exact values I provided:

[fleet_adapter-16] [INFO] [1700869852.672878003] [tinyRobot_command_handle]: Transformation error estimate for L1: 0.0

With those values, I would assume the position of the robots shown in RViz to be shifted by an offset of 2x2m compared to the real position (as shown in Gazebo in this case).

Edit: I shared my adjusted fleet_adapter.py for use with the demo here.

Edit2: And for context, I use the latest binary Iron version of RMF now

[xiyuoh]

Thanks for providing the info! I found where the errors lie: 1) The fleet adapter template adds the fleet configuration before adding the robot transformations to the fleet config 2) The transformation library in rmf_ros2 had a minor bug that we noticed earlier but fixed in another branch along with several other new features, so it hasn't made it into main or the binraries.

I've tried out your 2x2m offset config with the fixes and I think they're working now. Feel free to try them out:

• Apply the fixes from this commit in fleet_adapter_template, and
• Check out and build branch xiyu/transforms on rmf_ros2. This means you'd need to build RMF from source following the instructions here.

Let me know if it works for you!

[Vortex-TH]

I can confirm that it works as expected with the build from source.

I would prefer to use the binaries with my real application though. So, the second problem was that the transform was not inverted?

Is there maybe an easy way to do it in Python as a workaround?

[xiyuoh]

You can perform the transformation from the Python fleet adapter and passing it to RMF by using the transforms obtained via nudged to convert from RMF to robot coordinates. For robot to RMF, you can estimate the transforms in the opposite direction:

rmf_to_robot_tf = nudged.estimate(rmf_coords, robot_coords)
robot_to_rmf_tf = nudged.estimate(robot_coords, rmf_coords)

and use nudged's transform() API to convert your coordinates:

[rmf_x, rmf_y] = robot_to_rmf_tf.transform([robot_x, robot_y])
rmf_yaw = robot_yaw + robot_to_rmf_tf.get_rotation()  # this is the orientation offset

and vice versa for robot coords.

I've opened the relevant PRs https://github.com/open-rmf/fleet_adapter_template/pull/26 and https://github.com/open-rmf/rmf_ros2/pull/309. For the latter, the changes will be updated in the binaries after it's approved and merged. The next binary sync is scheduled to be in 2 weeks' time, so you can continue using source built for now and update your binaries when that happens. Thanks for catching the error!

[Vortex-TH]

Thank you! I'll use that until the binaries are updated.

[mrceki]

I am encountering an issue with the robot's position when using reference coordinates in rmf_config. Specifically, when entering reference coordinates, the robot's location is computed incorrectly. However, if I switch the order of RMF and robot coordinates in the compute_transforms function (i.e., compute the transformation from robot to RMF instead of RMF to robot), the transformation works correctly and the robot can be added to the fleet. Unfortunately, this causes an issue where the translation is inverted during navigation, resulting in incorrect target positions being sent. I'm not sure if the problem is related to this PR open-rmf/rmf_ros2#309 , but I thought it might be possible.

tf = nudged.estimate(rmf_coords, robot_coords)

[INFO] [1719323051.205529884] [demo_fleet_adapter]: Adding robot [TB3_1] to fleet [demo]. [ERROR] [1719323051.205878522] [demo_fleet_adapter]: Unable to compute a location on the navigation graph for robot [TB3_1] being added to fleet [demo] using map [L1] and position [-3.315, 17.640, 0.696] specified in the initial_state argument. This can happen if the map in initial_state does not match any of the map names in the navigation graph supplied or if the position reported in the initial_state is far away from the navigation graph. This robot will not be added to the fleet.

[xiyuoh]

Hello @mrceki ! Could you provide a sample of the config (reference coordinates portion) you used? It would also help to know which ROS 2 you're using, and whether you're working off RMF binaries or built from source.

[mrceki]

Hello @mrceki ! Could you provide a sample of the config (reference coordinates portion) you used? It would also help to know which ROS 2 you're using, and whether you're working off RMF binaries or built from source. But honestly I don't remember when was the last time I pulled the repo?

Hi @xiyuoh ! Thanks for the fast response. I'm using Humble and built RMF from source.

I'm currently working on tb3 map and the another with 30 degree rotated version.

First map:

reference_coordinates:
  L1:
    rmf: [[8.67, -11.2],
          [11.4, -11.2],
          [11.5, -7.27],
          [8.72, -7.21]
          ]
    robot: [[-1.35, -1.95],
          [1.35, -2.00],
          [1.45, 1.95],
          [-1.3, 2.0]
          ]

30 degree rotated map:

reference_coordinates:
  L1:
    rmf: [[13.2, -15.1],
          [15.6, -13.8],
          [13.7, -10.3],
          [11.2, -11.6]          
          ]
    robot: [[-1.35, -1.95],
          [1.35, -2.00],
          [1.45, 1.95],
          [-1.3, 2.0],
          ]

[xiyuoh]

Gotcha, can I check that you've applied the changes from this commit? There should be no need to add anything else to the fleet adapter template if you are using the latest code from rmf_ros2 main, as the inverse transform computation has been merged in already.

For sanity check, I've tried running both sets of coordinates you provided, and the transformation values from RMF to robot seems correct to me.

First map:

[fleet_adapter-15] ic| tf.get_rotation(): -0.002418257483943672
[fleet_adapter-15] ic| tf.get_scale(): 0.9947386598499548
[fleet_adapter-15] ic| tf.get_translation(): [-9.959796850686251, 9.195693346234409]

30 degree rotated values:

[fleet_adapter-15] ic| tf.get_rotation(): -0.5204454432998059
[fleet_adapter-15] ic| tf.get_scale(): 0.9848554270160192
[fleet_adapter-15] ic| tf.get_translation(): [-5.21395936414366, 17.426323823560587]

I might be missing something here, may I check with you the following:

• what is the actual position of your robot in robot coordinates,
• what are the RMF coordinates reflected in the ROS 2 topic /fleet_states, and
• what are the RMF coordinates reported by RMF in the Unable to compute a location ... log?

Do let me know which map you are using when providing these values too 😄

[mrceki]

Thanks @xiyuoh !! Turns out I was on version 2.3.2 from source. I tried both the binary and the latest versions from source, but there were compatibility issues (in rmf_fleet_msgs for mutex groups for binary installation and EasyFullControlAPI). So I switched to rolling from humble after seeing #23 and everything works fine!

[xiyuoh]

No worries, good that everything is working now 😄 I'll close this ticket as changes from both PRs mentioned have been merged in.