Robot belonging to fleet is very far

[KamalanathanN]

Hi, I have configured my ROS1 melodic robot with free_fleet and using ROS2 galactic on laptop as ros2 server and
RMF is build from source.

For creating building.yaml file i dont have the floor plan so i used the robot's scan map for both Levels and Layers. hope that's ok 😅

The pink marker indicating robot position comes inside desired location only when transform parameters in free_fleet server launch file are set to start's (x,y) with signs inverted (from above img).

COORDINATE TRANSFORMATION
   translation x (meters): -45.778
   translation y (meters): 44.955
   rotation (radians): 0.000
   scale: 1.000

I also tried the transforms values in right side of layers tab but the pink marker was out of bound.

I think this error might be due to cordinate transform mismatch between RMF and robots coordinate. although i'm not sure how to go about fixing it. i hope you can help me.

Video: https://youtu.be/cUw_Z83D_Mk
Error msg:

[full_control-13] [INFO] [1653048131.295217099] [test_fleet_fleet_adapter]: Assignments updated for robots in fleet [test_fleet] to accommodate task_id [patrol.dispatch-0]
[full_control-13] [INFO] [1653048131.298124513] [test_fleet_fleet_adapter]: Beginning new task [patrol.dispatch-0] for [test_fleet/testbot]. Remaining queue size: 0
[full_control-13] [ERROR] [1653048131.868593768] [test_fleet_fleet_adapter]: Robot named [testbot] belonging to fleet [test_fleet] is very far [2.958652m] from where it is supposed to be, but its remaining path is empty. This means the robot believes it is finished, but it is not where it's supposed to be.
[full_control-13] [ERROR] [1653048132.367955572] [test_fleet_fleet_adapter]: Robot named [testbot] belonging to fleet [test_fleet] is very far [2.958652m] from where it is supposed to be, but its remaining path is empty. This means the robot believes it is finished, but it is not where it's supposed to be.
[full_control-13] [ERROR] [1653048132.867835017] [test_fleet_fleet_adapter]: Robot named [testbot] belonging to fleet [test_fleet] is very far [2.958652m] from where it is supposed to be, but its remaining path is empty. This means the robot believes it is finished, but it is not where it's supposed to be.
[full_control-13] [ERROR] [1653048133.368504503] [test_fleet_fleet_adapter]: Robot named [testbot] belonging to fleet [test_fleet] is very far [2.958652m] from where it is supposed to be, but its remaining path is empty. This means the robot believes it is finished, but it is not where it's supposed to be.
[full_control-13] [ERROR] [1653048133.868815366] [test_fleet_fleet_adapter]: Robot named [testbot] belonging to fleet [test_fleet] is very far [2.958652m] from where it is supposed to be, but its remaining path is empty. This means the robot believes it is finished, but it is not where it's supposed to be.

Robot's rmf ws: https://github.com/KamalanathanN/testbot
Client launch file: https://gist.github.com/KamalanathanN/4b6ace6f42000e010822398b9288a0ce
Server launch file: https://gist.github.com/KamalanathanN/2f9b5385cc5ac9d73ea42ea8a900e2cb

[codebot]

Hello! 👋

Setting up the transforms between the fleets and RMF can be tricky! It looks like it's very close to working. There are lots of ways to proceed, but in this case, I would suggest to use the robot-generated map image as the base floorplan image in the Levels tab, and set its resolution to 0.05 using the "Edit..." button on the levels tab. I assume it's a 5cm per grid cell, according to the SLAM system. Since there isn't an architectural drawing available, I think it might just be adding extra complexity and potential confusion to also be adding the SLAM-generated map as a layer image. You can just delete the layer in the .building.yaml file to simplify the setup.

Once you have defined the robot-generated floorplan resolution as 0.05 meters per pixel, you should be able to delete the pink measurement edge. This will force the map to remain at 0.05 meters per pixel, as defined in the "Edit..." box of the layer in the layers table. I think the generator should be able to use the floorplan's manually-defined resolution to generate the navigation graph, but if there are build errors, just let us know. Using robot-generated maps when architectural-drawing floorplan images are not available is definitely a use case we'd like to support.

Once you have a navigation graph that is generated in the exact coordinate frame of the robot-generated map, this should make it easier to define the transforms for FreeFleet. One common point of confusion is that it's important to check the YAML file that is being launched by your robot's nav stack. Some SLAM stacks in ROS will generate a map YAML file with (0, 0) as the corner of the map, others will define (0, 0) as the center of the image, and others will have "expanded" the map as the robot wandered around during data-collection, so the origin may be offset. Specifically, it will be a small YAML file as defined here: http://wiki.ros.org/map_server#YAML_format

I'd suggest using (0, 0, 0) as the origin in that YAML file, and ensuring that the map resolution agrees with the SLAM configuration and what was defined in Traffic Editor. 0.05 meters/pixel is a common value. Then everything will hopefully line up between the ROS nav stack coordinates and the RMF coordinate system if the FreeFleet transform translation is (0, 0) and the transform scale is set to 1.0.

However, it may be necessary to have a y offset of the "height" of the map (i.e. resolution * number of vertical pixels) because there is a y-coordinate flip during navigation graph generation, resulting from the map coordinates in traffic-editor being defined in "image coordinates" with (0, 0) at upper-left, and the ROS map server treating the image as if it were in "Quadrant 1" using (0, 0) as the lower-left corner. You can either modify the YAML of map_server to offset the y-coordinate in the robot navstack, or define the FreeFleet transform to perform this Y offset. This may or may not be necessary depending on how your SLAM map is generated, but those should be the only two options possible to debug this case: y-offset=0 or y-offest = "map height". x-offset and yaw-offset should just be zero in this case.

[KamalanathanN]

hi @codebot , thank you for the detailed response. i followed as per your suggestion and faced these issues,

Once you have defined the robot-generated floorplan resolution as 0.05 meters per pixel, you should be able to delete the pink measurement edge.

when i delete the pink measuring edge and compile there is no error but when i launch it the nav graph is scaled down. when zoomed i can read the labels. When using pink measureing edge i get scale as 0.0457 and with this scale i get the normal sized nav graph path. i tried with both versions i get the same error as OP. which one should i proceed with?

I'd suggest using (0, 0, 0) as the origin in that YAML file, and ensuring that the map resolution agrees with the SLAM configuration and what was defined in Traffic Editor. 0.05 meters/pixel is a common value.

i used gmapping and .yaml file had (-50.0,-50.0) offset. i made them (0,0) noticed map shift to upper(+x)left(+y) as shown in img below and had to make changes in amcl initial_pose parameters to ensure bot loads within the map. i hope this is ok.

However, it may be necessary to have a y offset of the "height" of the map (i.e. resolution * number of vertical pixels)

Got its value (0.05*1984) and setting translation y (meters): 99.2 with scale: 1.0 this is where the pink marker spawns.

after manually adjusting the values i was able to make the pink marker load at start point but when assigned with task, i get the same error as before.

i request those who made it to work to share a procedural approach for configuring transforms for this senario.

[codebot]

Hmmm. Thanks for sending the screenshots. It would be unusual to the vertex-name font render very small in the GUI. It does seem like somewhere the scale is not being calculated correctly, and the fonts being tiny implies that something is not quite running as expected.

Which version of Traffic Editor are you using?

Using the current main branch of the rmf_traffic_editor repo, I just tried to create a new image-based map using a Turtlebot-created map with 0.05 meters/pixel grid cells, without any type of measurement annotations, and the resolution and font sizes seem reasonable. Can you send me your .building.yaml file and the robot-generated map PNG and YAML? I'm happy to take a look. My email address is in my GitHub profile: https://github.com/codebot/

[KamalanathanN]

It would be unusual to the vertex-name font render very small in the GUI. It does seem like somewhere the scale is not being calculated correctly, and the fonts being tiny implies that something is not quite running as expected.

i used main branch which was 2 months old, i created a new ws with latest repos and tested with same map, the vertex-name scale is still very small. I have send you the files in mail.

I just tried to create a new image-based map using a Turtlebot-created map with 0.05 meters/pixel grid cells, without any type of measurement annotations, and the resolution and font sizes seem reasonable.

that's really strange, i tried to do the same tb3 gmapping of tb3_world without measurement edge. and font is still very small. i have also shared the building.yaml and pgm png of this in mail.
this is how it looks:

[codebot]

Thanks for your patience as we brainstorm the best approach for this type of situation, where a reference floorplan drawing is not available.

There is a "Cartesian Meters" mode that can be used in traffic-editor and the building generator. You just set coordinate_system: cartesian_meters in the top level your .building.yaml file. Then you can add the robot-generated map layer and explicitly set it to 0.05 meters per pixel scaling, and offset the layer so that its lower-left corner is at (0, 0). For cases like this where there is not an architectural drawing with precise measurements, it seems the robot-measured map scale is convenient. Once this is done, then there shouldn't be any scaling required between free_fleet and RMF; at most there should just be a y-offset, provided the ROS map_server is using the lower-left of the robot map as (0, 0), which can be configured in the YAML file used to start map_server in ROS.

Previously in rmf_traffic_editor, cartesian-meters maps were rendering their vertex name fonts too large, because it was necessary to clamp the calculated font size so that it didn't go below 1.0 (for reasons unknown inside Qt). This PR implements a workaround:
open-rmf/rmf_traffic_editor#431

Anyway, I think it's worth seeing if using cartesian-meters mode in traffic-editor and the building / nav-graph generator might simplify things for this type of situation, where we have a robot-generated map but not a building floorplan.

[KamalanathanN]

heyy!!! it moved but just a little.🥲 i didnt have to give y-offset with map_server using the lower-left of the robot map as (0, 0). i did provide amcl initial_params to localize. after launching both client and server for the first time i could see both /fleet_states and /amcl_pose (x,y) almost the same.

when task is assigned i get the following error after the bot tries to move towards the goal:

[full_control-13] [INFO] [1653734513.717557667] [test_fleet_fleet_adapter]: Beginning new task [patrol.dispatch-0] for [test_fleet/testbot]. Remaining queue size: 0
[rmf_traffic_schedule_monitor-2] [WARN] [1653734514.921142271] [rmf_traffic_schedule_backup]: Failed to update using patch for DB version 84; requesting new update
[schedule_visualizer-5] [WARN] [1653734514.921143825] [rmf_visualization_schedule_data_node]: Failed to update using patch for DB version 84; requesting new update
[full_control-13] [WARN] [1653734514.921503309] [test_fleet_fleet_adapter]: Failed to update using patch for DB version 84; requesting new update

Here is a video of it : https://www.youtube.com/watch?v=Obhm0DloS6M

one more thing, as you add robot's map as Layer with empty Level, it just displays the nav graphs in rviz. i would like to have the map overlayed as before but when i try to import it there is no option to adjust its scale to 0.05 like layers. when i try to manually change values its not changing even after i hit save.